/**
 *                       _________ _______   __
 *                      /  _/ ___// ____/ | / /
 *                      / / \__ \/ __/ /  |/ /
 *                    _/ / ___/ / /___/ /|  /
 *                   /___//____/_____/_/ |_/
 *
 *  Isentropic model - ETH Zurich
 *  Copyright (C) 2016  Fabian Thuering (thfabian@student.ethz.ch)
 *
 *  This file is distributed under the MIT Open Source License. See
 *  LICENSE.TXT for details.
 */

#define _USE_MATH_DEFINES
#include <cmath>

#include <Isen/Boundary.h>
#include <Isen/Logger.h>
#include <Isen/Output.h>
#include <Isen/MeteoUtils.h>
#include <Isen/Progressbar.h>
#include <Isen/Solver.h>
#include <Isen/Timer.h>

#ifdef ISEN_PYTHON
#include <boost/python.hpp>
#endif

ISEN_NAMESPACE_BEGIN

Solver::Solver(const std::shared_ptr<NameList>& namelist, Output::ArchiveType archiveType)
{
    // Copy NameList
    namelist_ = std::make_shared<NameList>(*namelist);
    SOLVER_DECLARE_ALL_ALIASES

    Timer t;
    LOG() << "Allocating memory ... " << logger::flush;

    try
    {
        //-------------------------------------------------
        // Define physical fields
        //-------------------------------------------------

        // Topography
        topo_ = VectorXf::Zero(nxb);

        // Horizontal velocity
        zhtold_ = zhtnow_ = MatrixXf::Zero(nxb, nz1);

        // Horizontal velocity
        uold_ = unow_ = unew_ = MatrixXf::Zero(nxb1, nz);

        // Isentropic density
        sold_ = snow_ = snew_ = MatrixXf::Zero(nxb, nz);

        // Montgomery potential
        mtg_ = mtgnew_ = MatrixXf::Zero(nxb, nz);
        mtg0_ = VectorXf::Zero(nz);

        // Exner function
        exn_ = MatrixXf::Zero(nxb, nz1);
        exn0_ = VectorXf::Zero(nz1);

        // Pressure
        prs_ = MatrixXf::Zero(nxb, nz1);
        prs0_ = VectorXf::Zero(nz1);

        // Height-dependent diffusion coefficient
        tau_ = VectorXf::Zero(nz);

        // Upstream profile for theta
        th0_ = VectorXf::Zero(nz1);

        if(imoist)
        {
            // Precipitation
            prec_ = VectorXf::Zero(nxb);

            // Accumulated precipitation
            tot_prec_ = VectorXf::Zero(nxb);

            // Specific humidity
            qvold_ = qvnow_ = qvnew_ = MatrixXf::Zero(nxb, nz);

            // Specific cloud water content
            qcold_ = qcnow_ = qcnew_ = MatrixXf::Zero(nxb, nz);

            // Specific rain water content
            qrold_ = qrnow_ = qrnew_ = MatrixXf::Zero(nxb, nz);

            // Temperature
            temp_ = MatrixXf::Zero(nxb, nz1);

            // Parametrization
            if(imicrophys == 1)
                kessler_ = std::make_shared<Kessler>(namelist_);

            if(imicrophys == 2)
            {
                // Rain-droplet number density
                nrold_ = nrnow_ = nrnew_ = MatrixXf::Zero(nxb, nz);

                // Cloud-droplet number density
                ncold_ = ncnow_ = ncnew_ = MatrixXf::Zero(nxb, nz);
            }

            if(idthdt)
            {
                // Latent heating
                dthetadt_ = MatrixXf::Zero(nxb, nz1);
            }
        }

        //-------------------------------------------------
        // Define fields at lateral boundaries
        //-------------------------------------------------
        tbnd1_ = tbnd2_ = VectorXf::Zero(1);

        // Isentropic density
        sbnd1_ = sbnd2_ = VectorXf::Zero(nz);

        // Horizontal velocity
        ubnd1_ = ubnd2_ = VectorXf::Zero(nz);

        if(imoist)
        {
            // Specific humidity
            qvbnd1_ = qvbnd2_ = VectorXf::Zero(nz);

            // Specific cloud water content
            qcbnd1_ = qcbnd2_ = VectorXf::Zero(nz);

            // Specific rain water content
            qrbnd1_ = qrbnd2_ = VectorXf::Zero(nz);

            if(imicrophys == 2)
            {
                // Rain-droplet number density
                nrbnd1_ = nrbnd2_ = VectorXf::Zero(nz);

                // Cloud-droplet number density
                ncbnd1_ = ncbnd2_ = VectorXf::Zero(nz);
            }

            if(idthdt)
            {
                // Latent heating
                dthetadtbnd1_ = dthetadtbnd2_ = VectorXf::Zero(nz1);
            }
        }

        //-------------------------------------------------
        // Define scalar fields
        //-------------------------------------------------
        dtdx_ = dt / dx;
        topofact_ = 1.0;
    }
    catch(std::bad_alloc&)
    {
        LOG() << logger::failed;
        throw IsenException("out of memory");
    }
    LOG_SUCCESS(t);

    // Allocate space for output
    output_ = std::make_shared<Output>(namelist_, archiveType);
}

