/**
 *                       _________ _______   __
 *                      /  _/ ___// ____/ | / /
 *                      / / \__ \/ __/ /  |/ /
 *                    _/ / ___/ / /___/ /|  /
 *                   /___//____/_____/_/ |_/
 *
 *  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


//  (C) Copyright Gennadiy Rozental 2001.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//!@file
//!@brief Random generator
// ***************************************************************************

#ifndef BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER
#define BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER

// Boost.Test
#include <boost/test/data/config.hpp>

#if !defined(BOOST_TEST_NO_RANDOM_DATASET_AVAILABLE) ||                        \
    defined(BOOST_TEST_DOXYGEN_DOC__)

#include <boost/test/data/monomorphic/generate.hpp>
#include <boost/test/data/monomorphic/generators/keywords.hpp>

// STL
#include <random>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace data {

namespace {
nfp::keyword<struct seed_t> seed;
nfp::keyword<struct distribution_t> distribution;
nfp::keyword<struct engine_t> engine;
} // namespace

namespace monomorphic {

namespace ds_detail {
template <typename SampleType> struct default_distribution {
  typedef
      typename mpl::if_<std::is_integral<SampleType>,
                        std::uniform_int_distribution<SampleType>,
                        std::uniform_real_distribution<SampleType>>::type type;
};

} // namespace ds_detail

// ************************************************************************** //
// **************                   random_t                   ************** //
// ************************************************************************** //

/*!@brief Generator for the random sequences
 *
 * This class implements the generator concept (see @ref
 * boost::unit_test::data::monomorphic::generated_by) for implementing a random
 * number generator.
 */
template <typename SampleType = double,
          typename DistributionType =
              typename ds_detail::default_distribution<SampleType>::type,
          typename EngineType = std::default_random_engine>
class random_t {
public:
  typedef SampleType sample;
  typedef DistributionType distr_type;
  typedef EngineType engine_type;

  random_t() : m_distribution(), m_engine(std::random_device()()) {}
  explicit random_t(distr_type &&d)
      : m_distribution(std::forward<distr_type>(d)),
        m_engine(std::random_device()()) {}
  random_t(engine_type &&e, distr_type &&d)
      : m_distribution(std::forward<distr_type>(d)),
        m_engine(std::forward<engine_type>(e)) {}

  // Generator interface
  data::size_t capacity() const { return BOOST_TEST_DS_INFINITE_SIZE; }
  SampleType next() { return m_distribution(m_engine); }
  void reset() {}

  //! Sets the seed of the pseudo-random number engine.
  template <typename SeedType> void seed(SeedType &&seed) {
    m_engine.seed(std::forward<SeedType>(seed));
  }

private:
  // Data members
  DistributionType m_distribution;
  EngineType m_engine;
};

//____________________________________________________________________________//

} // namespace monomorphic

//! @brief Returns an infinite sequence of random numbers.
//!
//! The following overloads are available:
//! @code
//! auto d = random();
//! auto d = random(begin, end);
//! auto d = random(params);
//! @endcode
//!
//!
//! - The first overload uses the default distribution, which is uniform and
//! which elements
//!   are @c double type (the values are in [0, 1) ).
//! - The second overload generates numbers in the given interval. The
//! distribution is uniform (in [begin, end)
//!   for real numbers, and in [begin, end] for integers). The type of the
//!   distribution is deduced from the type of the @c begin and @c end
//!   parameters.
//! - The third overload generates numbers using the named parameter inside @c
//! params , which are:
//!   - @c distribution: the distribution used. In this overload, since the type
//!   of the samples cannot be deduced,
//!     the samples are of type @c double and the distribution is uniform real
//!     in [0, 1).
//!   - @c seed: the seed for generating the values
//!   - @c engine: the random number generator engine
//!
//! The function returns an object that implements the dataset API.
//! @note This function is available only for C++11 capable compilers.
inline monomorphic::generated_by<monomorphic::random_t<>> random() {
  return monomorphic::generated_by<monomorphic::random_t<>>(
      monomorphic::random_t<>());
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::random()
template <typename SampleType>
inline monomorphic::generated_by<monomorphic::random_t<SampleType>>
random(SampleType begin, SampleType end) {
  typedef monomorphic::random_t<SampleType> Gen;
  typedef typename Gen::distr_type distr_type;
  return monomorphic::generated_by<Gen>(Gen(distr_type(begin, end)));
}

//____________________________________________________________________________//

namespace ds_detail {
template <typename Params> struct random_gen_type {
  typedef typename nfp::param_type<Params, decltype(distribution),
                                   std::uniform_real_distribution<>>::type
      distr_type;
  typedef
      typename nfp::param_type<Params, decltype(engine),
                               std::default_random_engine>::type engine_type;
  typedef typename distr_type::result_type sample_type;

  typedef monomorphic::random_t<sample_type, distr_type, engine_type> type;
};

} // namespace ds_detail

/// @overload boost::unit_test::data::random()
template <typename Params>
inline monomorphic::generated_by<
    typename ds_detail::random_gen_type<Params>::type>
random(Params const &params) {
  typedef typename ds_detail::random_gen_type<Params>::type Gen;
  typedef typename Gen::distr_type distr_type;
  typedef typename Gen::engine_type engine_type;

  std::random_device rd;
  engine_type E;
  //    engine_type E( rd );
  if (params.has(engine))
    E = params[engine];

  distr_type D;
  if (params.has(distribution))
    D = params[distribution];

  Gen G(std::move(E), std::move(D));

  if (params.has(seed))
    G.seed(params[seed]);

  return monomorphic::generated_by<Gen>(std::move(G));
}

} // namespace data
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_NO_RANDOM_DATASET_AVAILABLE

#endif // BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_RANDOM_HPP_101512GER


/* boost random/detail/const_mod.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: const_mod.hpp 41369 2007-11-25 18:07:19Z bemandawes $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CONST_MOD_HPP
#define BOOST_RANDOM_CONST_MOD_HPP

#include <cassert>
#include <boost/static_assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <boost/detail/workaround.hpp>

namespace boost {
namespace random {

/*
 * Some random number generators require modular arithmetic.  Put
 * everything we need here.
 * IntType must be an integral type.
 */

namespace detail {

  template<bool is_signed>
  struct do_add
  { };

  template<>
  struct do_add<true>
  {
    template<class IntType>
    static IntType add(IntType m, IntType x, IntType c)
    {
      if (x < m - c)
        return x + c;
      else
        return x - (m-c);
    }
  };

  template<>
  struct do_add<false>
  {
    template<class IntType>
    static IntType add(IntType, IntType, IntType)
    {
      // difficult
      assert(!"const_mod::add with c too large");
      return 0;
    }
  };
} // namespace detail

#if !(defined(__BORLANDC__) && (__BORLANDC__ == 0x560))

