#include "camodocal/calib/HandEyeCalibration.h"

#include <boost/throw_exception.hpp>
#include <iostream>

#include <ceres/ceres.h>
#include "camodocal/EigenUtils.h"
#include "camodocal/calib/DualQuaternion.h"

namespace camodocal {

/// @todo there may be an alignment issue, see
/// http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
class PoseError {
  public:
    PoseError(Eigen::Vector3d r1, Eigen::Vector3d t1, Eigen::Vector3d r2,
              Eigen::Vector3d t2)
        : m_rvec1(r1), m_rvec2(r2), m_tvec1(t1), m_tvec2(t2) {}

    template <typename T>
    bool operator()(const T* const q4x1, const T* const t3x1,
                    T* residual) const {
        Eigen::Quaternion<T> q(q4x1[0], q4x1[1], q4x1[2], q4x1[3]);
        Eigen::Matrix<T, 3, 1> t;
        t << t3x1[0], t3x1[1], t3x1[2];

        DualQuaternion<T> dq(q, t);

        Eigen::Matrix<T, 3, 1> r1 = m_rvec1.cast<T>();
        Eigen::Matrix<T, 3, 1> t1 = m_tvec1.cast<T>();
        Eigen::Matrix<T, 3, 1> r2 = m_rvec2.cast<T>();
        Eigen::Matrix<T, 3, 1> t2 = m_tvec2.cast<T>();

        DualQuaternion<T> dq1(AngleAxisToQuaternion<T>(r1), t1);
        DualQuaternion<T> dq2(AngleAxisToQuaternion<T>(r2), t2);
        DualQuaternion<T> dq1_ = dq * dq2 * dq.inverse();

        DualQuaternion<T> diff = (dq1.inverse() * dq1_).log();
        residual[0] = diff.real().squaredNorm() + diff.dual().squaredNorm();

        return true;
    }

  private:
    Eigen::Vector3d m_rvec1, m_rvec2, m_tvec1, m_tvec2;

  public:
    /// @see
    /// http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

bool HandEyeCalibration::mVerbose = true;

HandEyeCalibration::HandEyeCalibration() {}

void HandEyeCalibration::setVerbose(bool on) { mVerbose = on; }

/// Reorganize data to prepare for running SVD
/// Daniilidis 1999 Section 6, Equations (31) and (33), on page 291
template <typename T>
static Eigen::MatrixXd ScrewToStransposeBlockofT(
    const Eigen::Matrix<T, 3, 1>& a, const Eigen::Matrix<T, 3, 1>& a_prime,
    const Eigen::Matrix<T, 3, 1>& b, const Eigen::Matrix<T, 3, 1>& b_prime) {
    Eigen::MatrixXd Stranspose(6, 8);
    Stranspose.setZero();

    typedef Eigen::Matrix<T, 3, 1> VecT;
    auto skew_a_plus_b = skew(VecT(a + b));
    auto a_minus_b = a - b;
    Stranspose.block<3, 1>(0, 0) = a_minus_b;
    Stranspose.block<3, 3>(0, 1) = skew_a_plus_b;
    Stranspose.block<3, 1>(3, 0) = a_prime - b_prime;
    Stranspose.block<3, 3>(3, 1) = skew(VecT(a_prime + b_prime));
    Stranspose.block<3, 1>(3, 4) = a_minus_b;
    Stranspose.block<3, 3>(3, 5) = skew_a_plus_b;

    return Stranspose;
}

/// Reorganize data to prepare for running SVD
/// Daniilidis 1999 Section 6, Equations (31) and (33), on page 291
// @pre no zero rotations, thus (rvec1.norm() != 0 && rvec2.norm() != 0) == true
template <typename T>
static Eigen::MatrixXd AxisAngleToSTransposeBlockOfT(
    const Eigen::Matrix<T, 3, 1>& rvec1, const Eigen::Matrix<T, 3, 1>& tvec1,
    const Eigen::Matrix<T, 3, 1>& rvec2, const Eigen::Matrix<T, 3, 1>& tvec2) {
    double theta1, d1;
    Eigen::Vector3d l1, m1;
    AngleAxisAndTranslationToScrew(rvec1, tvec1, theta1, d1, l1, m1);

    double theta2, d2;
    Eigen::Vector3d l2, m2;
    AngleAxisAndTranslationToScrew(rvec2, tvec2, theta2, d2, l2, m2);

    Eigen::Vector3d a = l1;
    Eigen::Vector3d a_prime = m1;
    Eigen::Vector3d b = l2;
    Eigen::Vector3d b_prime = m2;

    return ScrewToStransposeBlockofT(a, a_prime, b, b_prime);
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrew(
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    Eigen::Matrix4d& H_12, bool planarMotion) {
    int motionCount = rvecs1.size();
    Eigen::MatrixXd T(motionCount * 6, 8);
    T.setZero();

    for (size_t i = 0; i < motionCount; ++i) {
        const Eigen::Vector3d& rvec1 = rvecs1.at(i);
        const Eigen::Vector3d& tvec1 = tvecs1.at(i);
        const Eigen::Vector3d& rvec2 = rvecs2.at(i);
        const Eigen::Vector3d& tvec2 = tvecs2.at(i);

        // Skip cases with zero rotation
        if (rvec1.norm() == 0 || rvec2.norm() == 0)
            continue;

        T.block<6, 8>(i * 6, 0) =
            AxisAngleToSTransposeBlockOfT(rvec1, tvec1, rvec2, tvec2);
    }

    auto dq = estimateHandEyeScrewInitial(T, planarMotion);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: Before refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }

    estimateHandEyeScrewRefine(dq, rvecs1, tvecs1, rvecs2, tvecs2);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: After refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrew(
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    Eigen::Matrix4d& H_12, ceres::Solver::Summary& summary, bool planarMotion) {
    int motionCount = rvecs1.size();
    Eigen::MatrixXd T(motionCount * 6, 8);
    T.setZero();

    for (size_t i = 0; i < motionCount; ++i) {
        const Eigen::Vector3d& rvec1 = rvecs1.at(i);
        const Eigen::Vector3d& tvec1 = tvecs1.at(i);
        const Eigen::Vector3d& rvec2 = rvecs2.at(i);
        const Eigen::Vector3d& tvec2 = tvecs2.at(i);

        // Skip cases with zero rotation
        if (rvec1.norm() == 0 || rvec2.norm() == 0)
            continue;

        T.block<6, 8>(i * 6, 0) =
            AxisAngleToSTransposeBlockOfT(rvec1, tvec1, rvec2, tvec2);
    }

    auto dq = estimateHandEyeScrewInitial(T, planarMotion);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: Before refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }

    estimateHandEyeScrewRefine(dq, rvecs1, tvecs1, rvecs2, tvecs2, summary);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: After refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }
}

// docs in header
DualQuaterniond
HandEyeCalibration::estimateHandEyeScrewInitial(Eigen::MatrixXd& T,
                                                bool planarMotion) {

    // dq(r1, t1) = dq * dq(r2, t2) * dq.inv
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeFullU |
                                                 Eigen::ComputeFullV);

    // v7 and v8 span the null space of T, v6 may also be one
    // if rank = 5.
    Eigen::Matrix<double, 8, 1> v6 = svd.matrixV().block<8, 1>(0, 5);
    Eigen::Matrix<double, 8, 1> v7 = svd.matrixV().block<8, 1>(0, 6);
    Eigen::Matrix<double, 8, 1> v8 = svd.matrixV().block<8, 1>(0, 7);

    // if rank = 5
    if (planarMotion) //(rank == 5)
    {
        if (mVerbose) {
            std::cout
                << "# INFO: No unique solution, returned an arbitrary one. "
                << std::endl;
        }

        v7 += v6;
    }

    Eigen::Vector4d u1 = v7.block<4, 1>(0, 0);
    Eigen::Vector4d v1 = v7.block<4, 1>(4, 0);
    Eigen::Vector4d u2 = v8.block<4, 1>(0, 0);
    Eigen::Vector4d v2 = v8.block<4, 1>(4, 0);

    double lambda1 = 0;
    double lambda2 = 0.0;

    if (u1.dot(v1) == 0.0) {
        std::swap(u1, u2);
        std::swap(v1, v2);
    }
    if (u1.dot(v1) != 0.0) {
        double s[2];
        solveQuadraticEquation(u1.dot(v1), u1.dot(v2) + u2.dot(v1), u2.dot(v2),
                               s[0], s[1]);

        // find better solution for s
        double t[2];
        for (int i = 0; i < 2; ++i) {
            t[i] =
                s[i] * s[i] * u1.dot(u1) + 2 * s[i] * u1.dot(u2) + u2.dot(u2);
        }

        int idx;
        if (t[0] > t[1]) {
            idx = 0;
        } else {
            idx = 1;
        }

        double discriminant =
            4.0 * square(u1.dot(u2)) - 4.0 * (u1.dot(u1) * u2.dot(u2));
        if (discriminant == 0.0 && mVerbose) {
            //            std::cout << "# INFO: Noise-free case" << std::endl;
        }

        lambda2 = sqrt(1.0 / t[idx]);
        lambda1 = s[idx] * lambda2;
    } else {
        if (u1.norm() == 0 && u2.norm() > 0) {
            lambda1 = 0;
            lambda2 = 1.0 / u2.norm();
        } else if (u2.norm() == 0 && u1.norm() > 0) {
            lambda1 = 1.0 / u1.norm();
            lambda2 = 0;
        } else {
            std::ostringstream ss;

            ss << "camodocal::HandEyeCalibration error: normalization could "
                  "not be handled. Your rotations and translations are "
                  "probably either not aligned or not passed in properly.";
            ss << "u1:" << std::endl;
            ss << u1 << std::endl;
            ss << "v1:" << std::endl;
            ss << v1 << std::endl;
            ss << "u2:" << std::endl;
            ss << u2 << std::endl;
            ss << "v2:" << std::endl;
            ss << v2 << std::endl;
            ss << "Not handled yet. Your rotations and translations are "
                  "probably either not aligned or not passed in properly."
               << std::endl;

            BOOST_THROW_EXCEPTION(std::runtime_error(ss.str()));
        }
    }