void Solver::init() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    // Make upstream profiles and initial conditions
    //-------------------------------------------------------------
    const double g2 = g * g;

    Timer t;
    LOG() << "Create initial profile ... " << logger::flush;

    VectorXf z0 = VectorXf::Zero(nz1);

    VectorXf rh0, qv0, qc0, qr0, nc0, nr0;
    if(imoist)
    {
        rh0 = VectorXf::Zero(nz);

        qv0 = VectorXf::Zero(nz);
        qc0 = VectorXf::Zero(nz);
        qr0 = VectorXf::Zero(nz);

        if(imicrophys == 2)
        {
            nc0 = VectorXf::Zero(nz);
            nr0 = VectorXf::Zero(nz);
        }
    }

    // Upstream profile for Brunt-Vaisalla frequency (unstaggered)
    //------------------------------------------------------------
    VectorXf bv0 = bv00 * VectorXf::Ones(nz1).array();

    // Upstream profile of theta (staggered)
    // -----------------------------------------------------------
    th0_ = th00 * VectorXf::Ones(nz1).array() + dth * VectorXf::LinSpaced(nz1, 0, nz1 - 1).array();

    // Upstream profile for Exner function and pressure (staggered)
    //-------------------------------------------------------------
    exn0_[0] = exn00;
    for(int k = 1; k < nz1; ++k)
        exn0_[k] = exn0_[k - 1]
                   - (16 * g2 * (th0_[k] - th0_[k - 1]) / (pow2(bv0[k - 1] + bv0[k]) * pow2(th0_[k - 1] + th0_[k])));

    for(int k = 0; k < nz1; ++k)
        prs0_[k] = pref * std::pow(exn0_[k] / cp, cpdr);

    // Upstream profile for geometric height (staggered)
    //-------------------------------------------------------------
    z0[0] = z00;
    for(int k = 1; k < nz1; ++k)
        z0[k] = z0[k - 1] + (8 * g * (th0_[k] - th0_[k - 1]) / (pow2(bv0[k - 1] + bv0[k]) * (th0_[k - 1] + th0_[k])));

    // Upstream profile for Montgomery potential (unstaggered)
    //-------------------------------------------------------------
    mtg0_[0] = g * z0[0] + th00 * exn0_[0] + dth * exn0_[0] / 2.;

    double mtg0old = mtg0_[0];
    for(int k = 1; k < nz; ++k)
    {
        std::swap(mtg0_[k], mtg0old);
        mtg0_[k] += dth * exn0_[k];
    }

    // Upstream profile for isentropic density (unstaggered)
    //-------------------------------------------------------------
    VectorXf s0 = -1. / g * (prs0_.tail(nz1 - 1) - prs0_.head(nz1 - 1)) / dth;

    // Upstream profile for velocity (unstaggered)
    //-------------------------------------------------------------
    VectorXf u0 = u00 * VectorXf::Ones(nz).array();

    if(ishear)
    {
        for(int k = 0; k < k_shl; ++k)
            u0(k) = u00_sh;

        for(int k = k_shl; k < k_sht; ++k)
            u0(k) = u00_sh - (u00_sh - u00) * (k - k_shl) / (k_sht - k_shl);

        for(int k = k_sht; k < nz; ++k)
            u0(k) = u00;
    }

    // Upstream profile for moisture (unstaggered)
    //-------------------------------------------------------------
    if(imoist)
    {
        double rhmax = 0.98;
        const int kc = 12;
        const int kw = 10;

        for(int k = kc - kw; k < (kc + kw - 1); ++k)
        {
            double cos_k = std::cos((std::abs((k + 1) - kc) / double(kw)) * M_PI * 0.5);
            rh0[k] = rhmax * cos_k * cos_k;
        }

        for(int k = 0; k < nz; ++k)
        {
            qv0[k] = MeteoUtils::rrmixv1(0.5 * (prs0_[k] + prs0_[k + 1]) / 100,
                                         0.5 * (th0_[k] / cp * exn0_[k] + th0_[k + 1] / cp * exn0_[k + 1]), rh0[k],
                                         MeteoUtils::ERelative);
        }

        // Upstream profile for number densities(unstaggered)
        //---------------------------------------------------------
        if(imicrophys == 2)
        {
            for(int k = 0; k < nz; ++k)
            {
                nc0[k] = 0;
                nr0[k] = 0;
            }
        }
    }

    // Initial conditions for isentropic density (sigma), velocity u, and
    // moisture qv
    //-------------------------------------------------------------
    sold_ = s0.transpose().replicate(sold_.rows(), 1);
    snow_ = s0.transpose().replicate(snow_.rows(), 1);
    mtg_ = mtg0_.transpose().replicate(mtg_.rows(), 1);
    mtgnew_ = mtg0_.transpose().replicate(mtgnew_.rows(), 1);
    uold_ = u0.transpose().replicate(uold_.rows(), 1);
    unow_ = u0.transpose().replicate(unow_.rows(), 1);

    if(imoist)
    {
        qvold_ = qv0.transpose().replicate(qvold_.rows(), 1);
        qvnow_ = qv0.transpose().replicate(qvnow_.rows(), 1);
        qcold_ = qc0.transpose().replicate(qcold_.rows(), 1);
        qcnow_ = qc0.transpose().replicate(qcnow_.rows(), 1);
        qrold_ = qr0.transpose().replicate(qrold_.rows(), 1);
        qrnow_ = qr0.transpose().replicate(qrnow_.rows(), 1);

        // Droplet density for 2-moment scheme
        if(imicrophys == 2)
        {
            ncold_ = nc0.transpose().replicate(ncold_.rows(), 1);
            ncnow_ = nc0.transpose().replicate(ncnow_.rows(), 1);
            nrold_ = nr0.transpose().replicate(nrold_.rows(), 1);
            nrnow_ = nr0.transpose().replicate(nrnow_.rows(), 1);
        }
    }

    LOG_SUCCESS(t);

    // Save boundary values for the lateral boundary relaxation
    //-------------------------------------------------------------
    if(irelax)
    {
        LOG() << "Saving lateral boundary values ... " << logger::flush;
        t.start();

        sbnd1_ = snow_.row(0);
        sbnd2_ = snow_.row(snow_.rows() - 1);

        ubnd1_ = unow_.row(0);
        ubnd2_ = unow_.row(unow_.rows() - 1);

        if(imoist)
        {
            qvbnd1_ = qvnow_.row(0);
            qvbnd2_ = qvnow_.row(qvnow_.rows() - 1);

            qcbnd1_ = qcnow_.row(0);
            qcbnd2_ = qcnow_.row(qcnow_.rows() - 1);

            qrbnd1_ = qrnow_.row(0);
            qrbnd2_ = qrnow_.row(qrnow_.rows() - 1);

            if(imicrophys == 2)
            {
                ncbnd1_ = ncnow_.row(0);
                ncbnd2_ = ncnow_.row(ncnow_.rows() - 1);

                nrbnd1_ = nrnow_.row(0);
                nrbnd2_ = nrnow_.row(nrnow_.rows() - 1);
            }

            if(idthdt)
            {
                dthetadtbnd1_ = dthetadt_.row(0);
                dthetadtbnd2_ = dthetadt_.row(dthetadt_.rows() - 1);
            }
        }

        LOG_SUCCESS(t);
    }

    // Calculate geometric height (staggered)
    //-------------------------------------------------------------
    for(int k = 1; k < nz1; ++k)
    {
        zhtnow_.col(k) = zhtnow_.col(k - 1).array()
                         - rdcp / g * 0.5 * (th0_[k - 1] * exn0_[k - 1] + th0_[k] * exn0_[k])
                               * (prs0_[k] - prs0_[k - 1]) / (0.5 * (prs0_[k] + prs0_[k - 1]));
    }

    // Make topography
    //-------------------------------------------------------------
    LOG() << "Creating topography ... " << logger::flush;
    t.start();

    double x0 = (nxb - 1) / 2.0 + 1;
    VectorXf x = (VectorXf::LinSpaced(nxb, 0, nxb - 1).array() + 1 - x0) * dx;

    VectorXf toponf(nxb);
    for(int i = 0; i < nxb; ++i)
        toponf[i] = topomx * std::exp(-(pow2(x[i] / double(topowd))));

    for(int i = 1; i < nxb - 1; ++i)
        topo_[i] = toponf[i] + 0.25 * (toponf[i - 1] - 2.0 * toponf[i] + toponf[i + 1]);

    LOG_SUCCESS(t);

    // Switch between boundary relaxation / periodic boundary conditions
    //-------------------------------------------------------------
    if(irelax)
    {
        LOG() << "Relax topography ... " << logger::flush;
        t.start();

        tbnd1_[0] = topo_[0];
        tbnd2_[0] = topo_[topo_.size() - 1];
        Boundary::relax(topo_, nx, nb, tbnd1_, tbnd2_);

        LOG_SUCCESS(t);
    }
    else
    {
        LOG() << "Periodic topography ... " << logger::flush;
        t.start();
        Boundary::periodic(topo_, nx, nb);
        LOG_SUCCESS(t);
    }

    // Height-dependent diffusion coefficient
    //-------------------------------------------------------------
    LOG() << "Height-dependent diffusion coefficient ... " << logger::flush;
    t.start();

    tau_ = diff * VectorXf::Ones(nz).array();

    for(int k = nz - nab; k < nz; ++k)
    {
        double sin_k = std::sin(0.5 * M_PI * ((k + 1) - (nz - nab)) / nab);
        tau_(k) = diff + (diffabs - diff) * (sin_k * sin_k);
    }

    LOG_SUCCESS(t);

    // Set up getter maps
    //-------------------------------------------------------------
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("zhtold", &zhtold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("zhtnow", &zhtnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("uold", &uold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("unow", &unow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("unew", &unew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("sold", &sold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("snow", &snow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("snew", &snew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("mtg", &mtg_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("mtgnew", &mtgnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("exn", &exn_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("prs", &prs_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qvold", &qvold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qvnow", &qvnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qvnew", &qvnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qrold", &qrold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qrnow", &qrnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qrnew", &qrnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qcold", &qcold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qcnow", &qcnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("qcnew", &qcnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("temp", &temp_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("nrold", &nrold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("nrnow", &nrnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("nrnew", &nrnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("ncold", &ncold_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("ncnow", &ncnow_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("ncnew", &ncnew_));
    matMap_.insert(std::make_pair<std::string, MatrixXf*>("dthetadt", &dthetadt_));

    vecMap_.insert(std::make_pair<std::string, VectorXf*>("topo", &topo_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("mtg0", &mtg0_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("exn0", &exn0_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("prs0", &prs0_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("tau", &tau_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("th0", &th0_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("prec", &prec_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("tot_prec", &tot_prec_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("sbnd1", &sbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("sbnd2", &sbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("ubnd1", &ubnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("ubnd2", &ubnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qvbnd1", &qvbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qvbnd2", &qvbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qcbnd1", &qcbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qcbnd2", &qcbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qrbnd1", &qrbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("qrbnd2", &qrbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("dthetadtbnd1", &dthetadtbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("dthetadtbnd2", &dthetadtbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("nrbnd1", &nrbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("nrbnd2", &nrbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("ncbnd1", &ncbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("ncbnd2", &ncbnd2_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("tbnd1", &tbnd1_));
    vecMap_.insert(std::make_pair<std::string, VectorXf*>("tbnd2", &tbnd2_));

    // Output initial fields
    //-------------------------------------------------------------
    if(iiniout)
        output_->makeOutput(this);
}

const MatrixXf& Solver::getMat(std::string name) const
{
    try
    {
        return *matMap_.at(name);
    }
    catch(std::out_of_range&)
    {
        throw IsenException("no matrix named '%s' in Solver", name);
    }
}

const VectorXf& Solver::getVec(std::string name) const
{
    try
    {
        return *vecMap_.at(name);
    }
    catch(std::out_of_range&)
    {
        throw IsenException("no vector named '%s' in Solver", name);
    }
}

Eigen::Map<MatrixXf> Solver::getField(std::string name) const
{
    if(matMap_.find(name) != matMap_.end())
    {
        const auto& mat = matMap_.at(name);
        return Eigen::Map<MatrixXf>(const_cast<double*>(mat->data()), mat->rows(), mat->cols());
    }
    else if(vecMap_.find(name) != vecMap_.end())
    {
        const auto& vec = vecMap_.at(name);
        return Eigen::Map<MatrixXf>(const_cast<double*>(vec->data()), vec->rows(), vec->cols());
    }
    else
        throw IsenException("no field named '%s' in Solver", name);
}

void Solver::run()
{
    SOLVER_DECLARE_ALL_ALIASES

    Timer t;

    Progressbar pbar(nts);
    const bool logIsDisabled = LOG().isDisabled();
    Progressbar::disableProgressbar = logIsDisabled;

    double curTime = 0;

    // Loop over all time steps
    //------------------------------------------------------------
    for(int i = 1; i < (nts + 1); ++i)
    {
        if(!iprtcfl)
            pbar.advance();

        curTime += dt;
        topofact_ = std::min(1., curTime / topotim);

        // Special treatment of first time step
        //--------------------------------------------------------
        dtdx_ = i == 1 ? 0.5 * dt / dx : dt / dx;

        // Prognostic step
        //--------------------------------------------------------

        // Isentropic mass density
        progIsendens();

        // Moisture scalars
        if(imoist)
            progMoisture();

        // Velocity
        progVelocity();

        // Exchange boundaries if periodic
        //--------------------------------------------------------
        if(!irelax)
            applyPeriodicBoundary();

        // Relaxation of prognostic fields
        //--------------------------------------------------------
        if(irelax)
            applyRelaxationBoundary();

        uold_.swap(unow_);
        sold_.swap(snow_);
        qvold_.swap(qvnow_);
        qcold_.swap(qcnow_);
        qrold_.swap(qrnow_);

        unow_.swap(unew_);
        snow_.swap(snew_);
        qvnow_.swap(qvnew_);
        qcnow_.swap(qcnew_);
        qrnow_.swap(qrnew_);

        // Diffusion and gravity wave absorber
        //--------------------------------------------------------
        horizontalDiffusion();

        if(!irelax)
            applyPeriodicBoundary();

        if(imoist)
            clipMoisture();

        unow_.swap(unew_);
        snow_.swap(snew_);
        qvnow_.swap(qvnew_);
        qcnow_.swap(qcnew_);
        qrnow_.swap(qrnew_);

        // Diagnostic step
        //--------------------------------------------------------

        // Pressure
        diagPressure();

        // Montgomorey
        diagMontgomery();

        // Calculation of geometric height (staggered)
        //--------------------------------------------------------
        zhtnow_.swap(zhtold_);
        geometricHeight();

        // Microphysics
        //---------------------------------------------------------
        if(imoist)
        {
            if(imicrophys == 1) // Kessler scheme
            {
                kessler_->apply(
                    // Output
                    temp_, qvnew_, qcnew_, qrnew_, tot_prec_, prec_,

                    // Input
                    th0_, prs_, snow_, qvnow_, qcnow_, qrnow_, exn_, zhtnow_);
            }
            else if(imicrophys == 2) // Two-moment scheme
            {
                //TODO...
            }

            if(imicrophys > 0)
            {
                if(idthdt) // Diabatic flow
                {
                    //TODO...
                }
            }
        }

        qvnow_.swap(qvnew_);
        qcnow_.swap(qcnew_);
        qrnow_.swap(qrnew_);

        // Check maximum CFL condition
        //--------------------------------------------------------
        double umax = computeCFL();
        double cflmax = umax * dtdx_;

        if(iprtcfl)
            std::printf("CFL max: %f U max: %f m/s \n", cflmax, umax);

        if(cflmax > 1)
            warning("isen", (boost::format("CFL condition violated (CFL max %f)") % cflmax).str());
        if(std::isnan(cflmax))
            error("isen", "model encountered NaN values");

        // Output every 'iout'-th time step
        //--------------------------------------------------------
        if((i % iout) == 0)
            output_->makeOutput(this);

#ifdef ISEN_PYTHON
        // Handle Python signals
        //--------------------------------------------------------
        if(PyErr_CheckSignals() == -1)
            throw IsenException("PySolver::run : signal caught");
#endif
    }

    pbar.pause();
    if(!logIsDisabled)
        Progressbar::printBar('=');

    if(logIsDisabled && itime)
        std::printf("Elapsed time: %s\n", timeString(t.stop()).c_str());

    LOG() << "Finished time loop ...";
    LOG_SUCCESS(t);
}

double Solver::computeCFL() const noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    double umax = -std::numeric_limits<double>::max();
    for(int k = 0; k < nz; ++k)
        for(int i = 0; i < nxb; ++i)
            umax = std::max(umax, std::fabs(unow_(i, k)));
    return umax;
}

void Solver::horizontalDiffusion() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const int nxnb = nx + nb;
    const int nxnb1 = nx + nb + 1;

    for(int k = 0; k < nz; ++k)
    {
        const double tau = tau_(k);
        const bool sel = tau_(k) > 0.0;
        const bool negSel = !sel;

        // Velocity
        for(int i = nb; i < nxnb1; ++i)
        {
            unew_(i, k) = sel * (unow_(i, k) + 0.25 * tau * (unow_(i - 1, k) - 2 * unow_(i, k) + unow_(i + 1, k)))
                          + negSel * unow_(i, k);
        }

        // Isentropic density
        for(int i = nb; i < nxnb; ++i)
        {
            snew_(i, k) = sel * (snow_(i, k) + 0.25 * tau * (snow_(i - 1, k) - 2 * snow_(i, k) + snow_(i + 1, k)))
                          + negSel * snow_(i, k);
        }

        if(imoist && imoist_diff)
        {
            // Water vapor (qv)
            for(int i = nb; i < nxnb; ++i)
            {
                qvnew_(i, k)
                    = sel * (qvnow_(i, k) + 0.25 * tau * (qvnow_(i - 1, k) - 2 * qvnow_(i, k) + qvnow_(i + 1, k)))
                      + negSel * qvnow_(i, k);
            }

            // Specific cloud water content (qc)
            for(int i = nb; i < nxnb; ++i)
            {
                qcnew_(i, k)
                    = sel * (qcnow_(i, k) + 0.25 * tau * (qcnow_(i - 1, k) - 2 * qcnow_(i, k) + qcnow_(i + 1, k)))
                      + negSel * qcnow_(i, k);
            }

            // Specific rain water content (qr)
            for(int i = nb; i < nxnb; ++i)
            {
                qrnew_(i, k)
                    = sel * (qrnow_(i, k) + 0.25 * tau * (qrnow_(i - 1, k) - 2 * qrnow_(i, k) + qrnow_(i + 1, k)))
                      + negSel * qrnow_(i, k);
            }
        }
    }
}

void Solver::geometricHeight() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    for(int i = 0; i < nxb; ++i)
        zhtnow_(i, 0) = topo_(i) * topofact_;

    const double rcpg05 = 0.5 * r / cp / g;
    for(int k = 1; k < nz1; ++k)
        for(int i = 0; i < nxb; ++i)
        {
            double th0exn = (th0_(k - 1) * exn_(i, k - 1) + th0_(k) * exn_(i, k));
            double prs = (prs_(i, k) - prs_(i, k - 1)) / (0.5 * (prs_(i, k) + prs_(i, k - 1)));
            zhtnow_(i, k) = zhtnow_(i, k - 1) - rcpg05 * th0exn * prs;
        }
}

void Solver::applyPeriodicBoundary() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    assert(!irelax);
    Boundary::periodic(snew_, nx, nb);
    Boundary::periodic(unew_, nx + 1, nb);

    if(imoist)
    {
        Boundary::periodic(qvnew_, nx, nb);
        Boundary::periodic(qcnew_, nx, nb);
        Boundary::periodic(qrnew_, nx, nb);

        if(imicrophys == 2)
        {
            Boundary::periodic(ncnew_, nx, nb);
            Boundary::periodic(nrnew_, nx, nb);
        }
    }
}

void Solver::applyRelaxationBoundary() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    assert(irelax);
    Boundary::relax(snew_, nx, nb, sbnd1_, sbnd2_);
    Boundary::relax(unew_, nx1, nb, ubnd1_, ubnd2_);

    if(imoist)
    {
        Boundary::relax(qvnew_, nx, nb, qvbnd1_, qvbnd2_);
        Boundary::relax(qcnew_, nx, nb, qcbnd1_, qcbnd2_);
        Boundary::relax(qrnew_, nx, nb, qrbnd1_, qrbnd2_);

        if(imicrophys == 2)
        {
            Boundary::relax(ncnew_, nx, nb, ncbnd1_, ncbnd2_);
            Boundary::relax(nrnew_, nx, nb, nrbnd1_, nrbnd2_);
        }
    }
}

void Solver::clipMoisture() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    auto clip = [&](MatrixXf& mat)
    {
        for(int k = 0; k < nz; ++k)
            for(int i = 0; i < nxb; ++i)
                mat(i, k) = mat(i, k) < 0.0 ? 0.0 : mat(i, k);
    };

    clip(qvnew_);
    clip(qcnew_);
    clip(qrnew_);
}

void Solver::diagMontgomery() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const double dth05 = dth * 0.5;
    const double gtopofact_ = g * topofact_;

    // Exner function
    for(int k = 0; k < nz1; ++k)
        for(int i = 0; i < nxb; ++i)
            exn_(i, k) = cp * std::pow(prs_(i, k) / pref, rdcp);

    // Montgomery
    for(int i = 0; i < nxb; ++i)
        mtg_(i, 0) = gtopofact_ * topo_(i) + th0_(0) * exn_(i, 0) + dth05 * exn_(i, 0);

    for(int k = 1; k < nz; ++k)
        for(int i = 0; i < nxb; ++i)
            mtg_(i, k) = mtg_(i, k - 1) + dth * exn_(i, k);
}

void Solver::diagPressure() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const double gdth = g * dth;

    for(int i = 0; i < nxb; ++i)
        prs_(i, nz) = prs0_(nz);

    for(int k = nz - 1; k >= 0; --k)
        for(int i = 0; i < nxb; ++i)
            prs_(i, k) = prs_(i, k + 1) + gdth * snow_(i, k);
}

void Solver::progIsendens() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const double dtdx05 = 0.5 * dtdx_;
    const int nxnb = nx + nb;

    for(int k = 0; k < nz; ++k)
        for(int i = nb; i < nxnb; ++i)
            snew_(i, k) = sold_(i, k)
                          - (dtdx05) * (snow_(i + 1, k) * (unow_(i + 2, k) + unow_(i + 1, k))
                                        - snow_(i - 1, k) * (unow_(i, k) + unow_(i - 1, k)));
}

void Solver::progVelocity() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const double dtdx = dtdx_;
    const double dtdx2 = 2 * dtdx_;
    const int nx1nb = nx + nb + 1;

    for(int k = 0; k < nz; ++k)
    {
        for(int i = nb; i < nx1nb; ++i)
        {
            unew_(i, k) = uold_(i, k) - dtdx * unow_(i, k) * (unow_(i + 1, k) - unow_(i - 1, k))
                          - dtdx2 * (mtg_(i, k) - mtg_(i - 1, k));
        }
    }
}

void Solver::progMoisture() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES

    const double dtdx05 = 0.5 * dtdx_;
    const int nxnb = nx + nb;

    // Water vapor (qv)
    for(int k = 0; k < nz; ++k)
        for(int i = nb; i < nxnb; ++i)
            qvnew_(i, k)
                = qvold_(i, k) - dtdx05 * (unow_(i, k) + unow_(i + 1, k)) * (qvnow_(i + 1, k) - qvnow_(i - 1, k));

    // Specific cloud water content (qc)
    for(int k = 0; k < nz; ++k)
        for(int i = nb; i < nxnb; ++i)
            qcnew_(i, k)
                = qcold_(i, k) - dtdx05 * (unow_(i, k) + unow_(i + 1, k)) * (qcnow_(i + 1, k) - qcnow_(i - 1, k));

    // Specific rain water content (qr)
    for(int k = 0; k < nz; ++k)
        for(int i = nb; i < nxnb; ++i)
            qrnew_(i, k)
                = qrold_(i, k) - dtdx05 * (unow_(i, k) + unow_(i + 1, k)) * (qrnow_(i + 1, k) - qrnow_(i - 1, k));
}

void Solver::progNumdens() noexcept
{
    SOLVER_DECLARE_ALL_ALIASES
}

void Solver::write(std::string filename)
{
    output_->write(filename);
}

ISEN_NAMESPACE_END


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_STRAIGHT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED

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

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2_straight capabilities

    Computes the remainder modulo \f$\pi/2\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r, rc;
    as_integer<T> n;
    rem_pio2_straight<Range>(x, n, r);
    @endcode

    is similar to:

    @code
    as_integer<T> n = idivround2even(x, Pio_2<T>());
    T r =  remainder(x, Pio_2<T>());
    @endcode

    @par Note:
    @c rem_pio2_straight computes the remainder modulo \f$\pi/2\f$ with straight algorithm,
    and returns an angle quadrant which is always 1.
    This is a very quick version accurate if the input
    is in \f$[\pi/4,\pi/2]\f$. In fact it only substract \f$\pi/2\f$ to the input
    so it can be viewed as a specially accurate minuspio_2 function outside
    the interval in which it can be used as a substitute to rem_pio2.

    The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm over the whole floating point range
    is over costly and implies the knowledge
    of a few hundred \f$pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on specific intervals.

    @see rem_pio2, rem_pio2_medium,rem_2pi, rem_pio2_cephes,

  **/
  const boost::dispatch::functor<tag::rem_pio2_straight_> rem_pio2_straight = {};
} }
#endif

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

#endif


#include "ray.h"
#include "utils.h"
#include <cmath>
#include <numeric>
#include <algorithm>
#include <Eigen/Dense>

using Eigen::Vector3d;

namespace batoid {
    Ray::Ray(double x, double y, double z, double vx, double vy, double vz, double _t=0.0,
             double _wavelength=0.0, double _flux=1.0, bool _vignetted=false) :
        r(Vector3d(x, y, z)), v(Vector3d(vx, vy, vz)), t(_t),
        wavelength(_wavelength), flux(_flux), vignetted(_vignetted), failed(false) {}

    Ray::Ray(Vector3d _r, Vector3d _v, double _t=0.0, double _wavelength=0.0,
             double _flux=1.0, bool _vignetted=false) :
        r(_r), v(_v), t(_t), wavelength(_wavelength), flux(_flux), vignetted(_vignetted),
        failed(false) {}

    Ray::Ray(const bool failed) :
        r(Vector3d::Zero()), v(Vector3d::Zero()), t(0.0), wavelength(0.0), flux(0.0),
        vignetted(true), failed(true) {}

    std::string Ray::repr() const {
        std::ostringstream oss("Ray(", std::ios_base::ate);
        if(failed)
            oss << "failed=True)";
        else {
            oss << "[" << r[0] << "," << r[1] << "," << r[2] << "],["
                << v[0] << "," << v[1] << "," << v[2] << "]";
            if (t != 0.0) oss << ", t=" << t;
            if (wavelength != 0.0) oss << ", wavelength=" << wavelength;
            if (flux != 1.0) oss << ", flux=" << flux;
            if (vignetted) oss << ", vignetted=True";
            oss << ")";
        }
        return oss.str();
    }

    Vector3d Ray::positionAtTime(const double _t) const {
        return r+v*(_t-t);
    }

    Ray Ray::propagatedToTime(const double _t) const {
        return Ray(positionAtTime(_t), v, _t, wavelength, flux, vignetted);
    }

    void Ray::propagateInPlace(const double _t) {
        r += v*(_t-t);
        t = _t;
    }

    bool Ray::operator==(const Ray& other) const {
        // All failed rays are equal
        if (failed)
            return other.failed;
        if (other.failed)
            return false;
        return (r == other.r) &&
               (v == other.v) &&
               (t == other.t) &&
               (wavelength == other.wavelength) &&
               (flux == other.flux) &&
               (vignetted == other.vignetted);
    }

    bool Ray::operator!=(const Ray& other) const {
        return !(*this == other);
    }

    double Ray::phase(const Vector3d& _r, double _t) const {
        return k().dot(_r-r) - (_t-t)*omega();
    }

    std::complex<double> Ray::amplitude(const Vector3d& _r, double _t) const {
        return std::exp(std::complex<double>(0, 1)*phase(_r, _t));
    }
}


#pragma once

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

#include "utilities.h"

namespace IMF = OPENEXR_IMF_NAMESPACE;

using namespace IMF;
using namespace IMATH_NAMESPACE;

using std::string;
using std::stringstream;
using std::vector;

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

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

Var x, y, c, i, pcx, pcy;

struct Point
{
    float x;
    float y;
    float z;
    uint16_t r;
    uint16_t g;
    uint16_t b;
};

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

Buffer<uint8_t> loadImages(vector<string> filenames)
{
    if (filenames.empty())
    {
        throw std::invalid_argument("List of filenames cannot be empty");
    }
    vector<Buffer<uint8_t>> images;
    Func assigner;
    assigner(x, y, c, i) = cast<uint8_t>(0);
    for (int i = 0; i < filenames.size(); i++)
    {
        const auto filename = filenames[i];
        images.push_back(load_image(filename));
        assigner(x, y, c, i) = images[i](x, y, c);
    }
    Buffer<uint8_t> input(images[0].width(), images[0].height(), images[0].channels(), images.size());
    assigner.realize(input);
    return input;
}

Vector4h makeAxBzCLine(Vector4h a, Vector4h b)
{
    Vector4h result;
    result(0) = a(2) - b(2);
    result(1) = b(0) - a(0);
    result(2) = a(0) * b(2) - b(0) * a(2);
    result(3) = 1.0f;
    return result;
}

Vector4h cross3(Vector4h a, Vector4h b)
{
    Vector4h res;
    res(0) = a(1) * b(2) - a(2) * b(1);
    res(1) = a(2) * b(0) - a(0) * b(2);
    res(2) = a(0) * b(1) - a(1) * b(0);
    res(3) = 0;
    return res;
}

Expr dot3(Vector4h a, Vector4h b)
{
    Expr res;
    res = a(0) * b(0) + a(1) * b(1) + a(2) * b(2);
    return res;
}

tuple<Vector4h, Vector4h> pluckerLine(Vector4h p1, Vector4h p2)
{
    Vector4h l;
    Vector4h l_dash;
    l = p1(3) * p2 - p2(3) * p1;
    l(3) = 0;
    l_dash = cross3(p1, p2);
    return {l, l_dash};
}

Vector4h pluckerPlane(Vector4h l, Vector4h l_dash, Vector4h pluckPt)
{
    Vector4h u;
    u = -pluckPt(3) * l_dash + cross3(pluckPt, l);
    u(3) = dot3(pluckPt, l_dash);
    return u;
}

Vector4h intersectionLinePlane(Vector4h l, Vector4h l_dash, Vector4h plPlane)
{
    Vector4h intersection;
    intersection = -plPlane(3) * l + cross3(plPlane, l_dash);
    intersection(3) = dot3(plPlane, l);
    return intersection;
}

bool conditionProjector(float x, float y, float z)
{
    return (x < -10.0f || x > 10.0f || y < -10.0f || y > 10.0f || z < -10.0f || z > 10.0f);
}

bool noCondition(float x, float y, float z)
{
    return false;
}

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

template <typename Function>
void writeBufferToXYZFile(Buffer<float> &buffer, Buffer<uint8_t> &color, string filename, string deliminator, Function condFunc)
{
    std::vector<Point> points;
    for (int j = 0; j < buffer.height(); j++)
    {
        for (int i = 0; i < buffer.width(); i++)
        {
            const auto x = buffer(i, j, 0);
            const auto y = buffer(i, j, 1);
            const auto z = buffer(i, j, 2);
            uint8_t r = color(i, j, 0);
            uint8_t g = color(i, j, 1);
            uint8_t b = color(i, j, 2);
            if (condFunc(x, y, z))
            {
                continue;
            }
            points.push_back({x, y, z, r, g, b});
        }
    }
    std::ofstream outFile;
    outFile.open(filename);
    outFile << points.size() << "\n";
    const auto d = deliminator;
    for (const auto &point : points)
    {
        outFile << point.x << d << point.y << d << point.z << d << point.r << d << point.g << d << point.b << "\n";
    }
}

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

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

int main(int argc, char **argv)
{
    Buffer<uint8_t> input = load_image("images/scan-images/scan_30.png");

    Expr redFilter = input(x, y, 0) > 140;

    Matrix4h K = read4x4MatFromCSV("matrices/cam-mat.csv");
    Matrix4h invK = read4x4MatFromCSV("matrices/inv-cam-mat.csv");
    Matrix4h T_CP = read4x4MatFromCSV("matrices/T-C1-C2.csv");
    Matrix4h T_WC = read4x4MatFromCSV("matrices/T-W-C1.csv");
    Vector4h t_CP{0, 0, 0, 0};
    t_CP(0) = T_CP(0, 3);
    t_CP(1) = T_CP(1, 3);
    t_CP(2) = T_CP(2, 3);
    t_CP(3) = T_CP(3, 3);
    cout << t_CP << endl;

    Vector4h scanNormal_P{1.0f, 0.0f, 0.0f, 1.0f};
    Vector4h scanPlane_C = T_CP * scanNormal_P;
    scanPlane_C(3) = -dot3(scanPlane_C, t_CP);

    cout << scanPlane_C << endl;

    Vector4h px{x, y, 1.0f, 0.0f};
    Vector4h normCoord_C = invK * px;
    Vector4h lineDash{0.0f, 0.0f, 0.0f, 0.0f};
    Vector4h intersect = intersectionLinePlane(normCoord_C, lineDash, scanPlane_C);
    intersect = intersect * redFilter;
    intersect = intersect / intersect[3];
    intersect = T_WC * intersect;
    intersect = intersect / intersect[3];
    //saveImage(intersect[2], 1920, 1080, "images/debug/depth.png");

    Func result;
    result(x, y, c) = 0.0f;
    result(x, y, 0) = intersect[0];
    result(x, y, 1) = intersect[1];
    result(x, y, 2) = intersect[2];

    Buffer<float> output(1920, 1080, 3);
    result.realize(output);
    vector<Point> points;
    std::ofstream outFile;
    outFile.open("pointclouds/singleScan.txt");

    for (int xxx = 0; xxx < output.width(); xxx++)
    {
        for (int yyy = 0; yyy < output.height(); yyy++)
        {

            float xPoint = output(xxx, yyy, 0);
            float yPoint = output(xxx, yyy, 1);
            float zPoint = output(xxx, yyy, 2);
            if (abs(zPoint > 0.01))
            {
                outFile << xPoint << ";" << yPoint << ";" << zPoint << "\n";
            }
            points.push_back({xPoint, yPoint, zPoint, 0, 0, 0});
        }
    }
    outFile.close();

    //debugImageEXR(intersect[2], intersect[2], intersect[2], 1920, 1080, "images/debug/depth.exr");
}


#include "opq.h"

#include <boost/math/constants/constants.hpp>
#include <boost/thread/once.hpp>

#include "externals/cxx/namespace.hpp"
#include "externals/cxx/pretty_function.hpp"

#include <stdexcept>

BEGIN_NAMESPACE(rysq, asymptotic)

template<size_t N, typename T>
struct asymptotic_ {
    static void roots(T X, T *R, T *W) {
	boost::call_once(once_flag, &asymptotic_<N,T>::initialize);
	T r = 1/X;
	T w = 1/sqrt(X);
	for (size_t i = 0; i < N; ++i) {
	    R[i] = r*R_[i];
	    W[i] = w*W_[i];
	}
    }
private:
    static T R_[N], W_[N];
    static boost::once_flag once_flag;
    static void initialize() {
	static const size_t n = 2*N;
	T beta[n], alpha[n] = { 0 };
	beta[0] = boost::math::constants::root_pi<T>();
	for (size_t i = 1; i < n; ++i) {
	    beta[i] = T(i)/2;
	}
	T r[n], w[n];
	int status = opq::coefficients(n, alpha, beta, r, w);
	if (status != 0)  {
	    throw std::runtime_error(PRETTY_FUNCTION("opq::coefficients returned ",
						     status));
	}
	// CALL RYSGW_(N,ALPHA,BETA,EPS,RTS,W_TS,IERR,W_RK) for
	for (size_t i = 0; i < N; ++i) {
	    size_t j = i + N;
	    R_[i] = r[j]*r[j];
	    W_[i] = w[j];
	}
    }
};
template<size_t N, typename T> T asymptotic_<N,T>::R_[N];
template<size_t N, typename T> T asymptotic_<N,T>::W_[N];

template<size_t N, typename T>
boost::once_flag asymptotic_<N,T>::once_flag = BOOST_ONCE_INIT;

template<size_t N, typename T>
void roots(T X, T *R, T *W) {
    asymptotic_<N,T>::roots(X, R, W);
}

END_NAMESPACE(rysq, asymptotic)


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/Shapes/PointGroupElements.h"

#include "Molassembler/Temple/Functional.h"
#include "boost/optional.hpp"
#include <Eigen/Geometry>

namespace Scine {
namespace Molassembler {
namespace Shapes {
namespace Elements {
namespace {

template<typename EnumType>
constexpr auto underlying(const EnumType e) {
  return static_cast<std::underlying_type_t<EnumType>>(e);
}

inline bool collinear(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
  return std::fabs(std::fabs(a.dot(b) / (a.norm() * b.norm())) - 1) <= 1e-8;
}

inline bool orthogonal(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
  return std::fabs(a.dot(b) / (a.norm() * b.norm())) <= 1e-8;
}

template<typename T>
inline std::unique_ptr<SymmetryElement> wrap(T&& element) {
  using Decayed = std::decay_t<T>;
  return std::make_unique<Decayed>(std::forward<T>(element));
}

void addProperAxisElements(ElementsList& list, const Eigen::Vector3d& axis, const unsigned n) {
  // C2 gives only a C2, but C3 should also give a C3², etc.
  const Rotation element = Rotation::Cn(axis, n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    list.push_back(wrap(composite));
    composite = element * composite;
  }
}

void addImproperAxisElements(ElementsList& list, const Eigen::Vector3d& axis, const unsigned n) {
  const Rotation element = Rotation::Sn(axis, n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    list.push_back(wrap(composite));
    composite = element * composite;
  }
}

ElementsList Cnh(const unsigned n) {
  const auto sigma_xy = Reflection::sigma_xy();

  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.push_back(wrap(sigma_xy));
  std::vector<Rotation> rotations;
  const Rotation element = Rotation::Cn_z(n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    rotations.push_back(composite);
    composite = element * composite;
  }
  const unsigned S = rotations.size();
  // Add sigma_xy modified Cn axes
  for(unsigned i = 0; i < S; ++i) {
    rotations.push_back(sigma_xy * rotations.at(i));
  }
  for(auto& rotation : rotations) {
    elements.emplace_back(
      wrap(std::move(rotation))
    );
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Cnv(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addProperAxisElements(elements, Eigen::Vector3d::UnitZ(), n);
  // Reflection planes include z and increment by pi/n along z
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d planeNormal = Eigen::Vector3d::UnitY();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(
      wrap(Reflection(planeNormal))
    );
    planeNormal = rotation.matrix() * planeNormal;
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Dn(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addProperAxisElements(elements, Eigen::Vector3d::UnitZ(), n);
  // Dn groups have C2 axes along pi/n increments in the xy plane
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d c2axis = Eigen::Vector3d::UnitX();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    c2axis = rotation.matrix() * c2axis;
  }
  assert(elements.size() == 2 * n);
  return elements;
}

ElementsList Dnh(const unsigned n) {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.push_back(wrap(Reflection::sigma_xy()));
  elements.reserve(4 * n);
  std::vector<Rotation> rotations;
  const Rotation element = Rotation::Cn_z(n);
  Rotation composite = element;
  for(unsigned i = n; i > 1; --i) {
    rotations.push_back(composite);
    composite = element * composite;
  }
  const unsigned S = rotations.size();
  // Generate the S_n axes from the sigma_xy * C_n
  for(unsigned i = 0; i < S; ++i) {
    rotations.push_back(Reflection::sigma_xy() * rotations.at(i));
  }
  for(auto& rotation : rotations) {
    elements.emplace_back(
      wrap(std::move(rotation))
    );
  }

  /* Dnh groups have C2 axes along pi/n increments in the xy plane
   * and sigma_v planes perpendicular to those C2 axes
   */
  const auto rotation = Rotation::Cn_z(2 * n);
  Eigen::Vector3d c2axis = Eigen::Vector3d::UnitX();
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    elements.push_back(
      wrap(Reflection(
        Eigen::Vector3d::UnitZ().cross(c2axis)
      ))
    );
    c2axis = rotation.matrix() * c2axis;
  }

  assert(elements.size() == 4 * n);
  return elements;
}

ElementsList Dnd(const unsigned n) {
  const Eigen::Vector3d e_x = Eigen::Vector3d::UnitX();
  const Eigen::Vector3d e_z = Eigen::Vector3d::UnitZ();

  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  addImproperAxisElements(elements, e_z, 2 * n);
  /* C2 axes */
  const auto rotationMatrix = Rotation::Cn_z(2 * n).matrix();
  Eigen::Vector3d c2axis = e_x;
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Rotation::Cn(c2axis, 2)));
    c2axis = rotationMatrix * c2axis;
  }
  /* sigma_ds */
  Eigen::Vector3d planeNormal = (e_x + rotationMatrix * e_x).normalized().cross(e_z);
  for(unsigned i = 0; i < n; ++i) {
    elements.push_back(wrap(Reflection(planeNormal)));
    planeNormal = rotationMatrix * planeNormal;
  }
  assert(elements.size() == 4 * n);
  return elements;
}

ElementsList I() {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.reserve(60);

  const double phi = (1 + std::sqrt(5)) / 2;
  Eigen::Matrix<double, 3, 6> axes;
  axes << 0.0, 0.0, phi, -phi, 1.0, 1.0,
          1.0, 1.0, 0.0, 0.0, phi, -phi,
          phi, -phi, 1.0, 1.0, 0.0, 0.0;

  /* 12 C5, 12 C5^2 */
  for(unsigned i = 0; i < 6; ++i) {
    addProperAxisElements(elements, axes.col(i), 5);
  }

  /* 15 C2 along sums of two positions*/
  const Eigen::Matrix3d C5 = Eigen::AngleAxisd(2 * M_PI / 5, axes.col(0).normalized()).toRotationMatrix();
  Eigen::Matrix3d twoAxesBases;
  twoAxesBases.col(0) = (axes.col(0) + axes.col(2)) / 2;
  twoAxesBases.col(1) = (axes.col(2) + axes.col(4)) / 2;
  twoAxesBases.col(2) = (axes.col(2) - axes.col(3)) / 2;
  for(unsigned i = 0; i < 3; ++i) {
    Eigen::Vector3d compoundAxis = twoAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 2)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  /* 20 C3 along sums of three positions*/
  Eigen::Matrix<double, 3, 2> threeAxesBases;
  threeAxesBases.col(0) = (axes.col(0) + axes.col(2) + axes.col(4)) / 3;
  threeAxesBases.col(1) = (axes.col(2) + axes.col(4) - axes.col(3)) / 3;
  for(unsigned i = 0; i < 2; ++i) {
    Eigen::Vector3d compoundAxis = threeAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 3)));
      elements.push_back(wrap(Rotation::Cn(-compoundAxis, 3)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  assert(elements.size() == 60);
  return elements;
}

ElementsList Ih() {
  ElementsList elements;
  elements.push_back(wrap(Identity::E()));
  elements.reserve(120);
  /* i */
  elements.push_back(wrap(Inversion::i()));
  const double phi = (1 + std::sqrt(5)) / 2;
  Eigen::Matrix<double, 3, 6> axes;
  axes << 0.0, 0.0, phi, -phi, 1.0, 1.0,
          1.0, 1.0, 0.0, 0.0, phi, -phi,
          phi, -phi, 1.0, 1.0, 0.0, 0.0;

  /* 12 S10, 12 S10^3, 12 C5, 12 C5^2 */
  for(unsigned i = 0; i < 6; ++i) {
    const auto& axis = axes.col(i);
    elements.push_back(wrap(Rotation::Sn(axis, 10)));
    elements.push_back(wrap(Rotation::Sn(-axis, 10)));
    elements.push_back(wrap(Rotation::Sn(axis, 10, 3)));
    elements.push_back(wrap(Rotation::Sn(-axis, 10, 3)));
    // C5, C5^2, C5^3 = -C5^2, C5^4 = -C5
    addProperAxisElements(elements, axis, 5);
  }

  /* 15 C2, 15 sigma along sums of two positions */
  const Eigen::Matrix3d C5 = Eigen::AngleAxisd(2 * M_PI / 5, axes.col(0).normalized()).toRotationMatrix();
  Eigen::Matrix3d twoAxesBases;
  twoAxesBases.col(0) = (axes.col(0) + axes.col(2)) / 2;
  twoAxesBases.col(1) = (axes.col(2) + axes.col(4)) / 2;
  twoAxesBases.col(2) = (axes.col(2) - axes.col(3)) / 2;
  for(unsigned i = 0; i < 3; ++i) {
    Eigen::Vector3d compoundAxis = twoAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 2)));
      elements.push_back(wrap(Reflection(compoundAxis)));
      compoundAxis = C5 * compoundAxis;
    }
  }

  /* 20 S6, 20 C3 along sums of three positions */
  Eigen::Matrix<double, 3, 2> threeAxesBases;
  threeAxesBases.col(0) = (axes.col(0) + axes.col(2) + axes.col(4)) / 3;
  threeAxesBases.col(1) = (axes.col(2) + axes.col(4) - axes.col(3)) / 3;
  for(unsigned i = 0; i < 2; ++i) {
    Eigen::Vector3d compoundAxis = threeAxesBases.col(i);
    for(unsigned j = 0; j < 5; ++j) {
      elements.push_back(wrap(Rotation::Sn(compoundAxis, 6)));
      elements.push_back(wrap(Rotation::Sn(-compoundAxis, 6)));
      elements.push_back(wrap(Rotation::Cn(compoundAxis, 3)));
      elements.push_back(wrap(Rotation::Cn(-compoundAxis, 3)));
      compoundAxis = C5 * compoundAxis;
    }
  }
  assert(elements.size() == 120);
  return elements;
}

} // namespace

Identity Identity::E() {
  return {};
}

SymmetryElement::Matrix Identity::matrix() const {
  return Matrix::Identity();
}

boost::optional<SymmetryElement::Vector> Identity::vector() const {
  return boost::none;
}

std::string Identity::name() const {
  return "E";
}

Inversion Inversion::i() {
  return {};
}

SymmetryElement::Matrix Inversion::matrix() const {
  return -Matrix::Identity();
}

boost::optional<SymmetryElement::Vector> Inversion::vector() const {
  return boost::none;
}

std::string Inversion::name() const {
  return "i";
}

Rotation::Rotation(
  const Eigen::Vector3d& passAxis,
  const unsigned passN,
  const unsigned passPower,
  const bool passReflect
) : axis(passAxis.normalized()),
    n(passN),
    power(passPower),
    reflect(passReflect)
{}

Rotation Rotation::Cn(const Eigen::Vector3d& axis, const unsigned n, const unsigned power) {
  return Rotation(axis, n, power, false);
}

Rotation Rotation::Sn(const Eigen::Vector3d& axis, const unsigned n, const unsigned power) {
  return Rotation(axis, n, power, true);
}

Rotation Rotation::operator * (const Rotation& rhs) const {
  if(collinear(axis, rhs.axis)) {
    if(n == rhs.n) {
      return Rotation(axis, n, power + rhs.power, reflect xor rhs.reflect);
    }

    throw std::logic_error("Rotation data model cannot handle collinear multiplication of axes of different order n");
  }

  if(orthogonal(axis, rhs.axis)) {
    // Rotate rhs' axis by *this, but keep everything else
    return Rotation(matrix() * rhs.axis, rhs.n, rhs.power, rhs.reflect);
  }

  throw std::logic_error("Rotation data model cannot handle non-orthogonal multiplication of rotations");
}

SymmetryElement::Matrix Rotation::matrix() const {
  if(!reflect) {
    return Eigen::AngleAxisd(2 * M_PI * power / n, axis).toRotationMatrix();
  }

  const double angle = 2 * M_PI * power / n;
  const double sine = std::sin(angle);
  const double cosine = std::cos(angle);
  const double onePlusCosine = 1 + cosine;

  const double xx = cosine - axis(0) * axis(0) * onePlusCosine;
  const double yy = cosine - axis(1) * axis(1) * onePlusCosine;
  const double zz = cosine - axis(2) * axis(2) * onePlusCosine;

  const double xy = - axis(0) * axis(1) * onePlusCosine;
  const double xz = - axis(0) * axis(2) * onePlusCosine;
  const double yz = - axis(1) * axis(2) * onePlusCosine;

  const double x = axis(0) * sine;
  const double y = axis(1) * sine;
  const double z = axis(2) * sine;

  Eigen::Matrix3d rotationMatrix;

  rotationMatrix <<
        xx, xy - z, xz + y,
    xy + z,     yy, yz - x,
    xz - y, yz + x,     zz;

  return rotationMatrix;
}

boost::optional<SymmetryElement::Vector> Rotation::vector() const {
  return axis;
}

std::string Rotation::name() const {
  std::string composite = (reflect ? "S" : "C");
  composite += std::to_string(n);
  if(power > 1) {
    composite += "^" + std::to_string(power);
  }
  if(std::fabs(axis.z()) < 1e-8) {
    composite += "'";
  } else if(std::fabs(axis.x()) + std::fabs(axis.y()) > 1e-8) {
    composite += (
      " along {"
      + std::to_string(axis.x()) + ", "
      + std::to_string(axis.y()) + ", "
      + std::to_string(axis.z()) + "}"
    );
  }
  return composite;
}

Reflection::Reflection(const Eigen::Vector3d& passNormal) : normal(passNormal.normalized()) {}

Reflection Reflection::sigma_xy() {
  return Reflection(Eigen::Vector3d::UnitZ());
}

Reflection Reflection::sigma_xz() {
  return Reflection(Eigen::Vector3d::UnitY());
}

Reflection Reflection::sigma_yz() {
  return Reflection(Eigen::Vector3d::UnitX());
}

SymmetryElement::Matrix Reflection::matrix() const {
  Eigen::Matrix3d reflection;

  const double normalSquareNorm = normal.squaredNorm();

  for(unsigned i = 0; i < 3; ++i) {
    for(unsigned j = 0; j < 3; ++j) {
      reflection(i, j) = (i == j ? 1 : 0) - 2 * normal(i) * normal(j) / normalSquareNorm;
    }
  }

  return reflection;
}

boost::optional<SymmetryElement::Vector> Reflection::vector() const {
  if(orthogonal(normal, Eigen::Vector3d::UnitZ())) {
    return normal.cross(Eigen::Vector3d::UnitZ());
  }

  if(orthogonal(normal, Eigen::Vector3d::UnitX())) {
    return normal.cross(Eigen::Vector3d::UnitX());
  }

  if(orthogonal(normal, Eigen::Vector3d::UnitY())) {
    return normal.cross(Eigen::Vector3d::UnitY());
  }

  return boost::none;
}

std::string Reflection::name() const {
  std::string composite = "sigma";

  if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitZ(), 1e-8)) {
    composite += "h_";
  } else if(orthogonal(normal, Eigen::Vector3d::UnitZ())) {
    composite += "v_";
  } else {
    composite += (
      " w/ normal {"
      + std::to_string(normal.x()) + ", "
      + std::to_string(normal.y()) + ", "
      + std::to_string(normal.z()) + "}"
    );
  }

  if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitX(), 1e-8)) {
    composite += " (yz)";
  } else if(normal.cwiseAbs().isApprox(Eigen::Vector3d::UnitY(), 1e-8)) {
    composite += " (xz)";
  }

  return composite;
}

Rotation operator * (const Rotation& rot, const Reflection& reflection) {
  if(!collinear(rot.axis, reflection.normal)) {
    throw std::logic_error("Cannot handle off-axis Rotation / Reflection combination");
  }

  return Rotation(rot.axis, rot.n, rot.power, !rot.reflect);
}

Rotation operator * (const Reflection& reflection, const Rotation& rot) {
  return rot * reflection;
}

Eigen::Matrix3d improperRotationMatrix(
  const Eigen::Vector3d& axis,
  const double angle
) {
  const double sine = std::sin(angle);
  const double cosine = std::cos(angle);
  const double onePlusCosine = 1 + cosine;

  const double xx = cosine - axis(0) * axis(0) * onePlusCosine;
  const double yy = cosine - axis(1) * axis(1) * onePlusCosine;
  const double zz = cosine - axis(2) * axis(2) * onePlusCosine;

  const double xy = - axis(0) * axis(1) * onePlusCosine;
  const double xz = - axis(0) * axis(2) * onePlusCosine;
  const double yz = - axis(1) * axis(2) * onePlusCosine;

  const double x = axis(0) * sine;
  const double y = axis(1) * sine;
  const double z = axis(2) * sine;

  Eigen::Matrix3d rotationMatrix;

  rotationMatrix <<
        xx, xy - z, xz + y,
    xy + z,     yy, yz - x,
    xz - y, yz + x,     zz;

  return rotationMatrix;
}

Eigen::Matrix3d properRotationMatrix(
  const Eigen::Vector3d& axis,
  const double angle
) {
  return Eigen::AngleAxisd(angle, axis).toRotationMatrix();
}

Eigen::Matrix3d reflectionMatrix(const Eigen::Vector3d& planeNormal) {
  Eigen::Matrix3d reflection;

  const double normalSquareNorm = planeNormal.squaredNorm();

  for(unsigned i = 0; i < 3; ++i) {
    for(unsigned j = 0; j < 3; ++j) {
      reflection(i, j) = (i == j ? 1 : 0) - 2 * planeNormal(i) * planeNormal(j) / normalSquareNorm;
    }
  }

  return reflection;
}

//! Returns all symmetry elements of a point group
ElementsList symmetryElements(PointGroup group) noexcept {
  if(group == PointGroup::Cinfv) {
    group = PointGroup::C8v;
  }

  if(group == PointGroup::Dinfh) {
    group = PointGroup::D8h;
  }

  const Identity E {};
  const Inversion inversion {};

  const auto e_x = Eigen::Vector3d::UnitX();
  const auto e_y = Eigen::Vector3d::UnitY();
  const auto e_z = Eigen::Vector3d::UnitZ();

  const Reflection sigma_xy {e_z};
  const Reflection sigma_xz {e_y};
  const Reflection sigma_yz {e_x};

  const double tetrahedronAngle = 2 * std::atan(std::sqrt(2));

  ElementsList elements;
  elements.push_back(wrap(E));

  switch(group) {
    case(PointGroup::C1):
      return elements;

    case(PointGroup::Ci):
      {
        elements.push_back(wrap(inversion));
        return elements;
      }

    case(PointGroup::Cs):
      {
        elements.push_back(wrap(sigma_xy));
        return elements;
      }

    case(PointGroup::C2):
    case(PointGroup::C3):
    case(PointGroup::C4):
    case(PointGroup::C5):
    case(PointGroup::C6):
    case(PointGroup::C7):
    case(PointGroup::C8):
      {
        const unsigned n = 2 + underlying(group) - underlying(PointGroup::C2);
        addProperAxisElements(elements, e_z, n);
        assert(elements.size() == n);
        return elements;
      }

    case(PointGroup::C2h):
    case(PointGroup::C3h):
    case(PointGroup::C4h):
    case(PointGroup::C5h):
    case(PointGroup::C6h):
    case(PointGroup::C7h):
    case(PointGroup::C8h):
      {
        const unsigned n = 2 + underlying(group) - underlying(PointGroup::C2h);
        std::vector<Rotation> rotations;
        const Rotation element = Rotation::Cn(e_z, n);
        Rotation composite = element;
        for(unsigned i = n; i > 1; --i) {
          rotations.push_back(composite);
          composite = element * composite;
        }
        const unsigned S = rotations.size();
        // Add sigma_xy modified Cn axes
        for(unsigned i = 0; i < S; ++i) {
          rotations.push_back(sigma_xy * rotations.at(i));
        }
        return Cnh(2 + underlying(group) - underlying(PointGroup::C2h));
      }

    case(PointGroup::C2v):
    case(PointGroup::C3v):
    case(PointGroup::C4v):
    case(PointGroup::C5v):
    case(PointGroup::C6v):
    case(PointGroup::C7v):
    case(PointGroup::C8v):
      {
        return Cnv(2 + underlying(group) - underlying(PointGroup::C2v));
      }

    case(PointGroup::S4):
    case(PointGroup::S6):
    case(PointGroup::S8):
      {
        const unsigned n = 4 + 2 * (underlying(group) - underlying(PointGroup::S4));
        addImproperAxisElements(elements, e_z, n);
        assert(elements.size() == n);
        return elements;
      }

    case(PointGroup::D2):
    case(PointGroup::D3):
    case(PointGroup::D4):
    case(PointGroup::D5):
    case(PointGroup::D6):
    case(PointGroup::D7):
    case(PointGroup::D8):
      {
        return Dn(2 + underlying(group) - underlying(PointGroup::D2));
      }

    case(PointGroup::D2h):
    case(PointGroup::D3h):
    case(PointGroup::D4h):
    case(PointGroup::D5h):
    case(PointGroup::D6h):
    case(PointGroup::D7h):
    case(PointGroup::D8h):
      {
        return Dnh(2 + underlying(group) - underlying(PointGroup::D2h));
      }

    case(PointGroup::D2d):
    case(PointGroup::D3d):
    case(PointGroup::D4d):
    case(PointGroup::D5d):
    case(PointGroup::D6d):
    case(PointGroup::D7d):
    case(PointGroup::D8d):
      {
        return Dnd(2 + underlying(group) - underlying(PointGroup::D2d));
      }

    /* Cubic groups */
    case(PointGroup::T):
      {
        elements.reserve(12);
        const Eigen::Vector3d axis_2 = properRotationMatrix(e_y, tetrahedronAngle) * e_z;
        const Eigen::Vector3d axis_3 = Rotation::Cn(e_z, 3).matrix() * axis_2;
        const Eigen::Vector3d axis_4 = Rotation::Cn(e_z, 3).matrix() * axis_3;
        addProperAxisElements(elements, e_z, 3);
        addProperAxisElements(elements, axis_2, 3);
        addProperAxisElements(elements, axis_3, 3);
        addProperAxisElements(elements, axis_4, 3);
        elements.push_back(wrap(Rotation::Cn((e_z + axis_2).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_z + axis_3).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_z + axis_4).normalized(), 2)));
        assert(elements.size() == 12);
        return elements;
      }

    case(PointGroup::Td):
      {
        elements.reserve(24);
        Eigen::Matrix<double, 3, Eigen::Dynamic> positions(3, 4);
        positions.col(0) = e_z;
        positions.col(1) = properRotationMatrix(e_y, tetrahedronAngle) * e_z;
        positions.col(2) = Rotation::Cn(e_z, 3).matrix() * positions.col(1);
        positions.col(3) = Rotation::Cn(e_z, 3).matrix() * positions.col(2);
        // C3 axes
        addProperAxisElements(elements, e_z, 3);
        addProperAxisElements(elements, positions.col(1), 3);
        addProperAxisElements(elements, positions.col(2), 3);
        addProperAxisElements(elements, positions.col(3), 3);
        const Eigen::Vector3d axis_12 = (e_z + positions.col(1)).normalized();
        const Eigen::Vector3d axis_13 = (e_z + positions.col(2)).normalized();
        const Eigen::Vector3d axis_14 = (e_z + positions.col(3)).normalized();
        // S4, C2, S4^3
        addImproperAxisElements(elements, axis_12, 4);
        addImproperAxisElements(elements, axis_13, 4);
        addImproperAxisElements(elements, axis_14, 4);
        // Sigma d
        for(unsigned i = 0; i < 3; ++i) {
          for(unsigned j = i + 1; j < 4; ++j) {
            elements.push_back(
              wrap(Reflection(
                positions.col(i).cross(positions.col(j))
              ))
            );
          }
        }
        assert(elements.size() == 24);
        return elements;
      }
    case(PointGroup::Th):
      {
        elements.reserve(24);
        /* i */
        elements.push_back(wrap(inversion));
        /* 4 S6, 4 C3, 4 C3^2, 4 S6^5 along lin. comb. of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6, 5)));
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6, 5)));
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6, 5)));
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6)));
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6, 5)));
        }
        /* 3 C2 along axes */
        elements.push_back(wrap(Rotation::Cn(e_x, 2)));
        elements.push_back(wrap(Rotation::Cn(e_y, 2)));
        elements.push_back(wrap(Rotation::Cn(e_z, 2)));
        /* 3 sigma_h with normals along axes */
        elements.push_back(wrap(Reflection(e_x)));
        elements.push_back(wrap(Reflection(e_y)));
        elements.push_back(wrap(Reflection(e_z)));
        assert(elements.size() == 24);
        return elements;
      }

    case(PointGroup::O):
      {
        elements.reserve(24);
        /* 6 C4, 3 C2 (C4^2) == C4, C2 and C4^3 along the coordinate axes */
        addProperAxisElements(elements, e_x, 4);
        addProperAxisElements(elements, e_y, 4);
        addProperAxisElements(elements, e_z, 4);
        /* 8 C3 along linear combinations of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
        }
        /* 6 C2' along combinations of two axes */
        elements.push_back(wrap(Rotation::Cn((e_x + e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y - e_z).normalized(), 2)));
        assert(elements.size() == 24);
        return elements;
      }
    case(PointGroup::Oh):
      {
        elements.push_back(wrap(inversion));
        elements.reserve(48);
        /* 8 C3 and 8 S6 share the linear combinations of three axes */
        { // +++ <-> ---
          const Eigen::Vector3d axis_ppp = (  e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_ppp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_ppp, 6)));
        }
        { // ++- <-> --+
          const Eigen::Vector3d axis_ppm = (  e_x + e_y - e_z).normalized();
          addProperAxisElements(elements, axis_ppm, 3);
          elements.push_back(wrap(Rotation::Sn(axis_ppm, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_ppm, 6)));
        }
        { // +-+ <-> -+-
          const Eigen::Vector3d axis_pmp = (  e_x - e_y + e_z).normalized();
          addProperAxisElements(elements, axis_pmp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_pmp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_pmp, 6)));
        }
        { // -++ <-> +--
          const Eigen::Vector3d axis_mpp = (- e_x + e_y + e_z).normalized();
          addProperAxisElements(elements, axis_mpp, 3);
          elements.push_back(wrap(Rotation::Sn(axis_mpp, 6)));
          elements.push_back(wrap(Rotation::Sn(-axis_mpp, 6)));
        }
        /* 6 C2 along linear combinations of two axes */
        elements.push_back(wrap(Rotation::Cn((e_x + e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_y).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_x - e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y + e_z).normalized(), 2)));
        elements.push_back(wrap(Rotation::Cn((e_y - e_z).normalized(), 2)));
        /* 6 C4 and 3 C2 (C4^2) along axes */
        addProperAxisElements(elements, e_x, 4);
        addProperAxisElements(elements, e_y, 4);
        addProperAxisElements(elements, e_z, 4);
        /* 6 S4 along axes */
        elements.push_back(wrap(Rotation::Sn(  e_x, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_x, 4)));
        elements.push_back(wrap(Rotation::Sn(  e_y, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_y, 4)));
        elements.push_back(wrap(Rotation::Sn(  e_z, 4)));
        elements.push_back(wrap(Rotation::Sn(- e_z, 4)));
        /* 3 sigma h along combinations of two axes */
        elements.push_back(wrap(sigma_xy));
        elements.push_back(wrap(sigma_xz));
        elements.push_back(wrap(sigma_yz));
        /* 6 sigma d along linear combinations of three axes */
        elements.push_back(wrap(Reflection((e_x + e_y).cross(e_z))));
        elements.push_back(wrap(Reflection((e_x - e_y).cross(e_z))));
        elements.push_back(wrap(Reflection((e_x + e_z).cross(e_y))));
        elements.push_back(wrap(Reflection((e_x - e_z).cross(e_y))));
        elements.push_back(wrap(Reflection((e_y + e_z).cross(e_x))));
        elements.push_back(wrap(Reflection((e_y - e_z).cross(e_x))));
        assert(elements.size() == 48);
        return elements;
      }

    /* Icosahedral groups */
    case(PointGroup::I):
        return I();

    case(PointGroup::Ih):
        return Ih();

    default:
      return {};
  }
}

unsigned order(const PointGroup group) {
  const std::vector<unsigned> orders {
    1, 2, 2, // C1, Ci, Cs
    2, 3, 4, 5, 6, 7, 8, // Cn
    4, 6, 8, 10, 12, 14, 16, // Cnh
    4, 6, 8, 10, 12, 14, 16, // Cnv
    4, 6, 8, // S4, S6, S8
    4, 6, 8, 10, 12, 14, 16, // Dn
    8, 12, 16, 20, 24, 28, 32, // Dnh
    8, 12, 16, 20, 24, 28, 32, // Dnd
    12, 24, 24, // T, Td, Th
    24, 48, // O, Oh
    60, 120, // I, Ih
    16, 32 // Cinfv, Dinfh (forwarded to C8v and D8h)
  };
  return orders.at(underlying(group));
}

NpGroupingsMapType npGroupings(const ElementsList& elements) {
  assert(elements.front()->matrix() == Elements::Identity().matrix());
  const unsigned E = elements.size();

  NpGroupingsMapType npGroupings;

  auto testVector = [&](const Eigen::Vector3d& v) {
    // Check if there is already a grouping for this vector
    for(const auto& iterPair : npGroupings) {
      if(Temple::any_of(
        iterPair.second,
        [&v](const ElementGrouping& grouping) -> bool {
          return grouping.probePoint.isApprox(v, 1e-8);
        }
      )) {
        return;
      }
    }

    Eigen::Matrix<double, 3, Eigen::Dynamic> mappedPoints(3, E);
    mappedPoints.col(0) = v;
    unsigned np = 1;
    std::vector<
      std::vector<unsigned>
    > groups {
      {0}
    };

    for(unsigned i = 1; i < E; ++i) {
      Eigen::Vector3d mapped = elements.at(i)->matrix() * mappedPoints.col(0);
      bool found = false;
      for(unsigned j = 0; j < np; ++j) {
        if(mappedPoints.col(j).isApprox(mapped, 1e-8)) {
          found = true;
          groups.at(j).push_back(i);
          break;
        }
      }

      if(!found) {
        mappedPoints.col(np) = mapped;
        ++np;
        groups.push_back(std::vector<unsigned> {i});
      }
    }

    assert(std::is_sorted(std::begin(groups), std::end(groups)));

    ElementGrouping grouping;
    grouping.probePoint = mappedPoints.col(0);
    grouping.groups = std::move(groups);
    auto findIter = npGroupings.find(np);
    if(findIter == std::end(npGroupings)) {
      npGroupings.emplace(np, std::vector<ElementGrouping> {std::move(grouping)});
    } else {
      auto& groupingsList = findIter->second;
      if(
        !Temple::any_of(
          groupingsList,
          [&grouping](const ElementGrouping& group) -> bool {
            return grouping.groups == group.groups;
          }
        )
      ) {
        groupingsList.push_back(std::move(grouping));
      }
    }
  };

  testVector(Eigen::Vector3d::UnitZ());
  testVector(Eigen::Vector3d::UnitZ() + 0.1 * Eigen::Vector3d::UnitX());
  testVector(Eigen::Vector3d::UnitX());
  testVector(Eigen::Vector3d::UnitY());
  testVector(Eigen::Vector3d::Zero());

  for(const auto& elementPtr : elements) {
    if(auto axisOption = elementPtr->vector()) {
      testVector(*axisOption);
    }
  }

  return npGroupings;
}

} // namespace Elements
} // namespace Shapes
} // namespace Molassembler
} // namespace Scine