template<class IntType, IntType m>
class const_mod
{
public:
  static IntType add(IntType x, IntType c)
  {
    if(c == 0)
      return x;
    else if(c <= traits::const_max - m)    // i.e. m+c < max
      return add_small(x, c);
    else
      return detail::do_add<traits::is_signed>::add(m, x, c);
  }

  static IntType mult(IntType a, IntType x)
  {
    if(a == 1)
      return x;
    else if(m <= traits::const_max/a)      // i.e. a*m <= max
      return mult_small(a, x);
    else if(traits::is_signed && (m%a < m/a))
      return mult_schrage(a, x);
    else {
      // difficult
      assert(!"const_mod::mult with a too large");
      return 0;
    }
  }

  static IntType mult_add(IntType a, IntType x, IntType c)
  {
    if(m <= (traits::const_max-c)/a)   // i.e. a*m+c <= max
      return (a*x+c) % m;
    else
      return add(mult(a, x), c);
  }

  static IntType invert(IntType x)
  { return x == 0 ? 0 : invert_euclidian(x); }

private:
  typedef integer_traits<IntType> traits;

  const_mod();      // don't instantiate

  static IntType add_small(IntType x, IntType c)
  {
    x += c;
    if(x >= m)
      x -= m;
    return x;
  }

  static IntType mult_small(IntType a, IntType x)
  {
    return a*x % m;
  }

  static IntType mult_schrage(IntType a, IntType value)
  {
    const IntType q = m / a;
    const IntType r = m % a;

    assert(r < q);        // check that overflow cannot happen

    value = a*(value%q) - r*(value/q);
    // An optimizer bug in the SGI MIPSpro 7.3.1.x compiler requires this
    // convoluted formulation of the loop (Synge Todo)
    for(;;) {
      if (value > 0)
        break;
      value += m;
    }
    return value;
  }

  // invert c in the finite field (mod m) (m must be prime)
  static IntType invert_euclidian(IntType c)
  {
    // we are interested in the gcd factor for c, because this is our inverse
    BOOST_STATIC_ASSERT(m > 0);
#if BOOST_WORKAROUND(__MWERKS__, BOOST_TESTED_AT(0x3003))
    assert(boost::integer_traits<IntType>::is_signed);
#elif !defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS)
    BOOST_STATIC_ASSERT(boost::integer_traits<IntType>::is_signed);
#endif
    assert(c > 0);
    IntType l1 = 0;
    IntType l2 = 1;
    IntType n = c;
    IntType p = m;
    for(;;) {
      IntType q = p / n;
      l1 -= q * l2;           // this requires a signed IntType!
      p -= q * n;
      if(p == 0)
        return (l2 < 1 ? l2 + m : l2);
      IntType q2 = n / p;
      l2 -= q2 * l1;
      n -= q2 * p;
      if(n == 0)
        return (l1 < 1 ? l1 + m : l1);
    }
  }
};

// The modulus is exactly the word size: rely on machine overflow handling.
// Due to a GCC bug, we cannot partially specialize in the presence of
// template value parameters.
template<>
class const_mod<unsigned int, 0>
{
  typedef unsigned int IntType;
public:
  static IntType add(IntType x, IntType c) { return x+c; }
  static IntType mult(IntType a, IntType x) { return a*x; }
  static IntType mult_add(IntType a, IntType x, IntType c) { return a*x+c; }

  // m is not prime, thus invert is not useful
private:                      // don't instantiate
  const_mod();
};

template<>
class const_mod<unsigned long, 0>
{
  typedef unsigned long IntType;
public:
  static IntType add(IntType x, IntType c) { return x+c; }
  static IntType mult(IntType a, IntType x) { return a*x; }
  static IntType mult_add(IntType a, IntType x, IntType c) { return a*x+c; }

  // m is not prime, thus invert is not useful
private:                      // don't instantiate
  const_mod();
};

// the modulus is some power of 2: rely partly on machine overflow handling
// we only specialize for rand48 at the moment
#ifndef BOOST_NO_INT64_T
template<>
class const_mod<uint64_t, uint64_t(1) << 48>
{
  typedef uint64_t IntType;
public:
  static IntType add(IntType x, IntType c) { return c == 0 ? x : mod(x+c); }
  static IntType mult(IntType a, IntType x) { return mod(a*x); }
  static IntType mult_add(IntType a, IntType x, IntType c)
    { return mod(a*x+c); }
  static IntType mod(IntType x) { return x &= ((uint64_t(1) << 48)-1); }

  // m is not prime, thus invert is not useful
private:                      // don't instantiate
  const_mod();
};
#endif /* !BOOST_NO_INT64_T */

#else

//
// for some reason Borland C++ Builder 6 has problems with
// the full specialisations of const_mod, define a generic version
// instead, the compiler will optimise away the const-if statements:
//

template<class IntType, IntType m>
class const_mod
{
public:
  static IntType add(IntType x, IntType c)
  {
    if(0 == m)
    {
       return x+c;
    }
    else
    {
       if(c == 0)
         return x;
       else if(c <= traits::const_max - m)    // i.e. m+c < max
         return add_small(x, c);
       else
         return detail::do_add<traits::is_signed>::add(m, x, c);
    }
  }

  static IntType mult(IntType a, IntType x)
  {
    if(x == 0)
    {
       return a*x;
    }
    else
    {
       if(a == 1)
         return x;
       else if(m <= traits::const_max/a)      // i.e. a*m <= max
         return mult_small(a, x);
       else if(traits::is_signed && (m%a < m/a))
         return mult_schrage(a, x);
       else {
         // difficult
         assert(!"const_mod::mult with a too large");
         return 0;
       }
    }
  }

  static IntType mult_add(IntType a, IntType x, IntType c)
  {
    if(m == 0)
    {
       return a*x+c;
    }
    else
    {
       if(m <= (traits::const_max-c)/a)   // i.e. a*m+c <= max
         return (a*x+c) % m;
       else
         return add(mult(a, x), c);
    }
  }

  static IntType invert(IntType x)
  { return x == 0 ? 0 : invert_euclidian(x); }

private:
  typedef integer_traits<IntType> traits;

  const_mod();      // don't instantiate

  static IntType add_small(IntType x, IntType c)
  {
    x += c;
    if(x >= m)
      x -= m;
    return x;
  }

  static IntType mult_small(IntType a, IntType x)
  {
    return a*x % m;
  }

  static IntType mult_schrage(IntType a, IntType value)
  {
    const IntType q = m / a;
    const IntType r = m % a;

    assert(r < q);        // check that overflow cannot happen

    value = a*(value%q) - r*(value/q);
    while(value <= 0)
      value += m;
    return value;
  }