    // rotation
    Eigen::Vector4d q_coeffs = lambda1 * u1 + lambda2 * u2;
    Eigen::Vector4d q_prime_coeffs = lambda1 * v1 + lambda2 * v2;

    Eigen::Quaterniond q(q_coeffs(0), q_coeffs(1), q_coeffs(2), q_coeffs(3));
    Eigen::Quaterniond d(q_prime_coeffs(0), q_prime_coeffs(1),
                         q_prime_coeffs(2), q_prime_coeffs(3));

    return DualQuaterniond(q, d);
}

// docs in header
bool HandEyeCalibration::solveQuadraticEquation(double a, double b, double c,
                                                double& x1, double& x2) {
    double delta2 = b * b - 4.0 * a * c;

    if (delta2 < 0.0) {
        return false;
    }

    double delta = sqrt(delta2);

    x1 = (-b + delta) / (2.0 * a);
    x2 = (-b - delta) / (2.0 * a);

    return true;
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrewRefine(
    DualQuaterniond& dq,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    ceres::Solver::Summary& summary) {
    Eigen::Matrix4d H = dq.toMatrix();
    double p[7] = {dq.real().w(), dq.real().x(), dq.real().y(), dq.real().z(),
                   H(0, 3),       H(1, 3),       H(2, 3)};

    ceres::Problem problem;
    for (size_t i = 0; i < rvecs1.size(); i++) {
        // ceres deletes the objects allocated here for the user
        ceres::CostFunction* costFunction =
            new ceres::AutoDiffCostFunction<PoseError, 1, 4, 3>(
                new PoseError(rvecs1[i], tvecs1[i], rvecs2[i], tvecs2[i]));

        problem.AddResidualBlock(costFunction, NULL, p, p + 4);
    }

    // ceres deletes the object allocated here for the user
    ceres::LocalParameterization* quaternionParameterization =
        new ceres::QuaternionParameterization;

    problem.SetParameterization(p, quaternionParameterization);

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.jacobi_scaling = true;
    options.max_num_iterations = 500;

    // ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    if (mVerbose) {
        std::cout << summary.BriefReport() << std::endl;
    }

    Eigen::Quaterniond q(p[0], p[1], p[2], p[3]);
    Eigen::Vector3d t;
    t << p[4], p[5], p[6];
    dq = DualQuaterniond(q, t);
}

void HandEyeCalibration::estimateHandEyeScrewRefine(
    DualQuaterniond& dq,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2) {
    Eigen::Matrix4d H = dq.toMatrix();
    double p[7] = {dq.real().w(), dq.real().x(), dq.real().y(), dq.real().z(),
                   H(0, 3),       H(1, 3),       H(2, 3)};
    ceres::Solver::Summary summary;

    ceres::Problem problem;
    for (size_t i = 0; i < rvecs1.size(); i++) {
        // ceres deletes the objects allocated here for the user
        ceres::CostFunction* costFunction =
            new ceres::AutoDiffCostFunction<PoseError, 1, 4, 3>(
                new PoseError(rvecs1[i], tvecs1[i], rvecs2[i], tvecs2[i]));

        problem.AddResidualBlock(costFunction, NULL, p, p + 4);
    }

    // ceres deletes the object allocated here for the user
    ceres::LocalParameterization* quaternionParameterization =
        new ceres::QuaternionParameterization;

    problem.SetParameterization(p, quaternionParameterization);

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.jacobi_scaling = true;
    options.max_num_iterations = 500;

    // ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    if (mVerbose) {
        std::cout << summary.BriefReport() << std::endl;
    }

    Eigen::Quaterniond q(p[0], p[1], p[2], p[3]);
    Eigen::Vector3d t;
    t << p[4], p[5], p[6];
    dq = DualQuaterniond(q, t);
}
}


///////////////////////////////////////////////////////////////////////////////
// df.hpp                                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_DF_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_DF_HPP_ER_2010
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells_count.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/df_formula.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/lost_df.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/tag.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{

  	template<typename Keys,typename AccSet>
    long degrees_of_freedom(
        const boost::mpl::detail::wrapper<
            pearson_chisq_statistic::tag::independence_between<Keys>
        >& statistic,
        const AccSet& acc
    )
    {
        namespace ns = contingency_table;
        return pearson_chisq_statistic::degrees_of_freedom<long>(
            ns::cells_count<Keys>( acc ),
            pearson_chisq_statistic::lost_degrees_of_freedom( statistic, acc )
        );
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


#pragma once

#include <Eigen/Sparse>
#include <cassert>
#include <algorithm>
#include <ostream>

template <typename Scalar>
using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

template <typename Scalar>
using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

namespace detail
{
    template <typename Scalar, typename Index>
    Eigen::VectorXi submatrix_sparsity_pattern(const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
                                               const std::vector<Index> & reverse_rowmap,
                                               const std::vector<Index> & cols)
    {
        typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
        const auto submat_cols = static_cast<Index>(cols.size());

        // Each entry in the vector holds the number of non-zero rows in the column
        Eigen::VectorXi sparsity_pattern(submat_cols);

        for (Index col = 0; col < submat_cols; ++col)
        {
            sparsity_pattern(col) = 0;
            const auto original_col = cols[col];

            for (InnerIterator it(matrix, original_col); it; ++it)
            {
                const auto submatrix_row = reverse_rowmap[it.row()];
                if (submatrix_row >= 0)
                {
                    sparsity_pattern(col) += 1;
                }
            }
        }

        return sparsity_pattern;
    }
}

template <typename Scalar, typename Index>
Eigen::SparseMatrix<Scalar, 0, Index> sparse_submatrix(const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
                                                       const std::vector<Index> & rows,
                                                       const std::vector<Index> & cols)
{
    typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted.");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Column indices must be sorted.");

    // rows and cols map indices from the submatrix into the original matrix. We want to build a reverse map,
    // which is generally not surjective, hence we denote elements that should not be present by -1.
    std::vector<Index> reverse_rowmap(matrix.rows(), -1);

    for (size_t submatrix_row = 0; submatrix_row < rows.size(); ++submatrix_row)
    {
        const auto original_row = rows[submatrix_row];
        reverse_rowmap[original_row] = submatrix_row;
    }

    const auto submat_rows = static_cast<Index>(rows.size());
    const auto submat_cols = static_cast<Index>(cols.size());

    Eigen::SparseMatrix<Scalar, 0, Index> submat(submat_rows, submat_cols);
    if (submat_rows == 0 || submat_cols == 0)
    {
        return submat;
    }

    submat.reserve(detail::submatrix_sparsity_pattern(matrix, reverse_rowmap, cols));

    for (Index col = 0; col < submat.cols(); ++col)
    {
        const auto original_col = cols[col];

        // We implicitly make the assumption here that the original matrix's columns are relatively sparse,
        // which is almost always the case.
        for (InnerIterator it(matrix, original_col); it; ++it)
        {
            const auto submatrix_row = reverse_rowmap[it.row()];
            if (submatrix_row >= 0)
            {
                submat.insert(submatrix_row, col) = it.value();
            }
        }
    }

    submat.makeCompressed();
    return submat;
};

template <typename Scalar, typename Index>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> sparse_submatrix_as_dense(
        const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
        const std::vector<Index> & rows,
        const std::vector<Index> & cols)
{
    typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted.");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Column indices must be sorted.");

    const auto submat_rows = static_cast<Index>(rows.size());
    const auto submat_cols = static_cast<Index>(cols.size());

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> submat(submat_rows, submat_cols);
    submat.setZero();

    if (submat_rows > 0)
    {
        for (Index col = 0; col < submat.cols(); ++col)
        {
            const auto original_col = cols[col];
            Index current_submat_row = 0;

            InnerIterator it(matrix, original_col);

            for (InnerIterator it(matrix, original_col); it; ++it) {
                while (current_submat_row < submat_rows && it.row() > rows[current_submat_row]) {
                    ++current_submat_row;
                }

                if (current_submat_row < submat_rows && it.row() == rows[current_submat_row]) {
                    submat(current_submat_row, col) = it.value();
                }
            }
        }
    }

    return submat;
};

template <typename Scalar, int Rows, int Cols>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> submatrix(const Eigen::Matrix<Scalar, Rows, Cols> &matrix,
                                                                const std::vector<int> rows,
                                                                const std::vector<int> cols)
{
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Col indices must be sorted");

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> result(rows.size(), cols.size());

    for (size_t j = 0; j < cols.size(); ++j)
    {
        for (size_t i = 0; i < rows.size(); ++i)
        {
            const int row = static_cast<int>(rows[i]);
            const int col = static_cast<int>(cols[j]);
            result(i, j) = matrix(row, col);
        }
    }

    return result;
};


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example mtl4-with-viennacl.cpp
*
*   This tutorial shows how data can be directly transferred from the MTL4 Library to ViennaCL objects using the built-in convenience wrappers.
*
*   The first step is to include the necessary headers and activate the MTL4 convenience functions in ViennaCL:
**/

// System headers
#include <iostream>