#ifndef nomad__src__autodiff__first_order_hpp
#define nomad__src__autodiff__first_order_hpp

#include <iomanip>
#include <string>
#include <type_traits>

#include <Eigen/Core>

#include <src/var/var.hpp>
#include <src/autodiff/exceptions.hpp>

namespace nomad {

  template<class T_var>
  void first_order_forward_adj(const T_var& v) {
    for (nomad_idx_t i = 1; i <= v.node(); ++i)
      var_nodes_[i].first_order_forward_adj();
  }

  template<class T_var>
  void first_order_reverse_adj(const T_var& v) {
    var_nodes_[v.node()].first_grad() = 1.0;
    for (nomad_idx_t i = v.node(); i > 0; --i)
      var_nodes_[i].first_order_reverse_adj();
  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 1, void >::type
  gradient(const F& functional,
           const Eigen::VectorXd& x,
           double& f,
           Eigen::VectorXd& g) {

    reset();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();
      first_order_reverse_adj(f_var);

      for (eigen_idx_t i = 0; i < x.size(); ++i)
      g(i) = var_nodes_[i + 1].first_grad();

      reset();

    } catch (nomad_error& e) {
      reset();
      throw e;
    }

  }

  template <typename F>
  void gradient(const F& functional,
                const Eigen::VectorXd& x,
                Eigen::VectorXd& g) {
    double f;
    gradient(functional, x, f, g);
  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 0, void >::type
  finite_diff_gradient(const F& functional,
                       const Eigen::VectorXd& x,
                       Eigen::VectorXd& g,
                       const double epsilon = 1e-6) {

    Eigen::VectorXd x_dynam(x);

    for (eigen_idx_t i = 0; i < x.size(); ++i) {

      double delta_f = 0;

      x_dynam(i) += epsilon;
      auto v1 = functional(x_dynam);
      delta_f += v1.first_val();
      reset();

      x_dynam(i) -= 2.0 * epsilon;
      auto v2 = functional(x_dynam);
      delta_f -= v2.first_val();
      reset();

      x_dynam(i) += epsilon;

      delta_f /= 2.0 * epsilon;

      g(i) = delta_f;

    }

  }

  template <typename F>
  void test_gradient(const F& functional,
                     const Eigen::VectorXd& x,
                     const double epsilon = 1e-6) {

    Eigen::VectorXd auto_grad(x.size());
    try {
      gradient(functional, x, auto_grad);
    } catch (std::runtime_error& e) {
      std::cout << "Cannot compute Gradient Test" << std::endl;
      throw e;
    }

    Eigen::VectorXd diff_grad(x.size());
    try {
      finite_diff_gradient(functional, x, diff_grad, epsilon);
    } catch (std::runtime_error& e) {
      std::cout << "Cannot compute Gradient Test" << std::endl;
      throw e;
    }

    std::cout.precision(6);
    int width = 12;
    int n_column = 4;

    std::cout << "Gradient Test:" << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Component"
              << std::setw(width) << std::left << "Automatic"
              << std::setw(width) << std::left << "Finite"
              << std::setw(width) << std::left << "Delta / "
              << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "(i)"
              << std::setw(width) << std::left << "Derivative"
              << std::setw(width) << std::left << "Difference"
              << std::setw(width) << std::left << "Stepsize^{2}"
              << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;

    Eigen::VectorXd x_dynam(x);

    for (eigen_idx_t i = 0; i < x.size(); ++i) {
      std::cout << "    "
                << std::setw(width) << std::left << i
                << std::setw(width) << std::left << auto_grad(i)
                << std::setw(width) << std::left << diff_grad(i)
                << std::setw(width) << std::left << (auto_grad(i) - diff_grad(i)) / (epsilon * epsilon)
                << std::endl;

    }

    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << std::endl;

  }

  template <typename F>
  typename std::enable_if<is_var<typename F::var_type>::value && F::var_type::order() >= 1, void >::type
  gradient_dot_vector(const F& functional,
                      const Eigen::VectorXd& x,
                      const Eigen::VectorXd& v,
                      double& f,
                      double& grad_dot_v) {

    reset();

    try {

      auto f_var = functional(x);

      f = f_var.first_val();

      for (eigen_idx_t i = 0; i < x.size(); ++i)
      var_nodes_[i + 1].first_grad() = v(i);

      first_order_forward_adj(f_var);

      grad_dot_v = f_var.first_grad();

      reset();

    } catch (nomad_error& e) {
      reset();
      throw e;
    }

  }

  template <typename F>
  void gradient_dot_vector(const F& functional,
                           const Eigen::VectorXd& x,
                           const Eigen::VectorXd& v,
                           double& grad_dot_v) {
    double f;
    gradient_dot_vector(functional, x, v, f, grad_dot_v);
  }

  template <typename F>
  void test_gradient_dot_vector(const F& functional,
                                const Eigen::VectorXd& x,
                                const Eigen::VectorXd& v) {

    Eigen::VectorXd g_auto(x.size());
    try {
      gradient(functional, x, g_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Gradient Dot Vector Test" << std::endl;
      std::cout << e.what() << std::endl;
    }

    double g_dot_v = g_auto.dot(v);

    double g_dot_v_auto;
    try {
      gradient_dot_vector(functional, x, v, g_dot_v_auto);
    } catch (nomad_error& e) {
      std::cout << "Cannot compute Gradient Dot Vector Test" << std::endl;
      std::cout << e.what() << std::endl;
    }

    std::cout.precision(6);
    int width = 12;
    int n_column = 2;

    std::cout << "Gradient Dot Vector Test:" << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Automatic"
              << std::setw(width) << std::left << "Exact"
              << std::endl;
    std::cout << "    "
              << std::setw(width) << std::left << "Derivative"
              << std::setw(width) << std::left << ""
              << std::endl;
    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;

    std::cout << "    "
              << std::setw(width) << std::left << g_dot_v_auto
              << std::setw(width) << std::left << g_dot_v
              << std::endl;

    std::cout << "    " << std::setw(n_column * width) << std::setfill('-')
              << "" << std::setfill(' ') << std::endl;
    std::cout << std::endl;

  }

}

#endif


// Copyright (c) 2008-2017 Emil Dotchevski and Reverge Studios, Inc.

// 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_QVM_E5F500D4618DBE7B51573B33821F
#define BOOST_QVM_E5F500D4618DBE7B51573B33821F

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 3 &&
                                  mat_traits<A>::cols == 3 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator*(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a00 * b0 + a01 * b1 + a02 * b2;
  vec_traits<R>::template write_element<1>(r) = a10 * b0 + a11 * b1 + a12 * b2;
  vec_traits<R>::template write_element<2>(r) = a20 * b0 + a21 * b1 + a22 * b2;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_mv_defined;

template <> struct mul_mv_defined<3, 3> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<B>::rows == 3 &&
                                  mat_traits<B>::cols == 3 &&
                                  vec_traits<A>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    operator*(A const &a, B const &b) {
  typedef typename vec_traits<A>::scalar_type Ta;
  typedef typename mat_traits<B>::scalar_type Tb;
  Ta const a0 = vec_traits<A>::template read_element<0>(a);
  Ta const a1 = vec_traits<A>::template read_element<1>(a);
  Ta const a2 = vec_traits<A>::template read_element<2>(a);
  Tb const b00 = mat_traits<B>::template read_element<0, 0>(b);
  Tb const b01 = mat_traits<B>::template read_element<0, 1>(b);
  Tb const b02 = mat_traits<B>::template read_element<0, 2>(b);
  Tb const b10 = mat_traits<B>::template read_element<1, 0>(b);
  Tb const b11 = mat_traits<B>::template read_element<1, 1>(b);
  Tb const b12 = mat_traits<B>::template read_element<1, 2>(b);
  Tb const b20 = mat_traits<B>::template read_element<2, 0>(b);
  Tb const b21 = mat_traits<B>::template read_element<2, 1>(b);
  Tb const b22 = mat_traits<B>::template read_element<2, 2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a0 * b00 + a1 * b10 + a2 * b20;
  vec_traits<R>::template write_element<1>(r) = a0 * b01 + a1 * b11 + a2 * b21;
  vec_traits<R>::template write_element<2>(r) = a0 * b02 + a1 * b12 + a2 * b22;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_vm_defined;

template <> struct mul_vm_defined<3, 3> { static bool const value = true; };
} // namespace qvm_detail

} // namespace qvm
} // namespace boost

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_BESSEL_FUNCTIONS_SCALAR_J0_HPP_INCLUDED
#define NT2_BESSEL_FUNCTIONS_SCALAR_J0_HPP_INCLUDED

#include <nt2/bessel/functions/j0.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/cos.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/digits.hpp>
#include <boost/simd/sdk/math.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    NT2_FUNCTOR_CALL(1)
    {
      return nt2::j0(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if (is_inf(a0)) return Zero<A0>();
#if defined(BOOST_SIMD_HAS__J0)
      return ::_j0(a0);
#elif defined(BOOST_SIMD_HAS_J0)
      return ::j0(a0);
#else
#error j0 not supported
#endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( j0_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      if (is_inf(a0)) return Zero<A0>();
      A0 x = nt2::abs(a0);
      // if (x < 1.0e-3f) return oneminus(Quarter<A0>()*sqr(x));
      if (x <= Two<A0>())
      {
        A0 z = sqr(x);
        return (z-single_constant<float,0x40b90fdc> ())*
               horner< NT2_HORNER_COEFF_T(stype, 5,
                      (0xb382511c,
                       0x36d660a0,
                       0xb9d01fb1,
                       0x3c5a6271,
                       0xbe3110a6
                      ) ) > (z);
      }
      A0 q = rec(x);
      const A0 p3 = nt2::sqrt(q) *
        horner< NT2_HORNER_COEFF_T(stype, 8,
               (0xbd8c100e,
                0x3e3ef887,
                0xbe5ba616,
                0x3df54214,
                0xbb69539e,
                0xbd4b8bc1,
                0xb6612dc2,
                0x3f4c422a
                ) ) > (q);
            const A0 xn =  q*
              horner< NT2_HORNER_COEFF_T(stype, 8,
               (0x4201aee0,
                0xc2113945,
                0x418c7f6a,
                0xc09f3306,
                0x3f8040aa,
                0xbe46a57f,
                0x3d84ed6e,
                0xbdffff97
                ) ) > (sqr(q))-Pio_4<A0>();
      return p3*nt2::cos(xn+x);
    }
  };
} }