  // invert c in the finite field (mod m) (m must be prime)
  static IntType invert_euclidian(IntType c)
  {
    // we are interested in the gcd factor for c, because this is our inverse
    BOOST_STATIC_ASSERT(m > 0);
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(boost::integer_traits<IntType>::is_signed);
#endif
    assert(c > 0);
    IntType l1 = 0;
    IntType l2 = 1;
    IntType n = c;
    IntType p = m;
    for(;;) {
      IntType q = p / n;
      l1 -= q * l2;           // this requires a signed IntType!
      p -= q * n;
      if(p == 0)
        return (l2 < 1 ? l2 + m : l2);
      IntType q2 = n / p;
      l2 -= q2 * l1;
      n -= q2 * p;
      if(n == 0)
        return (l1 < 1 ? l1 + m : l1);
    }
  }
};

#endif

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_CONST_MOD_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_REMROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_REMROUND_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/remround.hpp>
#include <boost/simd/include/functions/simd/idivround.hpp>
#include <boost/simd/include/functions/simd/divround.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/dispatch/attributes.hpp>

/////////////////////////////////////////////////////////////////////////////
// The remround() function computes the remround of dividing x by y.  The
// return value is x-n*y, where n is the value x / y, rounded to the nearest
// integer.
// As the result can be negative the functor is not defined for unsigned
// entries
/////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( remround_, tag::cpu_
                                    , (A0)
                                    , (generic_< signed_<A0> >)
                                      (generic_< signed_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return selsub(is_nez(a1),a0,
                    boost::simd::multiplies(idivround(a0, a1), a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( remround_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0-divround(a0, a1)*a1;
    }
  };
} } }

#endif


/////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::normal::random.hpp                     //
//                                                                             //
//  (C) Copyright 2009 Erwann Rogard                                           //
//  Use, modification and distribution are subject to the                      //
//  Boost Software License, Version 1.0. (See accompanying file                //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)           //
/////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_RANDOM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_NORMAL_RANDOM_HPP_ER_2009
#include <boost/math/distributions/normal.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace meta{

    template<typename T,typename P>
    struct random_distribution<
        boost::math::normal_distribution<T,P>
    >{
        typedef boost::math::normal_distribution<T,P> dist_;
        typedef boost::normal_distribution<T> type;

        static type call(const dist_& d){
            return type(d.location(),d.scale());
        }
    };

}// meta
}// distribution
}// detail
}// statistics
}// boost

#endif


// Tests for those visual utility estimators that you can do good
// unittests for.

#include <boost/scoped_ptr.hpp>
#include <gtest/gtest.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
#include "visual_utility/VisualUtilityEstimator.h"

using namespace cv;
using namespace std;
using namespace boost;

namespace visual_utility {

class CenterSurroundHistogramTest : public ::testing::Test {
 protected:
  scoped_ptr<VisualUtilityEstimator> estimator;

  virtual void SetUp() {
    vector<double> scales;
    scales.push_back(1.0);
    estimator.reset(new CenterSurroundHistogram(scales, "chisq"));
  }

};

TEST_F(CenterSurroundHistogramTest, ZeroesAndOnesSeperate) {
  Mat_<uint8_t> image = Mat_<uint8_t>::zeros(32, 32);
  image(Rect(14,14,4,4)) = 255;

  vector<Rect> rois;
  rois.push_back(Rect(14, 14, 4, 4));

  vector<VisualUtilityEstimator::ROIScore> scores;
  estimator->CalculateVisualUtility(image, rois, 0, &scores);

  ASSERT_EQ(scores.size(), 1u);
  EXPECT_EQ(scores[0].second.x, 14);
  EXPECT_EQ(scores[0].second.y, 14);
  EXPECT_EQ(scores[0].second.width, 4);
  EXPECT_EQ(scores[0].second.height, 4);
  EXPECT_FLOAT_EQ(scores[0].first, 2.0);
}

TEST_F(CenterSurroundHistogramTest, ZeroesAndOnesOverlap) {
  Mat_<uint8_t> image = Mat_<uint8_t>::zeros(32, 32);
  image(Rect(15,14,4,4)) = 255;

  vector<Rect> rois;
  rois.push_back(Rect(14, 14, 4, 4));

  vector<VisualUtilityEstimator::ROIScore> scores;
  estimator->CalculateVisualUtility(image, rois, 0, &scores);

  ASSERT_EQ(scores.size(), 1u);
  EXPECT_EQ(scores[0].second.x, 14);
  EXPECT_EQ(scores[0].second.y, 14);
  EXPECT_EQ(scores[0].second.width, 4);
  EXPECT_EQ(scores[0].second.height, 4);
  EXPECT_FLOAT_EQ(scores[0].first, 0.6065163);
}

class LaplacianEntropyTest : public ::testing::Test {
 protected:
  scoped_ptr<VisualUtilityEstimator> estimator;

  virtual void SetUp() {
    estimator.reset(new RelativeEntropyVUWrapper(new LaplacianVU(5)));
  }

};

TEST_F(LaplacianEntropyTest, NanBox) {
  Mat image = imread("test/img_0003.png");

  vector<Rect> rois;
  rois.push_back(Rect(110, 200, 64, 128));

  vector<VisualUtilityEstimator::ROIScore> scores;
  estimator->CalculateVisualUtility(image, rois, 0, &scores);

  ROS_INFO_STREAM("Score is " << scores[0].first);
}

} // namespace

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  ros::Time::init();
  return RUN_ALL_TESTS();
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/enum.hpp>

#include <scitbx/matrix/svd.h>

namespace scitbx { namespace matrix { namespace boost_python {

  template <typename FloatType>
  struct bidiagonal_matrix_svd_decomposition_wrapper
  {
    typedef svd::bidiagonal_decomposition<FloatType> wt;
    typedef typename wt::scalar_t scalar_t;

    static void wrap(char const *name) {
      using namespace boost::python;
      enum_<svd::bidiagonal_kind>("bidiagonal_matrix_kind")
        .value("upper_diagonal", svd::upper_bidiagonal_kind)
        .value("lower_diagonal", svd::lower_bidiagonal_kind)
        ;
      class_<wt>(name, no_init)
      .def(init<af::ref<scalar_t> const&, af::ref<scalar_t> const&, int,
                af::ref<scalar_t, af::mat_grid> const &, bool,
                af::ref<scalar_t, af::mat_grid> const &, bool,
                optional<scalar_t, int> >(
                  (arg("diagonal"), arg("off_diagonal"), arg("kind"),
                   arg("u"), arg("accumulate_u"),
                   arg("v"), arg("accumulate_v"),
                   arg("epsilon"),
                   arg("max_iteration_multiplier"))))
      .def("compute", &wt::compute)
      .def("sort", &wt::sort)
      .def("numerical_rank", &wt::numerical_rank)
      .def_readonly("has_converged", &wt::has_converged)
      ;
    }
  };

  template <typename FloatType>
  struct matrix_svd_decomposition_wrapper {
    typedef svd::decompose<FloatType> wt;