// MTL4 headers
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on MTL4 objects
#define VIENNACL_WITH_MTL4 1

// ViennaCL headers
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"
#include "../benchmarks/benchmark-utils.hpp"

/**
*    The following function contains the main code for this tutorial.
*    It consists of the following steps:
*      - Creates MTL4 matrices and vectors
*      - Initializes them with data
*      - Create ViennaCL objects
*      - Copy them over to the respective ViennaCL objects
*      - Compute matrix-vector products in both MTL4 and ViennaCL and compare results.
*
**/
template<typename ScalarType>
void run_tutorial()
{
  typedef mtl::dense2D<ScalarType>        MTL4DenseMatrix;
  typedef mtl::compressed2D<ScalarType>   MTL4SparseMatrix;

  /**
  * Create and fill dense matrices from the MTL4 library:
  **/
  mtl::dense2D<ScalarType>   mtl4_densemat(5, 5);
  mtl::dense2D<ScalarType>   mtl4_densemat2(5, 5);
  mtl4_densemat(0,0) = 2.0;   mtl4_densemat(0,1) = -1.0;
  mtl4_densemat(1,0) = -1.0;  mtl4_densemat(1,1) =  2.0;  mtl4_densemat(1,2) = -1.0;
  mtl4_densemat(2,1) = -1.0;  mtl4_densemat(2,2) = -1.0;  mtl4_densemat(2,3) = -1.0;
  mtl4_densemat(3,2) = -1.0;  mtl4_densemat(3,3) =  2.0;  mtl4_densemat(3,4) = -1.0;
                              mtl4_densemat(4,4) = -1.0;  mtl4_densemat(4,4) = -1.0;

  /**
  * Create and fill sparse matrices from the MTL4 library:
  **/
  MTL4SparseMatrix mtl4_sparsemat;
  set_to_zero(mtl4_sparsemat);
  mtl4_sparsemat.change_dim(5, 5);

  MTL4SparseMatrix mtl4_sparsemat2;
  set_to_zero(mtl4_sparsemat2);
  mtl4_sparsemat2.change_dim(5, 5);

  {
    mtl::matrix::inserter< MTL4SparseMatrix >  ins(mtl4_sparsemat);
    typedef typename mtl::Collection<MTL4SparseMatrix>::value_type  ValueType;
    ins(0,0) <<  ValueType(2.0);   ins(0,1) << ValueType(-1.0);
    ins(1,1) <<  ValueType(2.0);   ins(1,2) << ValueType(-1.0);
    ins(2,2) << ValueType(-1.0);   ins(2,3) << ValueType(-1.0);
    ins(3,3) <<  ValueType(2.0);   ins(3,4) << ValueType(-1.0);
    ins(4,4) << ValueType(-1.0);
  }

  /**
  * Create and fill a few vectors from the MTL4 library:
  **/
  mtl::dense_vector<ScalarType> mtl4_rhs(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_result(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_temp(5, 0.0);

  mtl4_rhs(0) = 10.0;
  mtl4_rhs(1) = 11.0;
  mtl4_rhs(2) = 12.0;
  mtl4_rhs(3) = 13.0;
  mtl4_rhs(4) = 14.0;

  /**
  * Create the corresponding ViennaCL objects:
  **/
  viennacl::vector<ScalarType> vcl_rhs(5);
  viennacl::vector<ScalarType> vcl_result(5);
  viennacl::matrix<ScalarType> vcl_densemat(5, 5);
  viennacl::compressed_matrix<ScalarType> vcl_sparsemat(5, 5);

  /**
  * Directly copy the MTL4 objects to ViennaCL objects
  **/
  viennacl::copy(&(mtl4_rhs[0]), &(mtl4_rhs[0]) + 5, vcl_rhs.begin());  //method 1: via iterator interface (cf. std::copy())
  viennacl::copy(mtl4_rhs, vcl_rhs);  //method 2: via built-in wrappers (convenience layer)

  viennacl::copy(mtl4_densemat, vcl_densemat);
  viennacl::copy(mtl4_sparsemat, vcl_sparsemat);

  // For completeness: Copy matrices from ViennaCL back to Eigen:
  viennacl::copy(vcl_densemat, mtl4_densemat2);
  viennacl::copy(vcl_sparsemat, mtl4_sparsemat2);

  /**
  * Run dense matrix-vector products and compare results:
  **/
  mtl4_result = mtl4_densemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_densemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for dense matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_densemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for dense matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

  /**
  * Run sparse matrix-vector products and compare results:
  **/
  mtl4_result = mtl4_sparsemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_sparsemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_sparsemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

}

/**
*   In the main() routine we only call the worker function defined above with both single and double precision arithmetic.
**/
int main(int, char *[])
{
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Single precision" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  run_tutorial<float>();

#ifdef VIENNACL_HAVE_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Double precision" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    run_tutorial<double>();
  }

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#include "ale/Rotation.h"

#include <exception>

#include <Eigen/Geometry>

#include "ale/InterpUtils.h"

namespace ale {

///////////////////////////////////////////////////////////////////////////////
// Helper Functions
///////////////////////////////////////////////////////////////////////////////

  // Helper function to convert an axis number into a unit Eigen vector down that axis.
  Eigen::Vector3d axis(int axisIndex) {
    switch (axisIndex) {
      case 0:
        return Eigen::Vector3d::UnitX();
        break;
      case 1:
        return Eigen::Vector3d::UnitY();
        break;
      case 2:
        return Eigen::Vector3d::UnitZ();
        break;
      default:
        throw std::invalid_argument("Axis index must be 0, 1, or 2.");
    }
  }

  /**
   * Create the skew symmetric matrix used when computing the derivative of a
   * rotation matrix.
   *
   * This is actually the transpose of the skew AV matrix because we define AV
   * as the AV from the destination to the source. This matches how NAIF
   * defines AV.
   */
  Eigen::Quaterniond::Matrix3 avSkewMatrix(const Vec3d& av) {
    Eigen::Quaterniond::Matrix3 avMat;
    avMat <<  0.0,    av.z, -av.y,
             -av.z,  0.0,    av.x,
              av.y, -av.x,  0.0;
    return avMat;
  }

  ///////////////////////////////////////////////////////////////////////////////
  // Rotation Impl class
  ///////////////////////////////////////////////////////////////////////////////

  // Internal representation of the rotation as an Eigen Double Quaternion
  class Rotation::Impl {
    public:
      Impl() : quat(Eigen::Quaterniond::Identity()) { }

      Impl(double w, double x, double y, double z) : quat(w, x, y, z) { }

      Impl(const std::vector<double>& matrix) {
        if (matrix.size() != 9) {
          throw std::invalid_argument("Rotation matrix must be 3 by 3.");
        }
        quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()));
      }

      Impl(const std::vector<double>& angles, const std::vector<int>& axes) {
        if (angles.empty() || axes.empty()) {
          throw std::invalid_argument("Angles and axes must be non-empty.");
        }
        if (angles.size() != axes.size()) {
          throw std::invalid_argument("Number of angles and axes must be equal.");
        }
        quat = Eigen::Quaterniond::Identity();

        for (size_t i = 0; i < angles.size(); i++) {
          quat *= Eigen::Quaterniond(Eigen::AngleAxisd(angles[i], axis(axes[i])));
        }
      }

      Impl(const std::vector<double>& axis, double theta) {
        if (axis.size() != 3) {
          throw std::invalid_argument("Rotation axis must have 3 elements.");
        }
        Eigen::Vector3d eigenAxis((double *) axis.data());
        quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized()));
      }

      Eigen::Quaterniond quat;
  };

  ///////////////////////////////////////////////////////////////////////////////
  // Rotation Class
  ///////////////////////////////////////////////////////////////////////////////

  Rotation::Rotation() :
        m_impl(new Impl()) { }

  Rotation::Rotation(double w, double x, double y, double z) :
        m_impl(new Impl(w, x, y, z)) { }

  Rotation::Rotation(const std::vector<double>& matrix) :
        m_impl(new Impl(matrix)) { }

  Rotation::Rotation(const std::vector<double>& angles, const std::vector<int>& axes) :
        m_impl(new Impl(angles, axes)) { }

  Rotation::Rotation(const std::vector<double>& axis, double theta) :
        m_impl(new Impl(axis, theta)) { }

  Rotation::~Rotation() = default;

  Rotation::Rotation(Rotation && other) noexcept = default;

  Rotation& Rotation::operator=(Rotation && other) noexcept = default;

  // unique_ptr doesn't have a copy constructor so we have to define one
  Rotation::Rotation(const Rotation& other) : m_impl(new Impl(*other.m_impl)) { }

  // unique_ptr doesn't have an assignment operator so we have to define one
  Rotation& Rotation::operator=(const Rotation& other) {
    if (this != &other) {
      m_impl.reset(new Impl(*other.m_impl));
    }
    return *this;
  }

  std::vector<double> Rotation::toQuaternion() const {
    Eigen::Quaterniond normalized = m_impl->quat.normalized();
    return {normalized.w(), normalized.x(), normalized.y(), normalized.z()};
  }

  std::vector<double> Rotation::toRotationMatrix() const {
    Eigen::Quaterniond::RotationMatrixType mat = m_impl->quat.toRotationMatrix();
    return std::vector<double>(mat.data(), mat.data() + mat.size());
  }

  std::vector<double> Rotation::toStateRotationMatrix(const Vec3d &av) const {
    Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
    Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
    Eigen::Quaterniond::Matrix3 dtMat = rotMat * avMat;
    return {rotMat(0,0), rotMat(0,1), rotMat(0,2), 0.0,         0.0,         0.0,
            rotMat(1,0), rotMat(1,1), rotMat(1,2), 0.0,         0.0,         0.0,
            rotMat(2,0), rotMat(2,1), rotMat(2,2), 0.0,         0.0,         0.0,
            dtMat(0,0),  dtMat(0,1),  dtMat(0,2),  rotMat(0,0), rotMat(0,1), rotMat(0,2),
            dtMat(1,0),  dtMat(1,1),  dtMat(1,2),  rotMat(1,0), rotMat(1,1), rotMat(1,2),
            dtMat(2,0),  dtMat(2,1),  dtMat(2,2),  rotMat(2,0), rotMat(2,1), rotMat(2,2)};
  }

  std::vector<double> Rotation::toEuler(const std::vector<int>& axes) const {
    if (axes.size() != 3) {
      throw std::invalid_argument("Must have 3 axes to convert to Euler angles.");
    }
    if (axes[0] < 0 || axes[0] > 2 ||
        axes[1] < 0 || axes[1] > 2 ||
        axes[2] < 0 || axes[2] > 2) {
      throw std::invalid_argument("Invalid axis number.");
    }
    Eigen::Vector3d angles = m_impl->quat.toRotationMatrix().eulerAngles(
          axes[0],
          axes[1],
          axes[2]);
    return std::vector<double>(angles.data(), angles.data() + angles.size());
  }

  std::pair<std::vector<double>, double> Rotation::toAxisAngle() const {
    Eigen::AngleAxisd eigenAxisAngle(m_impl->quat);
    std::pair<std::vector<double>, double> axisAngle;
    axisAngle.first = std::vector<double>(
          eigenAxisAngle.axis().data(),
          eigenAxisAngle.axis().data() + eigenAxisAngle.axis().size()
    );
    axisAngle.second = eigenAxisAngle.angle();
    return axisAngle;
  }

  Vec3d Rotation::operator()(const Vec3d &vector) const {
    Eigen::Vector3d eigenVector(vector.x, vector.y, vector.z);
    Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector);
    std::vector<double> tempVec = std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size());
    return Vec3d(tempVec);
  }

  State Rotation::operator()(
        const State& state,
        const Vec3d& av
  ) const {
    Vec3d position = state.position;
    Vec3d velocity = state.velocity;

    Eigen::Vector3d positionVector(position.x, position.y, position.z);
    Eigen::Vector3d velocityVector(velocity.x, velocity.y, velocity.z);
    Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
    Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
    Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat;
    Eigen::Vector3d rotatedPosition = rotMat * positionVector;
    Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector;

    return State({rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
                  rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)});
  }

  Rotation Rotation::inverse() const {
    Eigen::Quaterniond inverseQuat = m_impl->quat.inverse();
    return Rotation(inverseQuat.w(), inverseQuat.x(), inverseQuat.y(), inverseQuat.z());
  }

  Rotation Rotation::operator*(const Rotation& rightRotation) const {
    Eigen::Quaterniond combinedQuat = m_impl->quat * rightRotation.m_impl->quat;
    return Rotation(combinedQuat.w(), combinedQuat.x(), combinedQuat.y(), combinedQuat.z());
  }

  Rotation Rotation::interpolate(
        const Rotation& nextRotation,
        double t,
        RotationInterpolation interpType
  ) const {
    Eigen::Quaterniond interpQuat;
    switch (interpType) {
      case SLERP:
        interpQuat = m_impl->quat.slerp(t, nextRotation.m_impl->quat);
        break;
      case NLERP:
        interpQuat = Eigen::Quaterniond(
              linearInterpolate(m_impl->quat.w(), nextRotation.m_impl->quat.w(), t),
              linearInterpolate(m_impl->quat.x(), nextRotation.m_impl->quat.x(), t),
              linearInterpolate(m_impl->quat.y(), nextRotation.m_impl->quat.y(), t),
              linearInterpolate(m_impl->quat.z(), nextRotation.m_impl->quat.z(), t)
        );
        interpQuat.normalize();
        break;
      default:
        throw std::invalid_argument("Unsupported rotation interpolation type.");
        break;
    }
    return Rotation(interpQuat.w(), interpQuat.x(), interpQuat.y(), interpQuat.z());
  }

}