#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the MPI-IS nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/special/MobiusStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>
#include <cmath>
#include <boost/math/constants/constants.hpp>

using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

MobiusStateSpace::MobiusStateSpace(double intervalMax, double radius) : radius_(radius)
{
    setName("Mobius" + getName());
    type_ = STATE_SPACE_MOBIUS;

    StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
    StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
    R1->as<RealVectorStateSpace>()->setBounds(-intervalMax, +intervalMax);

    addSubspace(SO2, 1.0);
    addSubspace(R1, 1.0);
    lock();
}

double MobiusStateSpace::distance(const State *state1, const State *state2) const
{
    double theta1 = state1->as<MobiusStateSpace::StateType>()->getU();
    double theta2 = state2->as<MobiusStateSpace::StateType>()->getU();

    double diff = theta2 - theta1;

    if (std::abs(diff) <= pi)
    {
        return CompoundStateSpace::distance(state1, state2);
    }
    else
    {
        // requires interpolation over the gluing strip
        const auto *cstate1 = static_cast<const CompoundState *>(state1);
        const auto *cstate2 = static_cast<const CompoundState *>(state2);

        // distance on S1 as usual
        double dist = 0.0;
        dist += weights_[0] * components_[0]->distance(cstate1->components[0], cstate2->components[0]);

        double r1 = state1->as<MobiusStateSpace::StateType>()->getV();
        double r2 = state2->as<MobiusStateSpace::StateType>()->getV();

        r2 = -r2;

        dist += std::sqrt((r2 - r1) * (r2 - r1));
        return dist;
    }
}

void MobiusStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
{
    double theta1 = from->as<MobiusStateSpace::StateType>()->getU();
    double theta2 = to->as<MobiusStateSpace::StateType>()->getU();

    double diff = theta2 - theta1;

    if (std::abs(diff) <= pi)
    {
        // interpolate as it would be a cylinder
        CompoundStateSpace::interpolate(from, to, t, state);
    }
    else
    {
        // requires interpolation over the gluing strip
        const auto *cfrom = static_cast<const CompoundState *>(from);
        const auto *cto = static_cast<const CompoundState *>(to);
        auto *cstate = static_cast<CompoundState *>(state);

        // interpolate S1 as usual
        components_[0]->interpolate(cfrom->components[0], cto->components[0], t, cstate->components[0]);

        double r1 = from->as<MobiusStateSpace::StateType>()->getV();
        double r2 = to->as<MobiusStateSpace::StateType>()->getV();

        // Need to mirror point for interpolation
        r2 = -r2;

        double r = r1 + (r2 - r1) * t;

        // check again if we need to invert (only if we already crossed gluing
        // line)
        double thetaNew = state->as<MobiusStateSpace::StateType>()->getU();
        double diff2 = theta2 - thetaNew;

        if (std::abs(diff2) <= pi)
        {
            r = -r;
        }

        state->as<MobiusStateSpace::StateType>()->setV(r);
    }
}

State *MobiusStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

Eigen::Vector3f MobiusStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f vec;

    const auto *s = state->as<MobiusStateSpace::StateType>();
    float u = s->getU();
    float v = s->getV();

    double R = radius_ + v * std::cos(0.5 * u);
    vec[0] = R * std::cos(u);
    vec[1] = R * std::sin(u);
    vec[2] = v * std::sin(0.5 * u);
    return vec;
}