    static void wrap(char const *name) {
      using namespace boost::python;

      class_<wt>(name, no_init)
        .def(init<af::ref<FloatType, af::mat_grid> const &,
          optional<FloatType, bool, bool> >(
          (arg("matrix"),
            arg("crossover")=5./3,
            arg("accumulate_u") = false,
            arg("accumulate_v") = false)))
        .add_property("u", &wt::getU)
        .add_property("v", &wt::getV)
        .add_property("sigma", &wt::getSigma)
        .def("numerical_rank", &wt::numerical_rank)
        .def("reconstruct", &wt::reconstruct)
        ;
    }
  };

  void wrap_svd() {
    using namespace matrix::boost_python;
    bidiagonal_matrix_svd_decomposition_wrapper<double>::wrap(
      "svd_decomposition_of_bidiagonal_matrix");
    matrix_svd_decomposition_wrapper<double>::wrap(
      "svd_decompose");
    using namespace boost::python;
    def("reconstruct_svd", matrix::svd::reconstruct<double>);
  }

}}}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file find_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE find_test
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/core/detail/print_utility.h"
#include "mcrl2/data/consistency.h"
#include "mcrl2/pbes/find.h"
#include "mcrl2/pbes/detail/parse.h"
#include "mcrl2/pbes/print.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

inline
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

inline
data::variable pos(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_pos::pos());
}

inline
data::variable bool_(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::bool_());
}

BOOST_AUTO_TEST_CASE(test_find)
{
  const std::string VARSPEC =
    "datavar         \n"
    "  m: Nat;       \n"
    "  n: Nat;       \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

  pbes_expression x = parse_pbes_expression("X(true, 2) && Y(n+1) && Y(m)", VARSPEC);

  //--- find_all_variables ---//
  data::variable m = nat("m");
  data::variable n = nat("n");
  std::set<data::variable> v = pbes_system::find_all_variables(x);
  std::set<data::variable> v_expected = { m, n };
  BOOST_CHECK(v == v_expected);

  //--- find_sort_expressions ---//
  std::set<data::sort_expression> s = pbes_system::find_sort_expressions(x);
  std::cout << "s = " << core::detail::print_set(s) << std::endl;
  std::set<data::sort_expression> s_expected = { data::sort_nat::nat(), data::sort_pos::pos() };
  BOOST_CHECK(std::includes(s.begin(), s.end(), s_expected.begin(), s_expected.end()));
}

BOOST_AUTO_TEST_CASE(test_free_variables)
{
  const std::string VARSPEC =
    "datavar         \n"
    "  n: Nat;       \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

  pbes_expression x = parse_pbes_expression("forall m:Nat.(X(true, 2) && Y(n+1) && Y(m))", VARSPEC);
  data::variable m = nat("m");
  data::variable n = nat("n");

  std::set<data::variable> v = pbes_system::find_free_variables(x);
  std::set<data::variable> v_expected = { n };
  BOOST_CHECK(v == v_expected);
}

BOOST_AUTO_TEST_CASE(test_find_free_variables)
{
  std::string test1 =
    "pbes                                                                   \n"
    "                                                                       \n"
    "nu X(b:Bool, n:Nat) = (val(b) => X(!b, n)) && (val(!b) => X(!b, n+1)); \n"
    "mu Y(c:Nat, d:Bool) = forall m:Nat. Y(c, true) || X(d, m);             \n"
    "                                                                       \n"
    "init X(true, 0);                                                       \n"
    ;

  pbes p = txt2pbes(test1);

  std::set<data::variable> v = find_free_variables(p);
  BOOST_CHECK(v.empty());

  v = find_free_variables(p.equations()[0]);
  BOOST_CHECK(v.empty());

  v = find_free_variables(p.equations()[1]);
  BOOST_CHECK(v.empty());

  v = find_free_variables(p.equations()[0].formula());
  BOOST_CHECK(v.size() == 2);

  v = find_free_variables(p.equations()[1].formula());
  BOOST_CHECK(v.size() == 2);
}


#include "uniform_bspline.hpp"

#include <fstream>
#include <Eigen/Core>

#include "gtest/gtest.h"
#include "test/test_utility.hpp"

namespace {
template <typename T>
using EigenAlignedVec = std::vector<T, Eigen::aligned_allocator<T>>;

auto testDataFolder() {
    return uniform_bspline::test::projectRootDir / "test" / "test_data";
}

std::string getSplineTestFile(int idx) {
    return (testDataFolder() / ("test_points_" + std::to_string(idx) + ".txt")).string();
}

std::string getSmoothnessTestFile(int idx) {
    return (testDataFolder() / ("test_points_smoothness_" + std::to_string(idx) + ".txt")).string();
}

template <int Derivative, typename Spline>
void readSpline(std::ifstream& file,
                Spline& spline,
                bool& hasDerivative,
                std::array<int, Spline::InputDims>& derivatives) {
    ASSERT_TRUE(file.is_open());

    // Read spline spline parameters.
    int degree = 0;
    int inputDims = 0;
    int outputDims = 0;
    int derivative = 0;
    file >> degree >> inputDims >> outputDims;

    ASSERT_EQ(Spline::Degree, degree) << "This is a compile time constant";
    ASSERT_EQ(Spline::InputDims, inputDims) << "This is a compile time constant";
    ASSERT_EQ(Spline::OutputDims, outputDims) << "This is a compile time constant";

    if (Derivative >= 0) {
        file >> derivative;
        ASSERT_EQ(Derivative, derivative) << "This is a compile time constant";
    } else {
        hasDerivative = false;
        derivatives = std::array<int, Spline::InputDims>{};
        for (int i = 0; i < Spline::InputDims; ++i) {
            file >> derivatives[i];
            ASSERT_GE(derivatives[i], 0);
            hasDerivative |= (derivatives[i] > 0);
        }
    }

    // Read control points.
    std::array<int, Spline::InputDims> controlPointDims{};
    for (auto& dims : controlPointDims) {
        file >> dims;
    }

    typename Spline::ControlPointsType points(controlPointDims);
    std::for_each(points.flat_begin(), points.flat_end(), [&file](typename Spline::OutputType& val) {
        for (int i = 0; i < val.size(); ++i) {
            file >> val[i];
        }
    });

    // Read lower and upper bounds.
    typename Spline::InputType lowerBound{};
    typename Spline::InputType upperBound{};
    for (int i = 0; i < Spline::InputDims; ++i) {
        file >> lowerBound[i];
        file >> upperBound[i];
    }

    // Construct the B Spline
    spline = Spline{lowerBound, upperBound, points};
}

template <typename Spline>
void readSpline(std::ifstream& file,
                Spline& spline,
                bool& hasDerivative,
                std::array<int, Spline::InputDims>& derivatives) {
    readSpline<-1>(file, spline, hasDerivative, derivatives);
}

template <int Derivative, typename Spline>
void readSpline(std::ifstream& file, Spline& spline) {
    std::array<int, Spline::InputDims> derivatives{};
    bool hasDerivative{};
    readSpline<Derivative>(file, spline, hasDerivative, derivatives);
}

template <typename Spline>
void testEvaluate(const std::string& filePath) {
    std::ifstream file(filePath);
    Spline spline{};
    bool hasDerivative{};
    std::array<int, Spline::InputDims> derivatives{};
    readSpline(file, spline, hasDerivative, derivatives);

    // Read test points and compare with Mathematica result.
    int numTestPoints = 0;
    file >> numTestPoints;
    ASSERT_GT(numTestPoints, 0);

    for (int testPointIdx = 0; testPointIdx < numTestPoints; ++testPointIdx) {
        typename Spline::InputType pos;
        for (int i = 0; i < pos.size(); ++i) {
            file >> pos[i];
        }

        ASSERT_TRUE(file.good());

        typename Spline::OutputType gtVal;
        for (int i = 0; i < gtVal.size(); ++i) {
            file >> gtVal[i];
        }

        typename Spline::OutputType splineVal{};

        if (!hasDerivative) {
            splineVal = spline.evaluate(pos);
        } else {
            splineVal = spline.derivative(pos, derivatives);
        }

        ASSERT_EQ(splineVal.size(), gtVal.size());

        for (int i = 0; i < splineVal.size(); ++i) {
            EXPECT_NEAR(gtVal[i], splineVal[i], 1e-10);
        }
    }
}

template <typename Spline, int Derivative>
void testSmoothnessEvaluation(const std::string& filePath) {
    std::ifstream file(filePath);
    Spline spline{};
    readSpline<Derivative>(file, spline);

    // Read test points and compare with mathematica result.
    using T = typename Spline::ValueType;
    T gtSmoothnessValue;
    file >> gtSmoothnessValue;
    T smoothnessValue = spline.template smoothness<Derivative>().sum();

    const auto bound = T(1e-5) * (T(1.0) + gtSmoothnessValue);
    EXPECT_NEAR(gtSmoothnessValue, smoothnessValue, bound) << "input file: " << filePath;
}
} // anonymous namespace