//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

// clang-format off

#include <Python.h>
#include <boost/python.hpp>
#include <boost/format.hpp>
#include <boost/python/make_constructor.hpp>
#include "PyImath.h"
#include "PyImathMathExc.h"
#include "PyImathFixedArray.h"
#include "PyImathRandom.h"
#include "PyImathDecorators.h"

namespace PyImath{
using namespace boost::python;

template <class Rand, class T>
static T
nextf2 (Rand &rand, T min, T max)
{
    MATH_EXC_ON;
    return rand.nextf(min, max);
}

template <class Rand>
static float
nextGauss (Rand &rand)
{
    MATH_EXC_ON;
    return gaussRand(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}
template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class Rand>
static Rand *Rand_constructor1(unsigned long int seed)
{
    return new Rand(seed);
}

template <class Rand>
static Rand *Rand_constructor2(Rand rand)
{
    Rand *r = new Rand();
    *r = rand;

    return r;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
hollowSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
solidSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand32>
register_Rand32()
{
    float (IMATH_NAMESPACE::Rand32::*nextf1)(void) = &IMATH_NAMESPACE::Rand32::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;

    class_< IMATH_NAMESPACE::Rand32 > rand32_class("Rand32");
    rand32_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand32>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand32>))
        .def("init", &IMATH_NAMESPACE::Rand32::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand32::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", nextf1,
        	 "r.nextf() -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(float, float) -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand32, float>)

        .def("nextb", &IMATH_NAMESPACE::Rand32::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand32>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    def("hollowSphereRand",&hollowSphereRand<float,IMATH_NAMESPACE::Rand32>,"hollowSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed across the surface of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    def("solidSphereRand",&solidSphereRand<float,IMATH_NAMESPACE::Rand32>,"solidSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed through the volume of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    decoratecopy(rand32_class);

    return rand32_class;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand48>
register_Rand48()
{
    double (IMATH_NAMESPACE::Rand48::*nextf1)(void) = &IMATH_NAMESPACE::Rand48::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;

    class_< IMATH_NAMESPACE::Rand48 > rand48_class("Rand48");
    rand48_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand48>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand48>))
        .def("init", &IMATH_NAMESPACE::Rand48::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand48::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextf", nextf1,
        	 "r.nextf() -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(double,double) -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand48, double>)

        .def("nextb", &IMATH_NAMESPACE::Rand48::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand48>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    decoratecopy(rand48_class);

    return rand48_class;
}

//

PyObject *
Rand32::wrap (const IMATH_NAMESPACE::Rand32 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand32>::type converter;
    PyObject *p = converter (r);
    return p;
}

PyObject *
Rand48::wrap (const IMATH_NAMESPACE::Rand48 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand48>::type converter;
    PyObject *p = converter (r);
    return p;
}

} //namespace PyIMath


//
//! Copyright © 2008-2011
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_UTILITIES_HPP
#define GEOMETRIX_UTILITIES_HPP
#pragma once

#include <geometrix/numeric/constants.hpp>
#include <geometrix/arithmetic/arithmetic.hpp>
#include <geometrix/tensor/numeric_sequence_compare.hpp>
#include <geometrix/primitive/segment_traits.hpp>
#include <geometrix/arithmetic/vector.hpp>
#include <geometrix/algebra/expression.hpp>
#include <geometrix/algebra/algebra.hpp>
#include <geometrix/algebra/exterior_product.hpp>
#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/fusion/include/all.hpp>
#include <geometrix/algorithm/orientation/point_segment_orientation.hpp>

namespace geometrix {

    namespace result_of {

        template <typename A, typename B>
        struct angle_from_a_to_b
        {
        private:
            typedef decltype(typename type_at<A, 0>::type() - typename type_at<B, 0>::type()) xtype;
            typedef decltype(typename type_at<A, 1>::type() - typename type_at<B, 1>::type()) ytype;
        public:
            typedef decltype(atan2(std::declval<ytype>(), std::declval<xtype>())) type;
        };

    }//! namespace result_of;

    //! Function to get the angle from an origin to a target point in the 2D XY plane.
    template <typename CoordinateSequenceA, typename CoordinateSequenceB>
    inline typename result_of::angle_from_a_to_b<CoordinateSequenceA, CoordinateSequenceB>::type
        angle_from_a_to_b( const CoordinateSequenceA& A,
                           const CoordinateSequenceB& B,
                       typename boost::enable_if_c
                       <
                           geometric_traits<CoordinateSequenceA>::dimension_type::value == 2 &&
                           geometric_traits<CoordinateSequenceB>::dimension_type::value == 2
                       > ::type* = 0 )
    {
        using std::atan2;
        return atan2( get<1>( B ) - get<1>( A ), get<0>( B ) - get<0>( A ) );
    }

    namespace result_of {

        template <typename Vector>
        struct vector_angle
        {
        private:
            typedef decltype(typename type_at<Vector, 0>::type()) xtype;
            typedef decltype(typename type_at<Vector, 1>::type()) ytype;
        public:
            typedef decltype(atan2(std::declval<ytype>(), std::declval<xtype>())) type;
        };

    }//! namespace result_of;

    //! Return the angle in which the specified vector points.
    template <typename Vector>
    inline typename result_of::vector_angle<Vector>::type
        vector_angle(const Vector& v)
    {
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector>));
        using std::atan2;
        return atan2(get<1>(v), get<0>(v));
    }

    //! Function to normalize an angle to within the interval [0,2*PI]
    template <typename CoordinateType>
    inline void normalize_angle_0_2pi(CoordinateType& angle)
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        CoordinateType twoPI = constants::two_pi<CoordinateType>();
        if (angle > twoPI || angle < constants::zero<CoordinateType>())
        {
            using std::floor;
            auto n = floor(angle / twoPI);
            if (n != constants::zero<decltype(n)>())
                angle -= twoPI * n;
            if (angle > twoPI)
                angle -= twoPI;
            else if (angle < constants::zero<CoordinateType>())
                angle += twoPI;
        }
    }

    //! Function to normalize a copy of a given angle to within the interval [0,2*PI] and return the normalized value.
    template <typename CoordinateType>
    inline CoordinateType normalize_angle_0_2pi_copy(CoordinateType angle)
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        normalize_angle_0_2pi(angle);
        return angle;
    }