namespace ubs {
// This test needs to be in the ubs namespace because of the friend declaration.
TEST(UniformBSpline, DerivativeFactors) {
    ubs::EigenUniformBSpline<double, 4, 1, 1> spline;

    Eigen::Matrix<double, 4, 4> gtDerivativeFactors{};
    gtDerivativeFactors << 1, 0, 0, 0, 2, 2, 0, 0, 3, 6, 6, 0, 4, 12, 24, 24;

    Eigen::Matrix<double, 4, 4> derivativeFactors = spline.getDerivativeFactors();

    // The derivate factors are stored with reversed columns.
    derivativeFactors.colwise().reverseInPlace();

    for (int r = 0; r < 4; ++r) {
        for (int c = 0; c < 4; ++c) {
            EXPECT_EQ(gtDerivativeFactors(r, c), derivativeFactors(r, c));
        }
    }
}
} // namespace ubs

TEST(UniformBSpline, Evaluate) {
    testEvaluate<ubs::EigenUniformBSpline<double, 5, 1, 1>>(getSplineTestFile(1));
    testEvaluate<ubs::EigenUniformBSpline<double, 4, 1, 2>>(getSplineTestFile(2));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 3>>(getSplineTestFile(3));
    testEvaluate<ubs::EigenUniformBSpline<double, 2, 3, 2>>(getSplineTestFile(4));
    testEvaluate<ubs::EigenUniformBSpline<double, 5, 1, 1>>(getSplineTestFile(5));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 1>>(getSplineTestFile(6));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 3>>(getSplineTestFile(7));
    testEvaluate<ubs::EigenUniformBSpline<double, 5, 1, 1>>(getSplineTestFile(8));
    testEvaluate<ubs::EigenUniformBSpline<double, 4, 1, 2>>(getSplineTestFile(9));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 3>>(getSplineTestFile(10));
    testEvaluate<ubs::EigenUniformBSpline<double, 2, 3, 2>>(getSplineTestFile(11));
    testEvaluate<ubs::EigenUniformBSpline<double, 5, 1, 1>>(getSplineTestFile(12));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 1>>(getSplineTestFile(13));
    testEvaluate<ubs::EigenUniformBSpline<double, 3, 2, 3>>(getSplineTestFile(14));
}

TEST(UniformBSpline, Smoothness) {
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 0>(getSmoothnessTestFile(1));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 1>(getSmoothnessTestFile(2));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 2>, 1>(getSmoothnessTestFile(3));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 1>(getSmoothnessTestFile(4));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 2>(getSmoothnessTestFile(5));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(6));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(7));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(8));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 0>(getSmoothnessTestFile(9));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 2>, 1>(getSmoothnessTestFile(10));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 2>, 2>(getSmoothnessTestFile(11));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 3, 2>, 2>(getSmoothnessTestFile(12));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 0>(getSmoothnessTestFile(13));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 1>(getSmoothnessTestFile(14));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 2>, 1>(getSmoothnessTestFile(15));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 1>(getSmoothnessTestFile(16));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 1, 1>, 2>(getSmoothnessTestFile(17));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(18));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(19));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 1>(getSmoothnessTestFile(20));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 1>, 0>(getSmoothnessTestFile(21));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 2>, 1>(getSmoothnessTestFile(22));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 2, 2>, 2>(getSmoothnessTestFile(23));
    testSmoothnessEvaluation<ubs::EigenUniformBSpline<double, 3, 3, 2>, 2>(getSmoothnessTestFile(24));
}

TEST(UniformBSpline, EigenControlPoints) {
    using Spline = ubs::UniformBSpline<double, 5, double, double, Eigen::VectorXd>;

    Spline spline(Eigen::VectorXd::Ones(50));

    const auto splineVal = spline.evaluate(0.5);
    EXPECT_NEAR(splineVal, 1.0, 1e-10);
}

TEST(UniformBSpline, EigenControlPoints2) {
    using Spline = ubs::UniformBSpline<double, 5, Eigen::Vector2d, double, Eigen::MatrixXd>;

    Spline spline(Eigen::MatrixXd::Ones(20, 15));

    const auto splineVal = spline.evaluate(Eigen::Vector2d(0.5, 0.1));
    EXPECT_NEAR(splineVal, 1.0, 1e-10);
}

TEST(UniformBSpline, DefaultConstructor) {
    ubs::UniformBSpline11d<4> spline;
    EXPECT_NEAR(0.0, spline.evaluate(0.0), 1e-10);
    EXPECT_NEAR(0.0, spline.evaluate(0.5), 1e-10);
    EXPECT_NEAR(0.0, spline.evaluate(1.0), 1e-10);
    EXPECT_EQ(spline.getLowerBound(0), 0.0);
    EXPECT_EQ(spline.getUpperBound(0), 1.0);
}

TEST(UniformBSpline, UniformBSpline1d) {
    using Spline = ubs::UniformBSpline<double, 5, double, double, Eigen::VectorXd>;
    Spline spline(Eigen::VectorXd::Constant(50, 20.0));

    const double val = spline.evaluate(0.3);
    const double derivative = spline.derivative(0.2, 2);
    const double smoothness = spline.smoothness<2>();

    EXPECT_NEAR(val, 20.0, 1e-10);
    EXPECT_NEAR(derivative, 0.0, 1e-10);
    EXPECT_NEAR(smoothness, 0.0, 1e-6);

    spline.setBounds(2.0, 9.5);

    const double lowerBound = spline.getLowerBound();
    const double upperBound = spline.getUpperBound();
    const double scaleVal = spline.getScale(0);
    const double scaleValGt = (50.0 - 5.0) / (9.5 - 2.0);

    EXPECT_EQ(lowerBound, 2.0);
    EXPECT_EQ(upperBound, 9.5);
    EXPECT_NEAR(scaleVal, scaleValGt, 1e-10);
}

namespace {
template <typename Spline>
void checkSplineBounds(const Spline& spline, const Eigen::Vector2d& lowerBound, const Eigen::Vector2d& upperBound) {
    static_assert(Spline::InputDims == 2, "Unsupported input dimensions.");
    for (int i = 0; i < Spline::InputDims; ++i) {
        EXPECT_EQ(spline.getLowerBound(i), lowerBound[i]);
        EXPECT_EQ(spline.getUpperBound(i), upperBound[i]);

        EXPECT_EQ(spline.getLowerBound()[i], lowerBound[i]);
        EXPECT_EQ(spline.getUpperBound()[i], upperBound[i]);
    }
}
} // namespace

TEST(UniformBSpline, Bounds) {
    {
        ubs::EigenUniformBSpline<double, 4, 2, 3> spline;
        checkSplineBounds(spline, {0.0, 0.0}, {1.0, 1.0});

        spline.setBounds({-2.0, -3.0}, {7.0, 8.0});
        checkSplineBounds(spline, {-2.0, -3.0}, {7.0, 8.0});

        EXPECT_TRUE(spline.inRange({0.0, 0.0}));
        EXPECT_TRUE(spline.inRange({-2.0, -3.0}));
        EXPECT_TRUE(spline.inRange({7.0, 8.0}));

        EXPECT_FALSE(spline.inRange({-3.0, 0.0}));
        EXPECT_FALSE(spline.inRange({8.0, 0.0}));
        EXPECT_FALSE(spline.inRange({0.0, -4.0}));
        EXPECT_FALSE(spline.inRange({0.0, 10.0}));
        EXPECT_FALSE(spline.inRange({-10.0, 20.0}));
    }
    {
        ubs::EigenUniformBSpline<double, 4, 2, 3> spline({-2.0, -3.0}, {7.0, 8.0});
        checkSplineBounds(spline, {-2.0, -3.0}, {7.0, 8.0});
    }
    {
        using Spline = ubs::UniformBSpline<double, 4, Eigen::Vector2d, double, Eigen::MatrixXd>;
        Eigen::MatrixXd controlPoints = Eigen::MatrixXd::Random(30, 25);
        Spline spline({-2.0, -3.0}, {7.0, 8.0}, controlPoints);
        checkSplineBounds(spline, {-2.0, -3.0}, {7.0, 8.0});
    }
}

TEST(UniformBSpline, Extrapolation) {
    using Spline = ubs::UniformBSpline<double, 4, double, double, std::vector<double>>;
    const std::vector<double> controlPoints{1.0, 4.0, 3.0, 10.0, 15.0};

    Spline spline(controlPoints);
    EXPECT_FALSE(spline.isExtrapolating());
    spline.setExtrapolate(true);
    EXPECT_TRUE(spline.isExtrapolating());

    {
        auto res = spline.getSpanIndexAndValue(0, -1.0);
        EXPECT_EQ(res.first, 0);
        EXPECT_DOUBLE_EQ(res.second, -1.0);
    }
    {
        auto res = spline.getSpanIndexAndValue(0, 1.5);
        EXPECT_EQ(res.first, 0);
        EXPECT_DOUBLE_EQ(res.second, 1.5);
    }
}

TEST(UniformBSpline, Cast) {
    using SplineIn = ubs::UniformBSpline<double, 4, double, Eigen::Vector2d, EigenAlignedVec<Eigen::Vector2d>>;

    EigenAlignedVec<Eigen::Vector2d> controlPoints(10);
    for (auto& c : controlPoints) {
        c.setRandom();
        c = (1000.0 * c).array().floor();
    }
    SplineIn splineIn(2.0, 10.0, controlPoints);

    using SplineOut = ubs::UniformBSpline<int, 3, int, Eigen::Vector2i, EigenAlignedVec<Eigen::Vector2i>>;

    splineIn.setExtrapolate(true);
    const auto splineOut = splineIn.cast<SplineOut>();

    EXPECT_EQ(splineIn.getLowerBound(0), splineOut.getLowerBound(0));
    EXPECT_EQ(splineIn.getUpperBound(0), splineOut.getUpperBound(0));
    EXPECT_EQ(splineIn.isExtrapolating(), splineOut.isExtrapolating());

    const auto& controlPointsIn = splineIn.getControlPoints();
    const auto& controlPointsOut = splineOut.getControlPoints();

    ASSERT_EQ(controlPointsIn.size(), controlPointsOut.size());

    for (int i = 0; i < int(controlPointsIn.size()); ++i) {
        EXPECT_EQ(controlPointsIn[i].cast<int>(), controlPointsOut[i]);
    }

    // Check self cast.
    EXPECT_EQ(&splineIn.cast<SplineIn>(), &splineIn);
}

TEST(UniformBSpline, GetSpanIndices) {
    using Spline = ubs::UniformBSpline<double, 5, Eigen::Vector2d, double, Eigen::MatrixXd>;
    Spline spline(Eigen::MatrixXd::Ones(20, 15));

    const Eigen::Vector2d pos{0.3, 0.5};
    const auto indices = spline.getSpanIndices(pos);

    ASSERT_EQ(indices.size(), pos.size());
    for (int i = 0; i < int(pos.size()); ++i) {
        EXPECT_EQ(indices[i], spline.getSpanIndex(i, pos[i]));
    }
}


#ifndef BIT_MATRIX_HPP
#define BIT_MATRIX_HPP

#include <assert.h>
#include <string>
#include <stdexcept>
#include <algorithm>
#include <vector>
#include <iostream>

#include <boost/dynamic_bitset/dynamic_bitset.hpp>
#include "config/config.hpp"
#include "bitvector.hpp"
#include <utility>

namespace argumatrix {  // argumatrix
using namespace std;

class bitmatrix{
 public:
	// constructors
	bitmatrix(size_type num_size);
	bitmatrix(size_type num_rows, size_type num_columns);
	// copy constructor
	bitmatrix(const bitmatrix& _bm);
	bitmatrix(): m_nRow(0), m_nColumn(0) {}  // default constructor