    //! Function to normalize an angle to within the interval [-PI,PI]
    template <typename CoordinateType>
    inline void normalize_angle_minus_pi_to_pi( CoordinateType& angle )
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi first and then into the -pi,pi range.
        normalize_angle_0_2pi(angle);

        CoordinateType pi = constants::pi<CoordinateType>();
        CoordinateType twoPI = constants::two_pi<CoordinateType>();
        if( angle > pi )
            angle -= twoPI;
        else if( angle <= geometrix::get(-pi) )
            angle += twoPI;
    }

    //! Function to normalize a copy of a given angle to within the interval [-PI,PI] and return the normalized value.
    template <typename CoordinateType>
    inline CoordinateType normalize_angle_minus_pi_to_pi_copy( CoordinateType angle )
    {
        //simplifies the angle to lay in the range of the interval 0 - 2*pi
        normalize_angle_minus_pi_to_pi(angle);
        return angle;
    }

    //! Function to determine if 3 points are collinear in the 2D XY plane.
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear( const PointA& A,
                       const PointB& B,
                       const PointC& C,
                       const NumberComparisonPolicy& compare,
                       typename boost::enable_if_c
                       <
                           geometric_traits<PointA>::dimension_type::value == 2 &&
                           geometric_traits<PointB>::dimension_type::value == 2 &&
                           geometric_traits<PointC>::dimension_type::value == 2
                       > ::type* = 0 )
    {
        auto det = exterior_product_area( B-A, C-A );
        return compare.equals( det, constants::zero<decltype(det)>() );//Absolute tolerance checks are fine for Zero checks.
    }

    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear( const PointA& A,
                       const PointB& B,
                       const PointC& C,
                       const NumberComparisonPolicy& compare,
                       typename boost::enable_if_c
                       <
                           geometric_traits<PointA>::dimension_type::value == 3 &&
                           geometric_traits<PointB>::dimension_type::value == 3 &&
                           geometric_traits<PointC>::dimension_type::value == 3
                       > ::type* = 0 )
    {
        using length_t = typename arithmetic_type_of<PointA>::type;
        using vector_t = vector<length_t, dimension_of<PointA>::value>;
        using cross_result_t = typename result_of::cross_product<vector_t, vector_t>::type;
		auto d = cross_result_t(( B - A ) ^ ( C - A ));
		return compare.equals( get<0>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() )
			&& compare.equals( get<1>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() )
			&& compare.equals( get<2>( d ), constants::zero<typename type_at<cross_result_t, 0>::type>() );
    }

    //! Function to determine if Point C is between points A-B
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_between( const PointA& A,
                     const PointB& B,
                     const PointC& C,
                     bool includeBounds,
                     const NumberComparisonPolicy& compare,
                     typename boost::enable_if_c
                     <
                        geometric_traits<PointA>::dimension_type::value == 2 &&
                        geometric_traits<PointB>::dimension_type::value == 2 &&
                        geometric_traits<PointC>::dimension_type::value == 2
                     >::type* = 0 )
    {
        return is_collinear( A, B, C, compare ) && is_collinear_point_between( A, B, C, includeBounds, compare );
    }

    //! Function to determine if Point C is between points A-B where C is already determined to be collinear to A-B.
    //! From Computational Geometry in C by J. O'Rourke.
    template <typename PointA, typename PointB, typename PointC, typename NumberComparisonPolicy>
    inline bool is_collinear_point_between( const PointA& A,
        const PointB& B,
        const PointC& C,
        bool includeBounds,
        const NumberComparisonPolicy& compare,
        typename boost::enable_if_c
        <
        geometric_traits<PointA>::dimension_type::value == 2 &&
        geometric_traits<PointB>::dimension_type::value == 2 &&
        geometric_traits<PointC>::dimension_type::value == 2
        > ::type* = 0 )
    {
        GEOMETRIX_ASSERT( is_collinear( A, B, C, compare ) );

        //If AB not vertical, check between on x; else on y.
        bool ABVertical = compare.equals( get<0>( A ), get<0>( B ) );
        if( !ABVertical )
        {
            if( includeBounds )
            {
                return ((compare.less_than_or_equal( get<0>( A ), get<0>( C ) ) && compare.less_than_or_equal( get<0>( C ), get<0>( B ) )) ||
                    (compare.greater_than_or_equal( get<0>( A ), get<0>( C ) ) && compare.greater_than_or_equal( get<0>( C ), get<0>( B ) )));
            }
            else
            {
                return ((compare.less_than( get<0>( A ), get<0>( C ) ) && compare.less_than( get<0>( C ), get<0>( B ) )) ||
                    (compare.greater_than( get<0>( A ), get<0>( C ) ) && compare.greater_than( get<0>( C ), get<0>( B ) )));
            }
        }
        else
        {
            if( includeBounds )
            {
                return ((compare.less_than_or_equal( get<1>( A ), get<1>( C ) ) && compare.less_than_or_equal( get<1>( C ), get<1>( B ) )) ||
                    (compare.greater_than_or_equal( get<1>( A ), get<1>( C ) ) && compare.greater_than_or_equal( get<1>( C ), get<1>( B ) )));
            }
            else
            {
                return ((compare.less_than( get<1>( A ), get<1>( C ) ) && compare.less_than( get<1>( C ), get<1>( B ) )) ||
                    (compare.greater_than( get<1>( A ), get<1>( C ) ) && compare.greater_than( get<1>( C ), get<1>( B ) )));
            }
        }
    }

    //! \brief Function to determine if vector c falls in between vectors a and b.
    //! This can be used to check angle ranges without using atan2.
    template <typename Vector1, typename Vector2, typename Vector3, typename NumberComparisonPolicy>
    inline bool is_vector_between(const Vector1& a, const Vector2& b, const Vector3& c, bool includeBounds, const NumberComparisonPolicy& cmp)
    {
        using namespace geometrix;
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector1>));
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector2>));
        BOOST_CONCEPT_ASSERT((Vector2DConcept<Vector3>));
        BOOST_CONCEPT_ASSERT((NumberComparisonPolicyConcept<NumberComparisonPolicy, double>));

        BOOST_AUTO(const detcb, exterior_product_area(c, b));

        //! If b is along c bounds included it's between.
        if (cmp.equals(detcb, constants::zero<decltype(detcb)>()) && cmp.greater_than_or_equal(dot_product(b, c), constants::zero<decltype(dot_product(b,c))>()))
            return includeBounds;

        BOOST_AUTO(const detac, exterior_product_area(a, c));

        //! If a is along c and includeBounds it's between.
        if (cmp.equals(detac, constants::zero<decltype(detac)>()) && cmp.greater_than_or_equal(dot_product(a, c), constants::zero<decltype(dot_product(a,c))>()))
            return includeBounds;

        BOOST_AUTO(const detab, exterior_product_area(a, b));

        //! If b is along a, c can only be between if it is along a and included and that's handled above.
        if (cmp.equals(detab, constants::zero<decltype(detab)>()) && cmp.greater_than_or_equal(dot_product(b, a), constants::zero<decltype(dot_product(b,a))>()))
            return false;

        //! If detab and detac have the same sign, then b and c are on the same side of a and can be compared directly.
        if (cmp.greater_than_or_equal(detac * detab, constants::zero<decltype(detac*detab)>()))
        {
            //! Both are on same side of a; compare to each other.
            return cmp.greater_than(detcb, constants::zero<decltype(detcb)>());
        }

        //! At this point b and c straddle a. A negative determinant means a large angle WRT a.
        //! If c's is positive it must be between a and b, else the opposite must be true.
        return cmp.greater_than(detac, constants::zero<decltype(detac)>());
    }

    template <typename Point, typename NumberComparisonPolicy>
    inline bool is_vertical( const Point& start,
                      const Point& end,
                      const NumberComparisonPolicy& compare )
    {
        return compare.equals( get<0>( start ), get<0>( end ) );
    }

    template <typename Segment, typename NumberComparisonPolicy>
    inline bool is_vertical( const Segment& s,
                      const NumberComparisonPolicy& compare )
    {
        return is_vertical( get_start( s ),
                            get_end( s ),
                            compare );
    }

    template <typename Point, typename NumberComparisonPolicy>
    inline bool is_horizontal( const Point& start, const Point& end, const NumberComparisonPolicy& compare )
    {
        return compare.equals( get<1>( start ),
                               get<1>( end ) );
    }

    template <typename Segment, typename NumberComparisonPolicy>
    inline bool is_horizontal( const Segment& s,const NumberComparisonPolicy& compare )
    {
        return is_horizontal( get_start( s ),
                              get_end( s ),
                              compare );
    }

    //! function to get the slope defined by two points
    template <typename Point>
    inline typename geometric_traits< Point >::arithmetic_type
        get_slope( const Point& s_start, const Point& s_end )
    {
        return arithmetic_promote(get<1>( s_end ) - get<1>( s_start )) / (get<0>( s_end )-get<0>( s_start ));
    }

    //! function to get the slope defined by a segment.
    template <typename Segment>
    inline typename geometric_traits< typename geometric_traits< Segment >::point_type >::arithmetic_type get_slope( const Segment& s )
    {
        return get_slope( get_start( s ), get_end ( s ) );
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType y_of_x( const Point& s_start,
                           const Point& s_end,
                           CoordinateType x,
                           CoordinateType& slope )
    {
        BOOST_AUTO( x0, get<0>( s_start ) );
        BOOST_AUTO( x1, get<0>( s_end ) );
        BOOST_AUTO( y0, get<1>( s_start ) );
        BOOST_AUTO( y1, get<1>( s_end ) );
        slope = arithmetic_promote( (y1-y0) )/(x1-x0);
        return (x - x0) * slope + y0;
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType y_of_x( const Point& s_start,
                           const Point& s_end,
                           CoordinateType x )
    {
        CoordinateType slope;
        return y_of_x( s_start, s_end, x, slope );
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType x_of_y( const Point& s_start,
                           const Point& s_end,
                           CoordinateType y,
                           CoordinateType& slope )
    {
        CoordinateType y0, y1, x0, x1;

        x0 = get<0>( s_start );
        x1 = get<0>( s_end );
        y0 = get<1>( s_start );
        y1 = get<1>( s_end );

        slope = (y1-y0)/(x1-x0);

        CoordinateType x = (y - y0)/slope + x0;
        return x;
    }

    //! Given two points which define a (non-vertical) line segment and a coordinate X calculate Y and the slope.
    template <typename Point, typename CoordinateType>
    inline CoordinateType x_of_y( const Point& s_start,
                           const Point& s_end,
                           CoordinateType y )
    {
        CoordinateType slope;
        return x_of_y( s_start, s_end, y, slope );
    }

    template <typename T1, typename T2>
    inline bool lexicographical_compare(const T1& t1, const T2& t2)
    {
        return t1 < t2;
    }

    template <typename T1, typename T2, typename U, typename ...Params>
    inline bool lexicographical_compare(const T1& t1, const T2& t2, const U& u, const Params&... p)
    {
        return !(t2 < t1) && (t1 < t2 || lexicographical_compare(u, p...));
    }

    namespace detail
    {
        template <std::size_t D>
        struct lexicographical
        {
            template <typename NumericSequence, typename NumberComparisonPolicy>
            static bool compare( const NumericSequence& lhs, const NumericSequence& rhs, const NumberComparisonPolicy& nCompare )
            {
                if( nCompare.less_than( get<dimension_of<NumericSequence>::value - D>( lhs ), get<dimension_of<NumericSequence>::value - D>( rhs ) ) )
                    return true;
                else if( nCompare.equals( get<dimension_of<NumericSequence>::value - D>( lhs ), get<dimension_of<NumericSequence>::value - D>( rhs ) ) )
                    return lexicographical<D-1>::compare( lhs, rhs, nCompare );
                else
                    return false;
            }
        };

        template <>
        struct lexicographical<0>
        {
            template <typename NumericSequence, typename NumberComparisonPolicy>
            static bool compare( const NumericSequence&, const NumericSequence&, const NumberComparisonPolicy& )
            {
                return false;//all were equal.
            }
        };
    }

    //! Lexicographical compare functor for Cartesian points. Sorts first in X and then in Y (then Z).
    template <typename NumberComparisonPolicy>
    class lexicographical_comparer
    {
        template <typename NumericSequence1, typename NumericSequence2>
        struct comparer
        {
            static bool compare( const NumericSequence1& lhs, const NumericSequence2& rhs, const NumberComparisonPolicy& nCompare )
            {
                return detail::lexicographical<dimension_of<NumericSequence1>::value>::compare( lhs, rhs, nCompare );
            }
        };

    public:

        lexicographical_comparer(){}
        lexicographical_comparer( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        template <typename NumericSequence1, typename NumericSequence2>
        bool operator()( const NumericSequence1& p1, const NumericSequence2& p2 ) const
        {
            return comparer<NumericSequence1, NumericSequence2>::compare( p1, p2, m_compare );
        }

    private:

        NumberComparisonPolicy m_compare;

    };

    //! Test if lhs sequence is lexicographically less than rhs sequence.
    template <typename NumericSequence1, typename NumericSequence2, typename NumberComparisonPolicy>
    inline bool lexicographically_less_than(const NumericSequence1& lhs, const NumericSequence2& rhs, const NumberComparisonPolicy& cmp)
    {
        return lexicographical_comparer<NumberComparisonPolicy>(cmp)(lhs, rhs);
    }

    //! Lexicographical compare functor for points - reversed to sort in (Z), then Y then X.
    template <typename NumberComparisonPolicy>
    struct reverse_lexicographical_point_compare
    {
        reverse_lexicographical_point_compare(){}
        reverse_lexicographical_point_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        //! older compilers require disambiguation
        template <int> struct disambiguation_tag { disambiguation_tag(int) {} };

        template <typename Point>
        bool operator()( const Point& p1, const Point& p2, typename boost::enable_if< boost::is_same< typename geometric_traits<Point>::dimension_type, dimension<2> > >::type* = 0, disambiguation_tag<0> = 0 ) const
        {
            return ( m_compare.less_than( get<1>( p1 ), get<1>( p2 ) ) ) ||
                   ( m_compare.equals( get<1>( p1 ), get<1>( p2 ) ) && m_compare.less_than( get<0>( p1 ), get<0>( p2 ) ) );
        }

        template <typename Point>
        bool operator()( const Point& p1, const Point& p2, typename boost::disable_if< boost::is_same< typename geometric_traits<Point>::dimension_type, dimension<2> > >::type* = 0, disambiguation_tag<1> = 0 ) const
        {
            return ( m_compare.less_than( get<2>( p1 ), get<2>( p2 ) ) )||
                   ( m_compare.equals( get<2>( p1 ), get<2>( p2 ) ) && m_compare.less_than( get<1>( p1 ), get<1>( p2 ) ) ) ||
                   ( m_compare.equals( get<2>( p1 ), get<2>( p2 ) ) && m_compare.equals( get<1>( p1 ), get<1>( p2 ) ) && m_compare.less_than( get<0>( p1 ), get<0>( p2 ) ) );
        }

        NumberComparisonPolicy m_compare;

    };

    //! Lexicographical compare functor for Cartesian points. Sorts first in X and then in Y (then Z).
    template <typename NumberComparisonPolicy>
    struct lexicographical_segment_compare
    {
        lexicographical_segment_compare(){}
        lexicographical_segment_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
            , m_pointCompare( compare )
        {}

        template <typename Segment>
        bool operator()( const Segment& s1, const Segment& s2 ) const
        {
            typedef typename geometric_traits< Segment >::point_type point_type;

            const point_type& start1 = get_start( s1 );
            const point_type& end1   = get_end( s2 );
            const point_type& start2 = get_start( s2 );
            const point_type& end2   = get_end( s2 );

            //Order the segments lexicographically first.
            const point_type* lower1;
            const point_type* upper1;
            const point_type* lower2;
            const point_type* upper2;

            if( m_pointCompare( start1, end1 ) )
            {
                lower1 = &start1;
                upper1 = &end1;
            }
            else
            {
                lower1 = &end1;
                upper1 = &start1;
            }

            if( m_pointCompare( start2, end2 ) )
            {
                lower2 = &start2;
                upper2 = &end2;
            }
            else
            {
                lower2 = &end2;
                upper2 = &start2;
            }

            return m_pointCompare( *lower1, *lower2 ) || ( numeric_sequence_equals( *lower1, *lower2, m_compare ) && m_pointCompare( *upper1, *upper2 ) );
        }

        NumberComparisonPolicy                            m_compare;
        lexicographical_comparer< NumberComparisonPolicy > m_pointCompare;

    };

    //! Functor to compare segments lexicographically as intervals A-C !< B-C if [A-B-C].
    template <typename NumberComparisonPolicy>
    struct segment_interval_compare
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;

        segment_interval_compare(){}
        segment_interval_compare( const NumberComparisonPolicy& compare )
            : m_lexCompare( compare ){}

        template <typename Segment>
        bool operator()( const Segment& lhs, const Segment& rhs ) const
        {
            typedef typename geometric_traits<Segment>::point_type       point_type;

            const point_type* lhs_end;
            if( m_lexCompare( get_start( lhs ), get_end( lhs ) ) )
                lhs_end   = &get_end( lhs );
            else
                lhs_end   = &get_start( lhs );

            const point_type* rhs_start;
            if( m_lexCompare( get_start( rhs ), get_end( rhs ) ) )
                rhs_start = &get_start( rhs );
            else
                rhs_start = &get_end( rhs );

            //Now we have the segments in lexi order... we can compare the intervals.
            return m_lexCompare( *lhs_end, *rhs_start );
        }

        lex_point_compare m_lexCompare;

    };

    //! Given a set of segments take the geometrix difference of the set and the specified segments.
    //! precondition segments must all be collinear.
    template <typename Segment, typename SegmentIntervalSet, typename NumberComparisonPolicy>
    inline void collinear_segment_difference( SegmentIntervalSet& segments, const Segment& segment, const NumberComparisonPolicy& compare )
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;
        typedef typename geometric_traits<Segment>::point_type    point_type;
        lex_point_compare lexCompare( compare );

        const point_type& C = get_start( segment );
        const point_type& D = get_end( segment );
        if (numeric_sequence_equals(C, D, compare))
            return;

        typename SegmentIntervalSet::iterator lb,ub;
        boost::tie( lb, ub ) = segments.equal_range( segment );

        std::vector< Segment > toInsert;
        while( lb != ub )
        {
            const Segment& overlappedSegment = *lb;

            const point_type& A = get_start( overlappedSegment );
            const point_type& B = get_end( overlappedSegment );

            //! must be collinear
            GEOMETRIX_ASSERT( is_collinear( C, D, A, compare ) && is_collinear( C, D, B, compare ) );

            bool CAD = is_between( C, D, A, true, compare );
            bool CBD = is_between( C, D, B, true, compare );

            if( CAD && CBD )
            {
                //remove this one.
                segments.erase( lb++ );
                continue;
            }

            bool AEqualC = numeric_sequence_equals( A, C, compare );
            bool AEqualD = numeric_sequence_equals( A, D, compare );
            bool BEqualC = numeric_sequence_equals( B, C, compare );
            bool BEqualD = numeric_sequence_equals( B, D, compare );

            bool ACB = is_between( A, B, C, true, compare );
            bool ADB = is_between( A, B, D, true, compare );
            if( ACB && ADB )
            {
                bool CDB = is_between( C, B, D, true, compare );
                if( CDB && !BEqualC )
                {
                    if( !AEqualC )
                        toInsert.push_back( construct< Segment >( A, C ) );
                    if( !BEqualD )
                        toInsert.push_back( construct< Segment >( D, B ) );
                    segments.erase( lb++ );
                    continue;
                }

                bool ADC = is_between(A, C, D, true, compare);
                if( ADC && !AEqualC )
                {
                    if( !AEqualD )
                        toInsert.push_back( construct< Segment >( A, D ) );
                    if( !BEqualC)
                        toInsert.push_back( construct< Segment >( C, B ) );
                    segments.erase( lb++ );
                    continue;
                }

                GEOMETRIX_ASSERT( false );
            }

            if( CAD && !(AEqualC || AEqualD) )
            {
                if( ADB && !BEqualD )
                {
                    toInsert.push_back( construct< Segment >( D, B ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
                else if( ACB && !BEqualC )
                {
                    toInsert.push_back( construct< Segment >( C, B ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
            }
            else if( CBD && !(BEqualC || BEqualD) )
            {
                if( ADB && !AEqualD)
                {
                    toInsert.push_back( construct< Segment >( A, D ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
                else if( ACB && !AEqualC )
                {
                    toInsert.push_back( construct< Segment >( A, C ) );
                    //remove this one.
                    segments.erase( lb++ );
                    continue;
                }
            }

            ++lb;
        }

        segments.insert( toInsert.begin(), toInsert.end() );
    }

    //! Given a set of segments take the geometrix union of the set and the specified segments.
    //! precondition segments must all be collinear.
    template <typename Segment, typename SegmentIntervalSet, typename NumberComparisonPolicy>
    inline void collinear_segment_union( SegmentIntervalSet& segments, const Segment& segment, const NumberComparisonPolicy& compare )
    {
        typedef lexicographical_comparer< NumberComparisonPolicy > lex_point_compare;
        typedef typename geometric_traits<Segment>::point_type    point_type;
        lex_point_compare lexCompare( compare );

        typename SegmentIntervalSet::iterator lb,ub;
        boost::tie( lb, ub ) = segments.equal_range( segment );

        Segment unionSegment = construct<Segment>( get_start( segment ), get_end( segment ) );

        while( lb != ub )
        {
            const Segment& overlappedSegment = *lb;

            const point_type& A = get_start( overlappedSegment );
            const point_type& B = get_end( overlappedSegment );
            const point_type& C = get_start( unionSegment );
            const point_type& D = get_end( unionSegment );

            //! must be collinear
            GEOMETRIX_ASSERT( is_collinear( C, D, A, compare ) && is_collinear( C, D, B, compare ) );

            bool CAD = is_between( C, D, A, true, compare );
            bool CBD = is_between( C, D, B, true, compare );

            //Already contained in the union.
            if( CAD && CBD )
            {
                //remove this one.
                segments.erase( lb++ );
                continue;
            }

            bool ACB = is_between( A, B, C, true, compare );
            bool ADB = is_between( A, B, D, true, compare );

            //New segment contains existing union.. replace.
            if( ACB && ADB )
            {
                unionSegment = construct<Segment>( A, B );
                segments.erase( lb++ );
                continue;
            }

            if( CAD )
            {
                if( ADB )
                {
                    unionSegment = construct<Segment>( C, B );
                    segments.erase( lb++ );
                    continue;
                }
                if( ACB )
                {
                    unionSegment = construct<Segment>( D, B );
                    segments.erase( lb++ );
                    continue;
                }
            }
            else if( CBD )
            {
                if( ADB )
                {
                    unionSegment = construct<Segment>( A, C );
                    segments.erase( lb++ );
                    continue;
                }
                if( ACB )
                {
                    unionSegment = construct<Segment>( A, D );
                    segments.erase( lb++ );
                    continue;
                }
            }

            ++lb;
        }

        segments.insert( unionSegment );
    }

    //! sorting compare functor to sort coordinates by less than order.
    template <typename CoordinateType, typename NumberComparisonPolicy>
    struct coordinate_less_compare
    {
        coordinate_less_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
        {}

        bool operator() ( const CoordinateType& lhs, const CoordinateType& rhs ) const
        {
            return m_compare.less_than( rhs, lhs );
        }

        NumberComparisonPolicy m_compare;
    };

    template <typename Compare>
    struct pair_first_compare
    {
        pair_first_compare( const Compare& compare )
            : m_compare( compare )
        {}

        template <typename T1, typename T2>
        bool operator()(const std::pair<T1,T2>& p1, const std::pair<T1,T2>& p2) const
        {
            return m_compare(p1.first, p2.first);
        }

        Compare m_compare;

    };

    template <typename Compare>
    struct pair_second_compare
    {
        pair_second_compare( const Compare& compare )
            : m_compare( compare )
        {}

        template <typename T1, typename T2>
        bool operator()(const std::pair<T1,T2>& p1, const std::pair<T1,T2>& p2) const
        {
            return m_compare(p1.second, p2.second);
        }

        Compare m_compare;

    };

    //! \struct dimension_compare
    //! \brief A predicate to compare two numeric sequences by the value at a specified dimension.
    template <std::size_t D, typename NumberComparisonPolicy>
    struct dimension_compare
    {
        dimension_compare( const NumberComparisonPolicy& compare )
            : m_compare( compare )
            , m_lexicographicalCompare( compare )
        {}

        template <typename NumericSequence>
        bool operator()( const NumericSequence& lhs, const NumericSequence& rhs ) const
        {
            //! Sequences are compared by dimension specified. In the case where coordinates at D are equal, the sequences are
            //! given a total order lexicographically.
            if( m_compare.less_than( get<D>( lhs ), get<D>( rhs ) ) )
                return true;
            else if( m_compare.equals( get<D>( lhs ), get<D>( rhs ) ) )
                return m_lexicographicalCompare( lhs, rhs );
            else
                return false;
        }

        NumberComparisonPolicy                          m_compare;
        lexicographical_comparer<NumberComparisonPolicy> m_lexicographicalCompare;
    };

    template <typename T>
    inline T min_copy(const T& a, const T& b)
    {
        return a < b ? a : b;
    }

    template <typename T>
    inline T max_copy(const T& a, const T& b)
    {
        return a > b ? a : b;
    }

    // clamp n to lie within the range [min, max]
    template <typename T>
    inline T clamp(T n, T min, T max)
    {
        if (n < min) return min;
        if (n > max) return max;
        return n;
    }

    template <typename T>
    inline int sign(const T& value)
    {
        return value >= constants::zero<T>() ? 1 : -1;
    }

}//namespace geometrix;

#endif //GEOMETRIX_UTILITIES_HPP


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric

    This function object computes the remainder modulo \f$\pi/2\f$,
     and the angle quadrant between 0 and 3.

     This is a rather slow version,  but accurate in the full floating range.

    @par Header <boost/simd/function/rem_pio2.hpp>

    @par Notes

     - This algorithm is accurate over  the full floating range,  but also is over
      costly and implies the knowledge  of a few hundred \f$\pi\f$ decimals

     - Some simpler algorithms  @ref rem_pio2_medium, @ref rem_pio2_cephes or @ref rem_pio2_straight
      can often be used, but the precision is only insured on smaller intervals.

      - Using  `std::tie(n, r) = rem_pio2(x);` is similar to ` n = div(inearbyint, x, Pio_2<T>())`
      and `r =  remainder(x, Pio_2<T>())`

    @see rem_pio2_medium, rem_pio2_straight, rem_2pi,  rem_pio2_cephes,

    @par Example:

      @snippet rem_pio2.cpp rem_pio2

    @par Possible output:

      @snippet rem_pio2.txt rem_pio2

  **/
  std::pair<IEEEValue, IEEEValue> rem_pio2(IEEEValue const & x);
} }
#endif

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

#endif


/*
 * padding.hpp
 *
 *  Created on: Apr 8, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math{

/**
 * Pad tensor by repeating its border values.
 * Same border behavior as mode='edge' for NumPy or BORDER_REPLICATE for OpenCV
 * NB: For simple padding with zeros, use the Tensor::pad method provided by Eigen.
 * @param tensor input tensor
 * @return padded tensor
 */
template<typename Scalar>
Eigen::Tensor<Scalar,3,Eigen::ColMajor> pad_replicate(const Eigen::Tensor<Scalar,3,Eigen::ColMajor>& tensor, int border_width=1);

//TODO: provide Eigen::Matrix overload (if there is no Eigen equivalent)

} //end namespace math


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

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;
using mtl::generate_mask; using mtl::row_major; using mtl::col_major;

template <typename Matrix>
void print_matrix(Matrix& matrix)
{
    using std::cout;
    typedef typename mtl::Collection<Matrix>::size_type size_type;
    for (size_type i= 0 ; i < num_rows(matrix); i++ ){
	for(size_type j=0; j < num_cols(matrix);  j++ ){
	    cout.fill (' '); cout.width (8); cout.precision (5); cout.flags (ios_base::left);
	    cout << showpoint <<  matrix[i][j] <<"  ";
	}
	cout << endl;
    }
}

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    namespace with_bracket = mtl::mat::with_bracket;
    // namespace with_iterator = mtl::mat::with_iterator;

    using mtl::mat::recursive_cholesky_visitor_t;
    using mtl::mat::detail::mult_schur_update_t;

    std::cout << "Test " << name << "\n-----\n\n";
    fill_matrix_for_cholesky(matrix);

    recursive_cholesky(matrix);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

#if 0
    with_iterator::recursive_cholesky_base_visitor_t  iter_vis;
    recursive_cholesky(matrix, iter_vis);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);
#endif

    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_bracket::cholesky_base_t, with_bracket::tri_solve_base_t,
                                 with_bracket::tri_schur_base_t, with_bracket::schur_update_base_t>
        iter_vis2;
    recursive_cholesky(matrix, iter_vis2);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

#if 0 // ITERATOR VERSION CURRENTLY NOT SUPPORTED -- CAUSES SEGFAULT, e.g. with icc 11.0 in r8536 !!!!

    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_iterator::cholesky_base_t, with_iterator::tri_solve_base_t,
                                 with_iterator::tri_schur_base_t, with_iterator::schur_update_base_t>
        iter_vis3;

    recursive_cholesky(matrix, iter_vis3);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    fill_matrix_for_cholesky(matrix);

    typedef mult_schur_update_t<mtl::gen_tiling_22_dmat_dmat_mult_t<mtl::assign::minus_sum> > schur_update_22_t;
    recursive_cholesky_visitor_t<mtl::recursion::bound_test_static<2>, with_iterator::cholesky_base_t, with_iterator::tri_solve_base_t,
                                 with_iterator::tri_schur_base_t, schur_update_22_t>
        iter_vis4;

    recursive_cholesky(matrix, iter_vis4);
    if (matrix.num_cols() <= 10) {
	print_matrix(matrix); std::cout << "\n";
    }

    typedef detail::mult_schur_update_t<gen_tiling_44_dmat_dmat_mult_t<minus_mult_assign_t> > schur_update_44_t;

#endif
}

int main(int argc, char* argv[])
{
    mtl::vampir_trace<9999> tracer;

    using namespace mtl;
    unsigned size= 13;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                dr(size, size);
    dense2D<double, mat::parameters<col_major> > dc(size, size);
    morton_dense<double,  morton_mask>             md(size, size);
    morton_dense<double,  morton_z_mask>           mzd(size, size);
    morton_dense<double,  doppled_2_row_mask>      d2r(size, size);
    morton_dense<double,  doppled_2_col_mask>      d2c(size, size);
    morton_dense<double,  doppled_16_row_mask>     d16r(size, size);
    morton_dense<double,  doppled_32_row_mask>     d32r(size, size);
    morton_dense<double,  doppled_64_row_mask>     d64r(size, size);
    morton_dense<double,  doppled_64_col_mask>     d64c(size, size);
    morton_dense<double,  doppled_128_col_mask>    d128r(size, size);
    size= 9;
    dense2D<double>                                dr2(size, size);

    test(dr2, "Dense row major");
    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(md, "Morton N-order");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(d2c, "Hybrid 2 column-major");
    test(d16r, "Hybrid 16 row-major");

    return 0;
}


#include "BaseSpectrumCode.h"
#include "SlabSpectrumCode.h"
#include "numeric_tools.h"

#include <boost/math/special_functions/expint.hpp>
#include <iostream>

using namespace Eigen;
using namespace Numerics;
using namespace PrintFuncs;
using namespace boost::math;

std::pair<Tensor3d, Tensor4d> SlabSpectrumCode::calcTracks()
{
    Tensor4d tau = Tensor4d(m_cells, m_cells, m_rays, m_energies);
	tau.setZero();

    Tensor3d zz(m_cells, m_cells, m_rays);
    zz.setZero();

	std::vector<double> delta(m_radii.size() - 1, 0.0);

	for(size_t i = 0; i < delta.size(); i++)
	{
		delta[i] = m_radii[i + 1] - m_radii[i];
	}

	for(int h = 0; h < m_energies; h++)
		for(int i = 0; i < m_cells; i++)
          for(int j = i; j < m_cells; j++)
	      {
	          if (i == j)
                  tau(i, j, 0, h) = delta[i] * m_totalXS(h, i);
              else
              {
                  if (i == 0)
					  tau(i, j, 0, h) = delta[j] * m_totalXS(h, j) + tau(0, j - 1, 0, h);
                  else
                      tau(i, j, 0, h) = delta[j] * m_totalXS(h, j)
                      + tau(0, j - 1, 0, h) - tau(0, i - 1, 0, h);
              }
		  }

	for(int h = 0; h < m_energies; h++)
		for(int i = 0; i < m_cells; i++)
          for(int j = i; j < m_cells; j++)
	      {
	        if (i != j) tau(i, j, 0, h) -= delta[j] * m_totalXS(h, j) +  delta[i] * m_totalXS(h, i);
			if (tau(i, j, 0, h) < 0.0) tau(i, j, 0, h) = 0.0;

			out.print(TraceLevel::TRACE, "Tau: {:3d} {:3d} {:3d} {:5.4f}", h, i, j, tau(i, j,  0, h));
		  }

    std::pair<Tensor3d, Tensor4d> trackData = std::make_pair(zz, tau);
	return trackData;
}

Tensor3d SlabSpectrumCode::calcCPs(std::pair<Tensor3d, Tensor4d> &trackData)
{
	Tensor4d tau = trackData.second;

	Tensor3d gcpm = Tensor3d(m_cells, m_cells, m_energies);
	gcpm.setZero();

	Tensor3d CSet1(m_cells, m_cells, m_cells);
	Tensor3d CSet2(m_cells, m_cells, m_cells);

	MatrixXd P = MatrixXd::Zero(m_cells, m_cells);

    for(int h = 0; h < m_energies; h++)
	{
	  for(int i = 0; i < m_cells; i++)
        for(int j = i; j < m_cells; j++)
		{
          if (j == i)
            P(i, j) = 1.0 - (1.0 / (2.0 * tau(i, i, 0, h))) * (1.0 - 2.0 * expint(3, tau(i, i, 0, h)));
          else
            P(i, j) = (1.0 / (2.0 * tau(i, i, 0, h))) * (expint(3, tau(i, j, 0, h))
			          - expint(3, tau(i, j, 0, h) + tau(i, i, 0, h))
					  - expint(3, tau(i, j, 0, h) + tau(j, j, 0, h))
					  + expint(3, tau(i, j, 0, h) + tau(j, j, 0, h) + tau(i, i, 0, h)));
		}

	for(int i = 0; i < m_cells; i++)
	  for(int j = i + 1; j < m_cells; j++)
          P(j, i) = P(i, j) * (m_totalXS(h, i) / m_totalXS(h, j)) * (m_volumes(i) / m_volumes(j));

	for(int i = 0; i < m_cells; i++)
	  for(int j = 0; j < m_cells; j++)
		gcpm(i, j, h) = P(i, j);
	}

	out.print(TraceLevel::INFO, "P matrix (vacuum BC)");
    printMatrix(gcpm, out, TraceLevel::INFO, "Group");
	return gcpm;
}

void SlabSpectrumCode::applyBoundaryConditions(Tensor3d &gcpm)
{
	for(int h = 0; h < m_energies; h++)
	{
		out.print(TraceLevel::INFO, "Apply boundary conditions Group {}", h + 1);

		double albedo = m_solverData.getAlbedo()[0];

		VectorXd Pis   = VectorXd::Zero(m_cells);
	    VectorXd psi   = VectorXd::Zero(m_cells);

		Pis.setZero();
		psi.setZero();

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) /= m_totalXS(h, j);  //reduced
			}
		}

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				Pis(i) += gcpm(i, j, h) * m_totalXS(h, j);
			}

			Pis(i) = 1.0 - Pis(i);
			psi(i) = (4.0 * m_volumes(i) / m_surface) * Pis(i);

			out.print(TraceLevel::INFO, "Cell n: {} Pis: {:7.6e}  psi [cm]: {:7.6e}", i, Pis(i), psi(i));
		}

		double Pss = 0.0;

		for(int i = 0; i < m_cells; i++)
        {
			Pss += psi(i) * m_totalXS(h, i);
		}

		Pss = 1.0 - Pss;

		out.print(TraceLevel::INFO, "Pss [cm2]: {:7.6e} \n", Pss);

		for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) += Pis(i) * psi(j) * (albedo / (1.0 - Pss * albedo));
				gcpm(i, j, h) *= m_totalXS(h, j);
			}
		}

	}

	out.print(TraceLevel::INFO, "P matrix (white BC)");
    printMatrix(gcpm, out, TraceLevel::INFO, "Group");

    for(int h = 0; h < m_energies; h++)
	{
	    for(int i = 0; i < m_cells; i++)
		{
			for(int j = 0; j < m_cells; j++)
			{
				gcpm(i, j, h) /= m_totalXS(h, j);
			}
		}
	}
}