	void show();
	bitvector& operator[](size_type pos);
	const bitvector& operator[](size_type pos) const { return m_bitData[pos]; }
	//const bitvector& operator[](const size_type pos);
	bitvector operator*(const bitvector& _bv);
	bitmatrix operator*(const bitmatrix& _bm);
	bitmatrix operator=(const bitmatrix& _bm);

	// set the row bitvector of bitmatrix at pos with _bv
	void setBitvector(const bitvector& _bv, size_type pos);

	// get a copy of the row bitvector of bitmatrix at pos
	bitvector getBitvector(size_type pos);

	bitmatrix transpose() const;
	size_type sizeR() const { return m_nRow; }
	size_type sizeC() const { return m_nColumn; }

	void push_back(bitvector& _bv);

	/**
	 * Get the diagonal elements of a bitmatrix
	 * @return the bitvector of the diagonal elements
	 */
	bitvector diag();

private:
	size_type m_nRow;
	size_type m_nColumn;
	std::vector< bitvector > m_bitData;
};

bitmatrix::bitmatrix(size_type num_rows, size_type num_columns )
	:m_nRow(num_rows),
	 m_nColumn(num_columns)
{
	m_bitData.resize(m_nRow);

	for(size_type i=0; i < m_nRow; i++){
		m_bitData[i].resize(m_nColumn, false);
	}
}

bitmatrix::bitmatrix(size_type num_size)
	:m_nRow(num_size),
	 m_nColumn(num_size)
{
	m_bitData.resize(m_nRow);

	for(size_type i=0; i < m_nRow; i++){
		m_bitData[i].resize(m_nColumn, false);
	}
}

bitmatrix::bitmatrix(const bitmatrix& _bm)
{
	m_nRow = _bm.m_nRow;
	m_nColumn = _bm.m_nColumn;
	m_bitData = _bm.m_bitData;
}

void bitmatrix::show(){
	for(size_type i=0; i<m_nRow; i++){
		std::cout<< m_bitData[i] << std::endl;
	}
}

bitvector& bitmatrix::operator[](size_type pos) {
	return m_bitData[pos];
}

bitvector bitmatrix::operator*(const bitvector& _bv){
	assert(_bv.size()==m_nColumn);
	bitvector bv1(_bv.size());
	for(size_type i=0; i<m_nColumn; i++){
		bv1[i] = m_bitData[i]*_bv;
	}
	return bv1;
}

bitmatrix bitmatrix::transpose() const {
	assert(m_nColumn>0);
	assert(m_nRow>0);

	bitmatrix bm(m_nColumn, m_nRow);

	for(size_type i=0; i<m_nColumn; i++){
		for(size_type j=0; j<m_nRow; j++){
			bm[i][j] = m_bitData[j][i];
		}
	}

	return bm;
}

bitmatrix bitmatrix::operator=(const bitmatrix& _bm){
	m_nRow = _bm.m_nRow;
	m_nColumn = _bm.m_nColumn;
	m_bitData = _bm.m_bitData;

	return *this;
}

bitmatrix bitmatrix::operator*(const bitmatrix& _bm_r){
	assert( m_nColumn == _bm_r.m_nRow);
	bitmatrix result_bm(m_nRow, _bm_r.m_nColumn);
	bitmatrix _bm_r_t = _bm_r.transpose();

	for(size_type i=0; i<m_nRow; i++){
		for(size_type j=0; j<_bm_r_t.m_nRow; j++){
			result_bm[i][j] = (m_bitData[i]*_bm_r_t.m_bitData[j]);
		}
	}

	return result_bm;
}

void bitmatrix::setBitvector(const bitvector& _bv, size_type pos)
{
	assert(pos >=0 && pos < m_nRow);
	assert(m_nColumn == _bv.size());

	m_bitData[pos] = _bv;
}

bitvector bitmatrix::getBitvector(size_type pos)
{
	assert(pos >=0 && pos < m_nRow);

	return m_bitData[pos];
}

void bitmatrix::push_back(bitvector& _bv)
{
	// The _bv must none empty, i.e., _bv.size()>0
	assert(!_bv.empty());

	// if the bitmatrix is empty, _bv will be the first row of it
	if (m_nRow == 0){
		m_nColumn = _bv.size();
	}

	assert( _bv.size() == m_nColumn );

	m_bitData.push_back(_bv);

	++m_nRow;
}

bitvector bitmatrix::diag()
{
	size_type _sz = min(m_nRow, m_nColumn);
	bitvector _bv( _sz );

	for (size_type i=0; i<_sz; i++)
		_bv[i] = m_bitData[i][i];

	return _bv;
}

} // namespace argumatrix
#endif


#include <stan/math/mix/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/rev/scal/fun/util.hpp>
#include <test/unit/math/mix/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaQ, FvarVar_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[1]);
}
TEST(AgradFwdGammaQ, Double_FvarVar_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[0]);
}
TEST(AgradFwdGammaQ, FvarVar_Double_1stDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.val_.grad(y, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);
}
TEST(AgradFwdGammaQ, FvarVar_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.18228334, a.d_.val());

  AVEC y = createAVEC(x.val_, z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(-0.19403456, g[0]);
  EXPECT_FLOAT_EQ(-0.096204743, g[1]);
}
TEST(AgradFwdGammaQ, Double_FvarVar_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);
  fvar<var> z(1.0, 1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val());

  AVEC y = createAVEC(z.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(0.31133062, g[0]);
}
TEST(AgradFwdGammaQ, FvarVar_Double_2ndDeriv) {
  using stan::math::fvar;
  using stan::math::var;

  fvar<var> x(0.5, 1.0);
  double z(1.0);
  fvar<var> a = stan::math::gamma_q(x, z);

  EXPECT_FLOAT_EQ(boost::math::gamma_q(0.5, 1.0), a.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.d_.val());

  AVEC y = createAVEC(x.val_);
  VEC g;
  a.d_.grad(y, g);
  EXPECT_FLOAT_EQ(0.21349931, g[0]);
}

TEST(AgradFwdGammaQ, FvarFvarVar_FvarFvarVar_1stDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[1]);
}
TEST(AgradFwdGammaQ, Double_FvarFvarVar_1stDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), g[0]);
}
TEST(AgradFwdGammaQ, FvarFvarVar_Double_1stDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0.38983709, g[0]);
}

TEST(AgradFwdGammaQ, FvarFvarVar_FvarFvarVar_2ndDeriv_x) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.21349931, g[0]);
  EXPECT_FLOAT_EQ(-0.40753537, g[1]);
}
TEST(AgradFwdGammaQ, FvarFvarVar_FvarFvarVar_2ndDeriv_y) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.40753385, g[0]);
  EXPECT_FLOAT_EQ(0.31133062, g[1]);
}
TEST(AgradFwdGammaQ, Double_FvarFvarVar_2ndDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.val_.grad(p, g);
  EXPECT_FLOAT_EQ(0.31133062, g[0]);
}
TEST(AgradFwdGammaQ, FvarFvarVar_Double_2ndDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(0, a.d_.val_.val());
  EXPECT_FLOAT_EQ(0, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.val_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.21349931, g[0]);
}

TEST(AgradFwdGammaQ, FvarFvarVar_FvarFvarVar_3rdDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_.val());
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_.val());
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_.val());
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_.val());

  AVEC p = createAVEC(x.val_.val_, y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(0.22403987, g[0]);
  EXPECT_FLOAT_EQ(0.40374705, g[1]);
}
TEST(AgradFwdGammaQ, Double_FvarFvarVar_3rdDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  double x(0.5);

  fvar<fvar<var> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;
  y.val_.d_ = 1.0;

  fvar<fvar<var> > a = gamma_q(x, y);

  AVEC p = createAVEC(y.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.57077283, g[0]);
}
TEST(AgradFwdGammaQ, FvarFvarVar_Double_3rdDeriv) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::var;

  fvar<fvar<var> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;
  x.d_.val_ = 1.0;

  double y(1.0);

  fvar<fvar<var> > a = gamma_q(x, y);

  AVEC p = createAVEC(x.val_.val_);
  VEC g;
  a.d_.d_.grad(p, g);
  EXPECT_FLOAT_EQ(-0.5462361, g[0]);
}

struct gamma_q_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return gamma_q(arg1, arg2);
  }
};

TEST(AgradFwdGammaQ, nan) {
  gamma_q_fun gamma_q_;
  test_nan_mix(gamma_q_, 3.0, 5.0, false);
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ror.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/bitwise_cast.hpp>

STF_CASE_TPL (" rorinteger", STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ror;
  STF_EXPR_IS( ror(T(), T()), T);

  T w = sizeof(T)*CHAR_BIT;

  for(T i=0;i<w;++i)
  {
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
    STF_EQUAL(ror(T(1),i), T(T(1)<<((w-i) & (w-1))));
  }

/*
  STF_ASSERT(ror(T(1),T(-1)));
  STF_ASSERT(ror(T(1),T(w+1)));
*/
}

STF_CASE_TPL (" rorreal", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ror;
  using bs::bitwise_cast;
  using iT = bd::as_integer_t<T> ;

  STF_EXPR_IS( ror(T(), iT()), T);

  iT w = sizeof(T)*CHAR_BIT;

  for(iT i=0;i<w;++i)
    STF_EQUAL( ror(bitwise_cast<T>(iT(1)),i)
                  , bitwise_cast<T>(iT(1)<<((w-i) & (w-1)))
                  );
/*
  STF_ASSERT(ror(T(1),iT(-1)));
  STF_ASSERT(ror(T(1),iT(w+1)));
*/
}


//==================================================================================================
/*!
  @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));
    }
}


//Copyright (c) 2008-2016 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)

#include <boost/qvm/mat_operations.hpp>
#include <boost/qvm/mat_traits_array.hpp>
#include <boost/qvm/mat.hpp>
#include "test_qvm_matrix.hpp"
#include "gold.hpp"

namespace
    {
    template <int Rows,int Cols>
    void
    test()
        {
        using namespace boost::qvm::sfinae;
        test_qvm::matrix<M1,Rows,Cols> const x(42,1);
        for( int i=0; i!=Rows; ++i )
            for( int j=0; j!=Cols; ++j )
                {
                    {
                    test_qvm::matrix<M1,Rows,Cols> y(x);
                    BOOST_QVM_TEST_EQ(x,y);
                    y.a[i][j]=0;
                    BOOST_QVM_TEST_NEQ(x,y);
                    }
                    {
                    test_qvm::matrix<M2,Rows,Cols> y; assign(y,x);
                    BOOST_QVM_TEST_EQ(x,y);
                    y.a[i][j]=0;
                    BOOST_QVM_TEST_NEQ(x,y);
                    }
                }
        }

    template <class T>
    struct test_scalar
    {
        T value_;
        test_scalar( T value ): value_(value) {}
    }; //No operator==

    struct
    equal_to
    {
        template <class T,class U>
        bool
        operator()( T const & a, U const & b )
        {
            return a.value_==b.value_;
        }
    };

    template <class A, class B>
    void
    test2()
        {
        typedef test_scalar<A> scalar_a;
        typedef test_scalar<B> scalar_b;
        typedef boost::qvm::mat<scalar_a, 3, 3> mat_a;
        typedef boost::qvm::mat<scalar_b, 3, 3> mat_b;
        mat_a const a = { { {42, 94, 96}, {72, 95, 81}, {12, 84, 33} } };
        mat_b const b = { { {42, 94, 96}, {72, 95, 81}, {12, 84, 33} } };
        mat_a const c = { { {21, 47, 48}, {36, 47, 65}, {79, 27, 41} } };
        mat_b const d = { { {21, 47, 48}, {36, 47, 65}, {79, 27, 41} } };
        BOOST_TEST(cmp(a,a,equal_to()));
        BOOST_TEST(cmp(a,b,equal_to()));
        BOOST_TEST(cmp(b,a,equal_to()));
        BOOST_TEST(cmp(b,b,equal_to()));
        BOOST_TEST(cmp(c,c,equal_to()));
        BOOST_TEST(cmp(c,d,equal_to()));
        BOOST_TEST(cmp(d,c,equal_to()));
        BOOST_TEST(cmp(d,d,equal_to()));
        BOOST_TEST(!cmp(a,c,equal_to()));
        BOOST_TEST(!cmp(c,a,equal_to()));
        BOOST_TEST(!cmp(a,d,equal_to()));
        BOOST_TEST(!cmp(d,a,equal_to()));
        BOOST_TEST(!cmp(b,c,equal_to()));
        BOOST_TEST(!cmp(c,b,equal_to()));
        BOOST_TEST(!cmp(b,d,equal_to()));
        BOOST_TEST(!cmp(d,b,equal_to()));
        }
    }

int
main()
    {
    test<1,2>();
    test<2,1>();
    test<2,2>();
    test<1,3>();
    test<3,1>();
    test<3,3>();
    test<1,4>();
    test<4,1>();
    test<4,4>();
    test<1,5>();
    test<5,1>();
    test<5,5>();
    test2<int, int>();
    test2<int, double>();
    test2<double, int>();
    test2<double, double>();
    return boost::report_errors();
    }


/*============================================================================

  research-computing-with-cpp-demo: CMake based demo code.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

============================================================================*/

#include <cstdlib>
#include <iostream>
#include <Eigen/Dense>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

int main(int argc, char** argv)
{
  // Check Eigen is correctly included.
  Eigen::MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << m << std::endl;

  // Check Boost is correctly included.
  std::cout << "The number 10.0 converted to string is " << boost::lexical_cast<std::string>(10.0) << std::endl;

  // Check Boost libraries are correctly included.
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()
      ("help", "produce help message")
  ;

  return EXIT_SUCCESS;
}