#include <Columns/ColumnNullable.h>
#include <Columns/ColumnString.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnsNumber.h>
#include <Columns/IColumn.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypesNumber.h>
#include <Functions/FunctionFactory.h>
#include <Functions/FunctionHelpers.h>
#include <Functions/IFunction.h>
#include <Functions/castTypeToEither.h>
#include <Interpreters/castColumn.h>
#include <boost/math/distributions/normal.hpp>
#include <Common/typeid_cast.h>

namespace DB
{

namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int BAD_ARGUMENTS;
}

class FunctionTwoSampleProportionsZTest : public IFunction
{
public:
    static constexpr auto POOLED = "pooled";
    static constexpr auto UNPOOLED = "unpooled";

    static constexpr auto name = "proportionsZTest";

    static FunctionPtr create(ContextPtr) { return std::make_shared<FunctionTwoSampleProportionsZTest>(); }

    String getName() const override { return name; }

    size_t getNumberOfArguments() const override { return 6; }
    ColumnNumbers getArgumentsThatAreAlwaysConstant() const override { return {5}; }

    bool useDefaultImplementationForNulls() const override { return false; }
    bool useDefaultImplementationForConstants() const override { return true; }
    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return false; }

    static DataTypePtr getReturnType()
    {
        auto float_data_type = std::make_shared<DataTypeNumber<Float64>>();
        DataTypes types(4, float_data_type);

        Strings names{"z_statistic", "p_value", "confidence_interval_low", "confidence_interval_high"};

        return std::make_shared<DataTypeTuple>(std::move(types), std::move(names));
    }

    DataTypePtr getReturnTypeImpl(const ColumnsWithTypeAndName & arguments) const override
    {
        for (size_t i = 0; i < 4; ++i)
        {
            if (!isUnsignedInteger(arguments[i].type))
            {
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "The {}th Argument of function {} must be an unsigned integer.",
                    i + 1,
                    getName());
            }
        }

        if (!isFloat(arguments[4].type))
        {
            throw Exception{ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                "The fifth argument {} of function {} should be a float,",
                arguments[4].type->getName(),
                getName()};
        }

        /// There is an additional check for constancy in ExecuteImpl
        if (!isString(arguments[5].type) || !arguments[5].column)
        {
            throw Exception{ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                "The sixth argument {} of function {} should be a constant string",
                arguments[5].type->getName(),
                getName()};
        }

        return getReturnType();
    }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & const_arguments, const DataTypePtr &, size_t input_rows_count) const override
    {
        auto arguments = const_arguments;
        /// Only last argument have to be constant
        for (size_t i = 0; i < 5; ++i)
            arguments[i].column = arguments[i].column->convertToFullColumnIfConst();

        static const auto uint64_data_type = std::make_shared<DataTypeNumber<UInt64>>();

        auto column_successes_x = castColumnAccurate(arguments[0], uint64_data_type);
        const auto & data_successes_x = checkAndGetColumn<ColumnVector<UInt64>>(column_successes_x.get())->getData();

        auto column_successes_y = castColumnAccurate(arguments[1], uint64_data_type);
        const auto & data_successes_y = checkAndGetColumn<ColumnVector<UInt64>>(column_successes_y.get())->getData();

        auto column_trials_x = castColumnAccurate(arguments[2], uint64_data_type);
        const auto & data_trials_x = checkAndGetColumn<ColumnVector<UInt64>>(column_trials_x.get())->getData();

        auto column_trials_y = castColumnAccurate(arguments[3], uint64_data_type);
        const auto & data_trials_y = checkAndGetColumn<ColumnVector<UInt64>>(column_trials_y.get())->getData();

        static const auto float64_data_type = std::make_shared<DataTypeNumber<Float64>>();

        auto column_confidence_level = castColumnAccurate(arguments[4], float64_data_type);
        const auto & data_confidence_level = checkAndGetColumn<ColumnVector<Float64>>(column_confidence_level.get())->getData();

        String usevar = checkAndGetColumnConst<ColumnString>(arguments[5].column.get())->getValue<String>();

        if (usevar != UNPOOLED && usevar != POOLED)
            throw Exception{ErrorCodes::BAD_ARGUMENTS,
                "The sixth argument {} of function {} must be equal to `pooled` or `unpooled`",
                arguments[5].type->getName(),
                getName()};

        const bool is_unpooled = (usevar == UNPOOLED);

        auto res_z_statistic = ColumnFloat64::create();
        auto & data_z_statistic = res_z_statistic->getData();
        data_z_statistic.reserve(input_rows_count);

        auto res_p_value = ColumnFloat64::create();
        auto & data_p_value = res_p_value->getData();
        data_p_value.reserve(input_rows_count);

        auto res_ci_lower = ColumnFloat64::create();
        auto & data_ci_lower = res_ci_lower->getData();
        data_ci_lower.reserve(input_rows_count);

        auto res_ci_upper = ColumnFloat64::create();
        auto & data_ci_upper = res_ci_upper->getData();
        data_ci_upper.reserve(input_rows_count);

        auto insert_values_into_result = [&data_z_statistic, &data_p_value, &data_ci_lower, &data_ci_upper](
                                             Float64 z_stat, Float64 p_value, Float64 lower, Float64 upper)
        {
            data_z_statistic.emplace_back(z_stat);
            data_p_value.emplace_back(p_value);
            data_ci_lower.emplace_back(lower);
            data_ci_upper.emplace_back(upper);
        };

        static constexpr Float64 nan = std::numeric_limits<Float64>::quiet_NaN();

        boost::math::normal_distribution<> nd(0.0, 1.0);

        for (size_t row_num = 0; row_num < input_rows_count; ++row_num)
        {
            const UInt64 successes_x = data_successes_x[row_num];
            const UInt64 successes_y = data_successes_y[row_num];
            const UInt64 trials_x = data_trials_x[row_num];
            const UInt64 trials_y = data_trials_y[row_num];
            const Float64 confidence_level = data_confidence_level[row_num];

            const Float64 props_x = static_cast<Float64>(successes_x) / trials_x;
            const Float64 props_y = static_cast<Float64>(successes_y) / trials_y;
            const Float64 diff = props_x - props_y;
            const UInt64 trials_total = trials_x + trials_y;

            if (successes_x == 0 || successes_y == 0 || successes_x > trials_x || successes_y > trials_y || trials_total == 0
                || !std::isfinite(confidence_level) || confidence_level < 0.0 || confidence_level > 1.0)
            {
                insert_values_into_result(nan, nan, nan, nan);
                continue;
            }

            Float64 se = std::sqrt(props_x * (1.0 - props_x) / trials_x + props_y * (1.0 - props_y) / trials_y);

            /// z-statistics
            /// z = \frac{ \bar{p_{1}} - \bar{p_{2}} }{ \sqrt{ \frac{ \bar{p_{1}} \left ( 1 - \bar{p_{1}} \right ) }{ n_{1} } \frac{ \bar{p_{2}} \left ( 1 - \bar{p_{2}} \right ) }{ n_{2} } } }
            Float64 zstat;
            if (is_unpooled)
            {
                zstat = (props_x - props_y) / se;
            }
            else
            {
                UInt64 successes_total = successes_x + successes_y;
                Float64 p_pooled = static_cast<Float64>(successes_total) / trials_total;
                Float64 trials_fact = 1.0 / trials_x + 1.0 / trials_y;
                zstat = diff / std::sqrt(p_pooled * (1.0 - p_pooled) * trials_fact);
            }

            if (!std::isfinite(zstat))
            {
                insert_values_into_result(nan, nan, nan, nan);
                continue;
            }

            // pvalue
            Float64 pvalue = 0;
            Float64 one_side = 1 - boost::math::cdf(nd, std::abs(zstat));
            pvalue = one_side * 2;

            // Confidence intervals
            Float64 d = props_x - props_y;
            Float64 z = -boost::math::quantile(nd, (1.0 - confidence_level) / 2.0);
            Float64 dist = z * se;
            Float64 ci_low = d - dist;
            Float64 ci_high = d + dist;

            insert_values_into_result(zstat, pvalue, ci_low, ci_high);
        }

        return ColumnTuple::create(
            Columns{std::move(res_z_statistic), std::move(res_p_value), std::move(res_ci_lower), std::move(res_ci_upper)});
    }
};

void registerFunctionZTest(FunctionFactory & factory)
{
    factory.registerFunction<FunctionTwoSampleProportionsZTest>();
}

}


/*
 * File:   HmcSampler.cpp
 * Author: aripakman
 *
 * Created on July 4, 2012, 10:44 AM
 */

#define _USE_MATH_DEFINES   // for the constant M_PI
//#include <cstdlib>
#include <cmath>
#include <tr1/random>
#include <Eigen/Dense>
#include <magnet/math/quartic.hpp>

#include "HmcSampler.h"

using namespace std;
using namespace std::tr1;
using namespace magnet::math;

const double HmcSampler::min_t = 0.00001;

HmcSampler::HmcSampler(const int & d, const int & seed ) {
dim=d;
//eng1.seed(static_cast<unsigned int >(time(NULL)));
eng1.seed(seed);
ud= uniform_real<>(0,M_PI);
}

void HmcSampler::setInitialValue(const VectorXd & initial_value){
   // double check =_verifyConstraints( initial_value );
  /*  if (check <0) {
        cout << "Initial condition out of constraint!" << endl;
        exit(1);
    } else */
            lastSample = initial_value;
    }

MatrixXd HmcSampler::sampleNext(bool returnTrace ) {

    MatrixXd tracePoints = MatrixXd(dim,0);   //this matrix will only be filled if(returnTrace)

    double T = ud(eng1);        // sample how much time T to move
    VectorXd b = lastSample;
    VectorXd a = VectorXd(dim);   // initial velocity

    while(2){

        double velsign =0;
        for (int i =0; i<dim; i++)     // Sample new initial velocity
        {  a(i)=  nd(eng1);            }

        double tt=T;   // tt is the time left to move
        double t1;
        double t2;
        int cn1,cn2; //constraint number

        bool first_bounce = true;    // for the first move, we do not fear that a small t1 or t2 is due to being in the boundary from the previous bounce.
        while (1){

            t1=0;
            if (!linearConstraints.empty())
                    _getNextLinearHitTime(a,b,t1,cn1);

            t2=0;
            if (!quadraticConstraints.empty() )
                    {   _getNextQuadraticHitTime(a,b,t2,cn2, first_bounce);
                        first_bounce=false;}

            double t =t1;     // how much time to move. if t==0, move tt
            bool linear_hit = true;
            if (t2>0 && (t1==0 || t2<t1 )){
                t=t2;
                linear_hit = false;
            }

            if (t==0 || tt < t){                 // if no wall to be hit (t==0) or not enough time left to hit the wall (tt<t2)
                break;
            }
            else{
                if (returnTrace){
                    _updateTrace(a,b,t,tracePoints);
                }

                tt = tt-t;
                VectorXd new_b   = sin(t)*a + cos(t)*b;   // hit location
                VectorXd hit_vel = cos(t)*a - sin(t)*b;   // hit velocity
                b = new_b;

                // reflect the velocity and verify that it points in the right direction

                if (!linear_hit){
                    QuadraticConstraint qc = quadraticConstraints[cn2];
                    VectorXd nabla = 2* ((qc.A)*b) + qc.B;
                    double alpha = (nabla.dot(hit_vel))/(nabla.dot(nabla));
                    a = hit_vel - 2*alpha*nabla;                  // reflected velocity
                    velsign = a.dot(nabla);
                }
                else {
                    LinearConstraint ql = linearConstraints[cn1];
                    double f2 = ((ql.f).dot((ql.f)));
                    double alpha = ((ql.f).dot(hit_vel))/f2;
                    a = hit_vel - 2*alpha*(ql.f);                  // reflected velocity
                    velsign = a.dot((ql.f));
                }
                if (velsign <0 ) break ;    //get out of while(1). resample the velocity and start again. this occurs rarely, due to numerical instabilities
            }

        } //while(1)

        if (velsign<0) {/* cout << "wrong velocity " << endl; */  continue;}    // go to beginning of while(2)

        VectorXd bb =  sin(tt)*a + cos(tt)*b;             //make last move of time tt without hitting walls

        double check = _verifyConstraints( bb);
        if (check >= 0){     //verify that we don't violate the constraints due to a numerical instability
            lastSample = bb;
            if (returnTrace){
                _updateTrace(a,b,tt,tracePoints);
                return tracePoints.transpose();

            } else return lastSample.transpose();
        }
       // at this point we have check<0, so we violated constraints: resample.

    } // while(2)
}

void HmcSampler::addLinearConstraint(const VectorXd & f, const double & g){
    LinearConstraint newConstraint;
    newConstraint.f =f;
    newConstraint.g =g;
    linearConstraints.push_back(newConstraint);
}
void HmcSampler::addQuadraticConstraint(const MatrixXd & A, const VectorXd & B, const double & C){
    QuadraticConstraint newConstraint;
    newConstraint.A =A;
    newConstraint.B =B;
    newConstraint.C =C;
    quadraticConstraints.push_back(newConstraint);

}

void HmcSampler::_getNextLinearHitTime(const VectorXd & a, const VectorXd & b, double & hit_time, int & cn ){
    hit_time=0;

    for (int i=0; i != linearConstraints.size(); i++ ){
        LinearConstraint lc = linearConstraints[i];
        double fa = (lc.f).dot(a);
        double fb = (lc.f).dot(b);
        double u = sqrt(fa*fa + fb*fb);
        if (u>lc.g && u>-lc.g){
                double phi =atan2(-fa,fb);      //     -pi < phi < pi
                double t1 = acos(-lc.g/u)-phi;  //     -pi < t1 < 2*pi

                if (t1<0) t1 += 2*M_PI;                //  0 < t1 < 2*pi
                if (abs(t1) < min_t ) t1=0;
                else if (abs(t1-2*M_PI) < min_t ) t1=0;

                double t2 = -t1-2*phi;             //  -4*pi < t2 < 3*pi
                if (t2<0) t2 += 2*M_PI;                 //-2*pi < t2 < 2*pi
                if (t2<0) t2 += 2*M_PI;                 //0 < t2 < 2*pi

                if (abs(t2) < min_t ) t2=0;
                else if (abs(t2-2*M_PI) < min_t ) t2=0;

                double t=t1;
                if (t1==0) t = t2;
                else if (t2==0) t = t1;
                else t=(t1<t2?t1:t2);

                if  (t> min_t  && (hit_time == 0 || t < hit_time)){
                    hit_time=t;
                    cn =i;
                }
        }
    }
}

void HmcSampler::_getNextQuadraticHitTime(const VectorXd & a, const VectorXd & b, double & hit_time, int & cn , const bool first_bounce ){
        hit_time=0;

        double mint;
        if (first_bounce) {mint=0;}
        else {mint=min_t;}

    for (int i=0; i != quadraticConstraints.size(); i++ ){

        QuadraticConstraint qc = quadraticConstraints[i];
        double q1= - ((a.transpose())*(qc.A))*a;
        q1 = q1 + ((b.transpose())*(qc.A))*b;
        double q2= (qc.B).dot(b);
        double q3= qc.C + a.transpose()*(qc.A)*a;
        double q4= 2*b.transpose()*(qc.A)*a;
        double q5= (qc.B).dot(a);

        double r4 = q1*q1 + q4*q4;
        double r3 = 2*q1*q2 + 2*q4*q5;
        double r2 = q2*q2 + 2*q1*q3 + q5*q5 -q4*q4;
        double r1 = 2*q2*q3 - 2*q4*q5;
        double r0=  q3*q3 - q5*q5;

        double roots[]={0,0,0,0};
        double aa = r3/r4;
        double bb = r2/r4;
        double cc = r1/r4;
        double dd = r0/r4;

        //Solve quartics of the form x^4 + aa x^3 + bb x^2 + cc x + dd ==0
        int sols = quarticSolve(aa, bb, cc, dd, roots[0], roots[1],  roots[2],  roots[3]);
        for (int j=0; j<sols; j++){
            double r = roots[j];
            if (abs(r) <=1 ){
                double l1 = q1*r*r + q2*r + q3;
                double l2 = -sqrt(1-r*r)*(q4*r + q5);
                if (l1/l2 > 0){
                    double t = acos(r);
                    if (   t> mint      && (hit_time == 0 || t < hit_time)){
                       hit_time=t;
                       cn=i;
                    }
                }
            }
        }
    }

}

double HmcSampler::_verifyConstraints(const VectorXd & b){
    double r =0;

    for (int i=0; i != quadraticConstraints.size(); i++ ){
        QuadraticConstraint qc = quadraticConstraints[i];
        double check = ((b.transpose())*(qc.A))*b + (qc.B).dot(b) + qc.C;
        if (i==0 || check < r) {
            r = check;
        }
    }

    for (int i=0; i != linearConstraints.size(); i++ ){
    LinearConstraint lc = linearConstraints[i];
    double check = (lc.f).dot(b) + lc.g;
    if (i==0 || check < r) {
        r = check;
    }
    }

    return r;
}

void HmcSampler::_updateTrace( VectorXd const & a,  VectorXd const & b, double const & t, MatrixXd & tracePoints){
    double const stepsize = .01;
    int steps = t/stepsize;

    int c = tracePoints.cols();
    tracePoints.conservativeResize(NoChange, c+steps+1);
    for (int i=0; i<steps; i++){
        VectorXd bb= sin(i*stepsize)*a + cos(i*stepsize)*b;
//      cout << bb.transpose() << endl;
        tracePoints.col(c+i) = bb;
    }
        VectorXd bb= sin(t)*a + cos(t)*b;
        tracePoints.col(c+steps) = bb;
}


#ifndef KANOOTH_NUMBERS_BOOST_INTEGER_HPP
#define KANOOTH_NUMBERS_BOOST_INTEGER_HPP

#include <boost/multiprecision/number.hpp>
#include <boost/multiprecision/detail/integer_ops.hpp>
#include <boost/multiprecision/detail/digits.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/cstdint.hpp>
#include <limits>

#include <kanooth/numbers/integer_base.hpp>
#include <kanooth/numbers/natural_number.hpp>

namespace boost {
namespace multiprecision {
namespace backends {

template <typename N>
struct kanooth_integer;

} // namespace backends

template <typename N>
struct number_category<backends::kanooth_integer<N> > : public mpl::int_<number_kind_integer> {};

namespace backends {

template <typename N>
class kanooth_integer : public kanooth::numbers::integer_base<N>
{
public:
    // needed by front-end
    typedef mpl::list<long>          signed_types;
    typedef mpl::list<unsigned long> unsigned_types;
    typedef mpl::list<>              float_types;

    template <typename T, typename R>
    struct if_supported_int
        : public enable_if<mpl::or_<mpl::contains<unsigned_types, T>,
                                    mpl::contains<signed_types, T> >, R> {};

    kanooth_integer() : base_type() {}
    kanooth_integer(long v) : base_type(v) {}
    kanooth_integer(unsigned long v) : base_type(v) {}
    kanooth_integer(const char* s) : base_type(s) {}
private:
    typedef kanooth::numbers::integer_base<N> base_type;
};

template <typename N>
inline bool eval_is_zero(const kanooth_integer<N>& val)
{
    return val.is_zero();
}

template <typename N>
inline void eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.add(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_add(kanooth_integer<N>& r, T a)
{
    r.add(r, a);
}

template <typename N>
inline void eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.add(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.add(a, b);
}

template <typename N>
inline void eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.subtract(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_subtract(kanooth_integer<N>& r, T a)
{
    r.subtract(r, a);
}

template <typename N>
inline void eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.subtract(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.subtract(a, b);
}

template <typename N>
inline void eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.multiply(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_multiply(kanooth_integer<N>& r, T a)
{
    r.multiply(r, a);
}

template <typename N>
inline void eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.multiply(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.multiply(a, b);
}

template <typename N>
inline void eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.divide_truncate(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_divide(kanooth_integer<N>& r, T a)
{
    r.divide_truncate(r, a);
}

template <typename N>
inline void eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.divide_truncate(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.divide_truncate(a, b);
}

template <typename N>
inline void eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.modulus_truncate(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_modulus(kanooth_integer<N>& r, T a)
{
    r.modulus_truncate(r, a);
}

template <typename N>
inline void eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.modulus_truncate(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.modulus_truncate(a, b);
}

template <typename N>
inline void eval_qr(const kanooth_integer<N>& x, const kanooth_integer<N>& y, kanooth_integer<N>& q, kanooth_integer<N>& r)
{
    kanooth_integer<N>::quotrem(q, r, x, y);
}

template <typename N>
inline void eval_gcd(kanooth_integer<N>& result, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    result.gcd(a, b);
}

template <typename N, class Integer>
inline typename enable_if<is_unsigned<Integer>, Integer>::type eval_integer_modulus(const kanooth_integer<N>& x, Integer val)
{
    if ((sizeof(Integer) <= sizeof(long)) || (val <= (std::numeric_limits<unsigned long>::max)())) {
        return kanooth_integer<N>::integer_modulus(x, static_cast<unsigned long>(val));
    } else {
        return default_ops::eval_integer_modulus(x, val);
    }
}

template <typename N, class Integer>
inline typename enable_if<is_signed<Integer>, Integer>::type eval_integer_modulus(const kanooth_integer<N>& x, Integer val)
{
   typedef typename make_unsigned<Integer>::type unsigned_type;
   return eval_integer_modulus(x, static_cast<unsigned_type>(std::abs(val)));
}

template <typename N>
inline void eval_bitwise_and(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_and(result, v);
}

template <typename N>
inline void eval_bitwise_or(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_or(result, v);
}

template <typename N>
inline void eval_bitwise_xor(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_xor(result, v);
}

template <typename N>
inline void eval_bitwise_and(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_and(u, v);
}

template <typename N>
inline void eval_bitwise_or(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_or(u, v);
}

template <typename N>
inline void eval_bitwise_xor(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_xor(u, v);
}

template <typename N>
inline void eval_left_shift(kanooth_integer<N>& r, unsigned long v)
{
   r.left_shift(r, v);
}

template <typename N>
inline void eval_left_shift(kanooth_integer<N>& r, kanooth_integer<N>& u, unsigned long v)
{
   r.left_shift(u, v);
}

template <typename N>
inline void eval_right_shift(kanooth_integer<N>& r, unsigned long v)
{
   r.right_shift(r, v);
}

template <typename N>
inline void eval_right_shift(kanooth_integer<N>& r, kanooth_integer<N>& u, unsigned long v)
{
   r.right_shift(u, v);
}

template <typename N>
inline bool eval_bit_test(const kanooth_integer<N>& u, unsigned pos)
{
    return u.bit_test(pos);
}

template <typename N>
inline void eval_bit_set(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_set(pos);
}

template <typename N>
inline void eval_bit_unset(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_unset(pos);
}

template <typename N>
inline void eval_bit_flip(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_flip(pos);
}

template <typename N>
inline bool eval_eq(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) == 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_eq(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) == 0;
}

template <typename N>
inline bool eval_lt(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) < 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_lt(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) < 0;
}

template <typename N>
inline bool eval_gt(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) > 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_gt(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) > 0;
}

} // namespace backends
} // namespace multiprecision
} // namespace boost

namespace kanooth {
namespace numbers {

typedef boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<natural_number> > boost_integer;

} // namespace numbers
} // namespace kanooth

namespace std {

template <typename N, boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<N>, ExpressionTemplates> >
{
   typedef boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<N>, ExpressionTemplates> number_type;
public:
   BOOST_STATIC_CONSTEXPR bool is_specialized = true;
   //
   // Largest and smallest numbers are bounded only by available memory, set
   // to zero:
   //
   static number_type (min)() BOOST_NOEXCEPT
   {
      return number_type();
   }
   static number_type (max)() BOOST_NOEXCEPT
   {
      return number_type();
   }
   static number_type lowest() BOOST_NOEXCEPT { return (min)(); }
   BOOST_STATIC_CONSTEXPR int digits = INT_MAX;
   BOOST_STATIC_CONSTEXPR int digits10 = (INT_MAX / 1000) * 301L;
   BOOST_STATIC_CONSTEXPR int max_digits10 = digits10 + 2;
   BOOST_STATIC_CONSTEXPR bool is_signed = true;
   BOOST_STATIC_CONSTEXPR bool is_integer = true;
   BOOST_STATIC_CONSTEXPR bool is_exact = true;
   BOOST_STATIC_CONSTEXPR int radix = 2;
   static number_type epsilon() BOOST_NOEXCEPT { return number_type(); }
   static number_type round_error() BOOST_NOEXCEPT { return number_type(); }
   BOOST_STATIC_CONSTEXPR int min_exponent = 0;
   BOOST_STATIC_CONSTEXPR int min_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR bool has_infinity = false;
   BOOST_STATIC_CONSTEXPR bool has_quiet_NaN = false;
   BOOST_STATIC_CONSTEXPR bool has_signaling_NaN = false;
   BOOST_STATIC_CONSTEXPR float_denorm_style has_denorm = denorm_absent;
   BOOST_STATIC_CONSTEXPR bool has_denorm_loss = false;
   static number_type infinity() BOOST_NOEXCEPT { return number_type(); }
   static number_type quiet_NaN() BOOST_NOEXCEPT { return number_type(); }
   static number_type signaling_NaN() BOOST_NOEXCEPT { return number_type(); }
   static number_type denorm_min() BOOST_NOEXCEPT { return number_type(); }
   BOOST_STATIC_CONSTEXPR bool is_iec559 = false;
   BOOST_STATIC_CONSTEXPR bool is_bounded = false;
   BOOST_STATIC_CONSTEXPR bool is_modulo = false;
   BOOST_STATIC_CONSTEXPR bool traps = false;
   BOOST_STATIC_CONSTEXPR bool tinyness_before = false;
   BOOST_STATIC_CONSTEXPR float_round_style round_style = round_toward_zero;
};

} // namespace std

#endif // KANOOTH_NUMBERS_BOOST_INTEGER_HPP


#ifndef LIDAR_UNDISTORTION_HPP_
#define LIDAR_UNDISTORTION_HPP_

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/common/eigen.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

class LidarUndistortion
{
public:
  LidarUndistortion() {}

  // Ref:LeGO-LOAM(BSD-3 LICENSE)
  // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L431-L459
  void getImu(
    Eigen::Vector3f angular_velo, Eigen::Vector3f acc, const Eigen::Quaternionf quat,
    const double imu_time /*[sec]*/)
  {
    float roll, pitch, yaw;
    Eigen::Affine3f affine(quat);
    pcl::getEulerAngles(affine, roll, pitch, yaw);

    imu_ptr_last_ = (imu_ptr_last_ + 1) % imu_que_length_;

    if ((imu_ptr_last_ + 1) % imu_que_length_ == imu_ptr_front_) {
      imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_;
    }

    imu_time_[imu_ptr_last_] = imu_time;
    imu_roll_[imu_ptr_last_] = roll;
    imu_pitch_[imu_ptr_last_] = pitch;
    imu_yaw_[imu_ptr_last_] = yaw;
    imu_acc_x_[imu_ptr_last_] = acc.x();
    imu_acc_y_[imu_ptr_last_] = acc.y();
    imu_acc_z_[imu_ptr_last_] = acc.z();
    imu_angular_velo_x_[imu_ptr_last_] = angular_velo.x();
    imu_angular_velo_y_[imu_ptr_last_] = angular_velo.y();
    imu_angular_velo_z_[imu_ptr_last_] = angular_velo.z();

    Eigen::Matrix3f rot = quat.toRotationMatrix();
    acc = rot * acc;
    // angular_velo = rot * angular_velo;

    int imu_ptr_back = (imu_ptr_last_ - 1 + imu_que_length_) % imu_que_length_;
    double time_diff = imu_time_[imu_ptr_last_] - imu_time_[imu_ptr_back];
    if (time_diff < scan_period_) {
      imu_shift_x_[imu_ptr_last_] =
        imu_shift_x_[imu_ptr_back] + imu_velo_x_[imu_ptr_back] * time_diff + acc(0) * time_diff *
        time_diff * 0.5;
      imu_shift_y_[imu_ptr_last_] =
        imu_shift_y_[imu_ptr_back] + imu_velo_y_[imu_ptr_back] * time_diff + acc(1) * time_diff *
        time_diff * 0.5;
      imu_shift_z_[imu_ptr_last_] =
        imu_shift_z_[imu_ptr_back] + imu_velo_z_[imu_ptr_back] * time_diff + acc(2) * time_diff *
        time_diff * 0.5;

      imu_velo_x_[imu_ptr_last_] = imu_velo_x_[imu_ptr_back] + acc(0) * time_diff;
      imu_velo_y_[imu_ptr_last_] = imu_velo_y_[imu_ptr_back] + acc(1) * time_diff;
      imu_velo_z_[imu_ptr_last_] = imu_velo_z_[imu_ptr_back] + acc(2) * time_diff;

      imu_angular_rot_x_[imu_ptr_last_] = imu_angular_rot_x_[imu_ptr_back] + angular_velo(0) *
        time_diff;
      imu_angular_rot_y_[imu_ptr_last_] = imu_angular_rot_y_[imu_ptr_back] + angular_velo(1) *
        time_diff;
      imu_angular_rot_z_[imu_ptr_last_] = imu_angular_rot_z_[imu_ptr_back] + angular_velo(2) *
        time_diff;
    }
  }

  // Ref:LeGO-LOAM(BSD-3 LICENSE)
  // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L491-L619
  void adjustDistortion(
    pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
    const double scan_time /*[sec]*/)
  {
    bool half_passed = false;
    int cloud_size = cloud->points.size();

    float start_ori = -std::atan2(cloud->points[0].y, cloud->points[0].x);
    float end_ori = -std::atan2(cloud->points[cloud_size - 1].y, cloud->points[cloud_size - 1].x);
    if (end_ori - start_ori > 3 * M_PI) {
      end_ori -= 2 * M_PI;
    } else if (end_ori - start_ori < M_PI) {
      end_ori += 2 * M_PI;
    }
    float ori_diff = end_ori - start_ori;

    Eigen::Vector3f rpy_start, shift_start, velo_start, rpy_cur, shift_cur, velo_cur;
    Eigen::Vector3f shift_from_start;
    Eigen::Matrix3f r_s_i, r_c;
    Eigen::Vector3f adjusted_p;
    float ori_h;
    for (int i = 0; i < cloud_size; ++i) {
      pcl::PointXYZI & p = cloud->points[i];
      ori_h = -std::atan2(p.y, p.x);
      if (!half_passed) {
        if (ori_h < start_ori - M_PI * 0.5) {
          ori_h += 2 * M_PI;
        } else if (ori_h > start_ori + M_PI * 1.5) {
          ori_h -= 2 * M_PI;
        }

        if (ori_h - start_ori > M_PI) {
          half_passed = true;
        }
      } else {
        ori_h += 2 * M_PI;
        if (ori_h < end_ori - 1.5 * M_PI) {
          ori_h += 2 * M_PI;
        } else if (ori_h > end_ori + 0.5 * M_PI) {
          ori_h -= 2 * M_PI;
        }
      }

      float rel_time = (ori_h - start_ori) / ori_diff * scan_period_;

      if (imu_ptr_last_ > 0) {
        imu_ptr_front_ = imu_ptr_last_iter_;
        while (imu_ptr_front_ != imu_ptr_last_) {
          if (scan_time + rel_time > imu_time_[imu_ptr_front_]) {
            break;
          }
          imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_;
        }

        if (scan_time + rel_time > imu_time_[imu_ptr_front_]) {
          rpy_cur(0) = imu_roll_[imu_ptr_front_];
          rpy_cur(1) = imu_pitch_[imu_ptr_front_];
          rpy_cur(2) = imu_yaw_[imu_ptr_front_];
          shift_cur(0) = imu_shift_x_[imu_ptr_front_];
          shift_cur(1) = imu_shift_y_[imu_ptr_front_];
          shift_cur(2) = imu_shift_z_[imu_ptr_front_];
          velo_cur(0) = imu_velo_x_[imu_ptr_front_];
          velo_cur(1) = imu_velo_y_[imu_ptr_front_];
          velo_cur(2) = imu_velo_z_[imu_ptr_front_];
        } else {
          int imu_ptr_back = (imu_ptr_front_ - 1 + imu_que_length_) % imu_que_length_;
          float ratio_front = (scan_time + rel_time - imu_time_[imu_ptr_back]) /
            (imu_time_[imu_ptr_front_] - imu_time_[imu_ptr_back]);
          float ratio_back = 1.0 - ratio_front;
          rpy_cur(0) = imu_roll_[imu_ptr_front_] * ratio_front + imu_roll_[imu_ptr_back] *
            ratio_back;
          rpy_cur(1) = imu_pitch_[imu_ptr_front_] * ratio_front + imu_pitch_[imu_ptr_back] *
            ratio_back;
          rpy_cur(2) = imu_yaw_[imu_ptr_front_] * ratio_front + imu_yaw_[imu_ptr_back] * ratio_back;
          shift_cur(0) = imu_shift_x_[imu_ptr_front_] * ratio_front + imu_shift_x_[imu_ptr_back] *
            ratio_back;
          shift_cur(1) = imu_shift_y_[imu_ptr_front_] * ratio_front + imu_shift_y_[imu_ptr_back] *
            ratio_back;
          shift_cur(2) = imu_shift_z_[imu_ptr_front_] * ratio_front + imu_shift_z_[imu_ptr_back] *
            ratio_back;
          velo_cur(0) = imu_velo_x_[imu_ptr_front_] * ratio_front + imu_velo_x_[imu_ptr_back] *
            ratio_back;
          velo_cur(1) = imu_velo_y_[imu_ptr_front_] * ratio_front + imu_velo_y_[imu_ptr_back] *
            ratio_back;
          velo_cur(2) = imu_velo_z_[imu_ptr_front_] * ratio_front + imu_velo_z_[imu_ptr_back] *
            ratio_back;
        }

        r_c = (
          Eigen::AngleAxisf(rpy_cur(2), Eigen::Vector3f::UnitZ()) *
          Eigen::AngleAxisf(rpy_cur(1), Eigen::Vector3f::UnitY()) *
          Eigen::AngleAxisf(rpy_cur(0), Eigen::Vector3f::UnitX())
          ).toRotationMatrix();

        if (i == 0) {
          rpy_start = rpy_cur;
          shift_start = shift_cur;
          velo_start = velo_cur;
          r_s_i = r_c.inverse();
        } else {
          shift_from_start = shift_cur - shift_start - velo_start * rel_time;
          adjusted_p = r_s_i * (r_c * Eigen::Vector3f(p.x, p.y, p.z) + shift_from_start);
          p.x = adjusted_p.x();
          p.y = adjusted_p.y();
          p.z = adjusted_p.z();
        }
      }
      imu_ptr_last_iter_ = imu_ptr_front_;
    }
  }

  void setScanPeriod(const double scan_period /*[sec]*/)
  {
    scan_period_ = scan_period;
  }

private:
  double scan_period_{0.1};
  static const int imu_que_length_{200};
  int imu_ptr_front_{0}, imu_ptr_last_{-1}, imu_ptr_last_iter_{0};

  std::array<double, imu_que_length_> imu_time_;
  std::array<float, imu_que_length_> imu_roll_;
  std::array<float, imu_que_length_> imu_pitch_;
  std::array<float, imu_que_length_> imu_yaw_;

  std::array<float, imu_que_length_> imu_acc_x_;
  std::array<float, imu_que_length_> imu_acc_y_;
  std::array<float, imu_que_length_> imu_acc_z_;
  std::array<float, imu_que_length_> imu_velo_x_;
  std::array<float, imu_que_length_> imu_velo_y_;
  std::array<float, imu_que_length_> imu_velo_z_;
  std::array<float, imu_que_length_> imu_shift_x_;
  std::array<float, imu_que_length_> imu_shift_y_;
  std::array<float, imu_que_length_> imu_shift_z_;

  std::array<float, imu_que_length_> imu_angular_velo_x_;
  std::array<float, imu_que_length_> imu_angular_velo_y_;
  std::array<float, imu_que_length_> imu_angular_velo_z_;
  std::array<float, imu_que_length_> imu_angular_rot_x_;
  std::array<float, imu_que_length_> imu_angular_rot_y_;
  std::array<float, imu_que_length_> imu_angular_rot_z_;
};

#endif  // LIDAR_UNDISTORTION_HPP_


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

#include <nt2/elliptic/functions/am.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/sin.hpp>
#include <nt2/include/functions/simd/sqrt.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  (am_, tag::cpu_,
                             (A0)(A1)(A2)(A3),
                             (generic_<floating_<A0> >)
                             (generic_<floating_<A1> >)
                             (scalar_<floating_<A2> >)
                             (scalar_<integer_<A3> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0 & u, const A1 & x,
                                             const A2 & tol, const A3 & choice) const
    {
      switch (choice)
      {
      case 'a': return am(u,nt2::sin(x), tol);
      case 'm': return am(u,nt2::sqrt(nt2::abs(x)), tol);
      default : return am(u,x,tol);
      }
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (am_, tag::cpu_,
                             (A0)(A1)(A2),
                             (generic_<floating_<A0> >)
                             (generic_<floating_<A1> >)
                             (scalar_<integer_<A2> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0 & u, const A1 & x,
                                             const A2 & choice) const
    {
      typedef typename meta::scalar_of<result_type>::type sA0;
      return am(u,x,Eps<sA0>(),choice);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  (am_, tag::cpu_,
                              (A0)(A1),
                              (generic_<floating_<A0> >)
                              (generic_<floating_<A1> >)
                             )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(const A0 & u, const A1 & x) const
    {
      typedef typename meta::scalar_of<result_type>::type sA0;
      return am(u,x,Eps<sA0>());
    }
  };
} }
#endif


/*
 * DistributionRightHandSide.cc
 *
 *  Created on: 30.06.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/types.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe.h>
#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/fe/fe_values.h>

#include <base/DiscretizedFunction.h>
#include <forward/DistributionRightHandSide.h>

#include <vector>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template <int dim>
DistributionRightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const FiniteElement<dim> &fe,
                                                                         const Quadrature<dim> &quad)
    : fe_values(fe, quad, update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
DistributionRightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
DistributionRightHandSide<dim>::DistributionRightHandSide(Function<dim> *f1, Function<dim> *f2) {
  this->f1 = f1;
  this->f2 = f2;
}

template <int dim>
void DistributionRightHandSide<dim>::copy_local_to_global(Vector<double> &result, const AssemblyCopyData &copy_data) {
  for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i)
    result(copy_data.local_dof_indices[i]) += copy_data.cell_rhs(i);
}

template <int dim>
void DistributionRightHandSide<dim>::local_assemble_cc(const Function<dim> *const f1, const Function<dim> *const f2,
                                                       const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                       AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
    if (f1 != nullptr) {
      const double val1 = f1->value(scratch_data.fe_values.quadrature_point(q_point));

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        copy_data.cell_rhs(i) +=
            val1 * scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
    }

    if (f2 != nullptr) {
      auto val2 = f2->gradient(scratch_data.fe_values.quadrature_point(q_point));

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        copy_data.cell_rhs(i) +=
            val2 * scratch_data.fe_values.shape_grad(i, q_point) * scratch_data.fe_values.JxW(q_point);
    }
  }

  cell->get_dof_indices(copy_data.local_dof_indices);
}

template <int dim>
void DistributionRightHandSide<dim>::local_assemble_dd(const Vector<double> &f1, const Vector<double> &f2,
                                                       const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                       AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
        copy_data.cell_rhs(i) += (f1[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point) *
                                      scratch_data.fe_values.shape_value(i, q_point) +
                                  f2[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_grad(k, q_point) *
                                      scratch_data.fe_values.shape_grad(i, q_point)) *
                                 scratch_data.fe_values.JxW(q_point);
}

template <int dim>
void DistributionRightHandSide<dim>::local_assemble_cd(const Function<dim> *const f1, const Vector<double> &f2,
                                                       const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                       AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
    if (f1 != nullptr) {
      const double val1 = f1->value(scratch_data.fe_values.quadrature_point(q_point));

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        copy_data.cell_rhs(i) +=
            val1 * scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
    }

    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
        copy_data.cell_rhs(i) += f2[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_grad(k, q_point) *
                                 scratch_data.fe_values.shape_grad(i, q_point) * scratch_data.fe_values.JxW(q_point);
    }
  }
}

template <int dim>
void DistributionRightHandSide<dim>::local_assemble_dc(const Vector<double> &f1, const Function<dim> *const f2,
                                                       const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                       AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    for (unsigned int i = 0; i < dofs_per_cell; ++i) {
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
        copy_data.cell_rhs(i) += f1[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point) *
                                 scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);

      if (f2 != nullptr) {
        auto val2 = f2->gradient(scratch_data.fe_values.quadrature_point(q_point));

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          copy_data.cell_rhs(i) +=
              val2 * scratch_data.fe_values.shape_grad(i, q_point) * scratch_data.fe_values.JxW(q_point);
      }
    }
}

template <int dim>
void DistributionRightHandSide<dim>::create_right_hand_side(const DoFHandler<dim> &dof, const Quadrature<dim> &quad,
                                                            Vector<double> &rhs) const {
  f1->set_time(this->get_time());
  f2->set_time(this->get_time());

  auto f1_d = dynamic_cast<DiscretizedFunction<dim> *>(f1);
  auto f2_d = dynamic_cast<DiscretizedFunction<dim> *>(f2);

  if (f1_d != nullptr)
    Assert(f1_d->get_function_coefficients(f1_d->get_time_index()).size() == dof.n_dofs(),
           ExcDimensionMismatch(f1_d->get_function_coefficients(f1_d->get_time_index()).size(), dof.n_dofs()));

  if (f2_d != nullptr)
    Assert(f2_d->get_function_coefficients(f2_d->get_time_index()).size() == dof.n_dofs(),
           ExcDimensionMismatch(f2_d->get_function_coefficients(f2_d->get_time_index()).size(), dof.n_dofs()));

  if (f1_d != nullptr && f2_d != nullptr)
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DistributionRightHandSide<dim>::local_assemble_cc, f1, f2, std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DistributionRightHandSide<dim>::copy_local_to_global, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  else if (f1_d == nullptr && f2_d != nullptr)
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DistributionRightHandSide<dim>::local_assemble_cd, f1,
                  std::ref(f2_d->get_function_coefficients(f2_d->get_time_index())), std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DistributionRightHandSide<dim>::copy_local_to_global, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  else if (f1_d != nullptr && f2_d == nullptr)
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DistributionRightHandSide<dim>::local_assemble_dc,
                  std::ref(f1_d->get_function_coefficients(f1_d->get_time_index())), f2, std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DistributionRightHandSide<dim>::copy_local_to_global, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  else
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DistributionRightHandSide<dim>::local_assemble_cc, f1, f2, std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DistributionRightHandSide<dim>::copy_local_to_global, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
}

template class DistributionRightHandSide<1>;
template class DistributionRightHandSide<2>;
template class DistributionRightHandSide<3>;

} /* namespace forward */
} /* namespace wavepi */


#ifndef DDI_PROCESS_HPP
#define DDI_PROCESS_HPP
#define _USE_MATH_DEFINES
#include <math.h>
#include <boost/math/special_functions/sinc.hpp>
#include "basic_process.hpp"
#include "basic_voxel.hpp"
#include "image_model.hpp"

double base_function(double theta);
class QSpace2Odf  : public BaseProcess
{
public:// recorded for scheme balanced
    std::vector<image::vector<3,double> > q_vectors_time;
public:
    std::vector<unsigned int> b0_images;
    std::vector<float> sinc_ql;

public:
    virtual void init(Voxel& voxel)
    {
        b0_images.clear();
        for(unsigned int index = 0;index < voxel.bvalues.size();++index)
            if(voxel.bvalues[index] == 0)
                b0_images.push_back(index);
        if(b0_images.size() > 1)
            throw std::runtime_error("Correct B0 failed. Two b0 images found in src file");

        unsigned int odf_size = voxel.ti.half_vertices_count;
        float sigma = voxel.param[0]; //optimal 1.24
        if(!voxel.grad_dev.empty())
        {
            q_vectors_time.resize(voxel.bvalues.size());
            for (unsigned int index = 0; index < voxel.bvalues.size(); ++index)
            {
                q_vectors_time[index] = voxel.bvectors[index];
                q_vectors_time[index] *= std::sqrt(voxel.bvalues[index]*0.01506);// get q in (mm) -1
                q_vectors_time[index] *= sigma;
            }
            return;
        }
        sinc_ql.resize(odf_size*voxel.bvalues.size());
        // calculate reconstruction matrix
        for (unsigned int j = 0,index = 0; j < odf_size; ++j)
            for (unsigned int i = 0; i < voxel.bvalues.size(); ++i,++index)
                sinc_ql[index] = voxel.bvectors[i]*
                             image::vector<3,float>(voxel.ti.vertices[j])*
                               std::sqrt(voxel.bvalues[i]*0.01506);

        for (unsigned int index = 0; index < sinc_ql.size(); ++index)
            sinc_ql[index] = voxel.r2_weighted ?
                         base_function(sinc_ql[index]*sigma):
                         boost::math::sinc_pi(sinc_ql[index]*sigma);

    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        if(b0_images.size() == 1 && voxel.half_sphere)
            data.space[b0_images.front()] *= 0.5;

        if(!voxel.grad_dev.empty()) // correction for gradient nonlinearity
        {
            /*
            new_bvecs = (I+grad_dev) * bvecs;
            */
            float grad_dev[9];
            for(unsigned int i = 0; i < 9; ++i)
                grad_dev[i] = voxel.grad_dev[i][data.voxel_index];
            image::mat::transpose(grad_dev,image::dim<3,3>());
            std::vector<float> new_sinc_ql(data.odf.size()*data.space.size());
            for (unsigned int j = 0,index = 0; j < data.odf.size(); ++j)
            {
                image::vector<3,float> from(voxel.ti.vertices[j]);
                from.rotate(grad_dev);
                from.normalize();
                if(voxel.r2_weighted)
                    for (unsigned int i = 0; i < data.space.size(); ++i,++index)
                        new_sinc_ql[index] = base_function(q_vectors_time[i]*from);
                else
                    for (unsigned int i = 0; i < data.space.size(); ++i,++index)
                        new_sinc_ql[index] = boost::math::sinc_pi(q_vectors_time[i]*from);

            }
            image::mat::vector_product(&*new_sinc_ql.begin(),&*data.space.begin(),&*data.odf.begin(),
                                    image::dyndim(data.odf.size(),data.space.size()));
        }
        else
            image::mat::vector_product(&*sinc_ql.begin(),&*data.space.begin(),&*data.odf.begin(),
                                    image::dyndim(data.odf.size(),data.space.size()));
    }
};

class HQSpace2Odf  : public BaseProcess
{
public:// recorded for scheme balanced
    std::vector<image::vector<3,double> > q_vectors_time;
public:
    std::vector<float> sinc_ql;
public:
    bool hgqi = false;
    std::vector<float> hraw;
    std::vector<int> offset;
    std::vector<float> scaling;
public:
    virtual void init(Voxel& voxel)
    {
        if(voxel.bvalues.size() != 1)
        {
            hgqi = false;
            return;
        }
        hgqi = true;
        hraw.resize(voxel.dim.size());
        int range = 2;
        for(int dz = 0;dz <= range;++dz) // half sphere
            for(int dy = -range;dy <= range;++dy)
                for(int dx = -range;dx <= range;++dx)
                {
                    int r2 = dx*dx+dy*dy+dz*dz;
                    voxel.bvalues.push_back(r2*500);
                    image::vector<3> dir(dx,dy,dz);
                    dir.normalize();
                    voxel.bvectors.push_back(dir);
                    offset.push_back(dx + dy*voxel.dim.width() + dz*voxel.dim.plane_size());
                    scaling.push_back(std::exp(-r2));
                }

        unsigned int odf_size = voxel.ti.half_vertices_count;
        float sigma = voxel.param[0]; //optimal 1.24
        sinc_ql.resize(odf_size*voxel.bvalues.size());
        // calculate reconstruction matrix
        for (unsigned int j = 0,index = 0; j < odf_size; ++j)
            for (unsigned int i = 0; i < voxel.bvalues.size(); ++i,++index)
                sinc_ql[index] = voxel.bvectors[i]*
                             image::vector<3,float>(voxel.ti.vertices[j])*
                               std::sqrt(voxel.bvalues[i]*0.01506);

        for (unsigned int index = 0; index < sinc_ql.size(); ++index)
            sinc_ql[index] = voxel.r2_weighted ?
                         base_function(sinc_ql[index]*sigma):
                         boost::math::sinc_pi(sinc_ql[index]*sigma);

    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        if(!hgqi)
            return;
        if(data.space[0] == 0)
        {
            hraw[data.voxel_index] = 0;
            std::fill(data.odf.begin(),data.odf.end(),0.0f);
            return;
        }
        hraw[data.voxel_index] = data.space[0];
        auto I = image::make_image(voxel.dwi_data[0],voxel.dim);
        data.space.resize(voxel.bvalues.size());
        for(int i = 1;i < voxel.bvalues.size();++i)
        {
            int pos1 = data.voxel_index;
            int pos2 = data.voxel_index;
            pos1 += offset[i-1];
            pos2 -= offset[i-1];
            if(pos1 < 0 || pos1 >= voxel.dim.size())
                pos1 = pos2;
            if(pos2 < 0 || pos2 >= voxel.dim.size())
                pos2 = pos1;
            data.space[i] = std::fabs(data.space[0]-(I[pos1]+I[pos2])/2.0f)*scaling[i-1];
        }

        image::mat::vector_product(&*sinc_ql.begin(),&*data.space.begin(),&*data.odf.begin(),
                                    image::dyndim(data.odf.size(),data.space.size()));
    }
    virtual void end(Voxel&,gz_mat_write& mat_writer)
    {
        if(hgqi)
            mat_writer.write("hraw",&hraw[0],1,hraw.size());
    }
};

class SchemeConverter : public BaseProcess
{
    QSpace2Odf from,to;
    std::vector<int> piv;
    std::vector<image::vector<3,float> > bvectors;
    std::vector<float> bvalues;
    std::vector<std::vector<unsigned short> > dwi;
    std::vector<unsigned short> b0;
    std::vector<float> A,Rt;
    unsigned int total_value;
    unsigned int total_negative_value;
public:
    virtual void init(Voxel& voxel)
    {

        if(!voxel.file_name.empty())
        {
            std::ifstream read(voxel.file_name.c_str());
            std::vector<float> values;
            std::copy(std::istream_iterator<float>(read),std::istream_iterator<float>(),std::back_inserter(values));
            for(unsigned int i = 0;i < values.size()/4;++i)
            {
                if(values[i*4] == 0.0)
                    continue;
                bvalues.push_back(voxel.param[1]);
                bvectors.push_back(image::vector<3,float>(values[i*4+1],values[i*4+2],values[i*4+3]));
            }
        }
        else
        {
            bvalues.resize(voxel.ti.half_vertices_count);
            bvectors.resize(voxel.ti.half_vertices_count);
            for (unsigned int index = 0; index < bvectors.size(); ++index)
                bvectors[index] = voxel.ti.vertices[index];
            std::fill(bvalues.begin(),bvalues.end(),voxel.param[1]); // set the output b-value
        }

        //allocated output image space
        dwi.resize(bvalues.size());
        for(unsigned int index = 0;index < dwi.size();++index)
            dwi[index].resize(voxel.dim.size());

        from.init(voxel);
        voxel.bvalues.swap(bvalues);
        voxel.bvectors.swap(bvectors);
        to.init(voxel);
        voxel.bvalues.swap(bvalues);
        voxel.bvectors.swap(bvectors);

        if(!from.b0_images.empty())
            b0.resize(voxel.dim.size());

        Rt.resize(dwi.size()*dwi.size());
        image::mat::transpose(&*to.sinc_ql.begin(),&*Rt.begin(),image::dyndim(dwi.size(),dwi.size()));
        A.resize(dwi.size()*dwi.size());
        piv.resize(dwi.size());
        image::mat::product_transpose(&*Rt.begin(),&*Rt.begin(),&*A.begin(),
                                       image::dyndim(dwi.size(),dwi.size()),image::dyndim(dwi.size(),dwi.size()));
        float max_value = *std::max_element(A.begin(),A.end());
        for (unsigned int i = 0,index = 0; i < dwi.size(); ++i,index += dwi.size() + 1)
            A[index] += max_value*voxel.param[2];
        image::mat::lu_decomposition(A.begin(),piv.begin(),image::dyndim(dwi.size(),dwi.size()));

        total_negative_value = 0;
        total_value = 0;

        voxel.recon_report
                << " The converted HARDI has a total of " << dwi.size() << " diffusion sampling directions.";
    }

    virtual void run(Voxel& voxel, VoxelData& data)
    {
        if(!from.b0_images.empty())
        {
            b0[data.voxel_index] = data.space[from.b0_images.front()];
            data.space[from.b0_images.front()] = 0;
        }
        from.run(voxel,data);
        std::vector<float> hardi_data(dwi.size()),tmp(dwi.size());
        image::mat::vector_product(&*Rt.begin(),&*data.odf.begin(),&*tmp.begin(),image::dyndim(dwi.size(),dwi.size()));
        image::mat::lu_solve(&*A.begin(),&*piv.begin(),&*tmp.begin(),&*hardi_data.begin(),image::dyndim(dwi.size(),dwi.size()));
        for(unsigned int index = 0;index < dwi.size();++index)
        {
            if(hardi_data[index] < 0.0)
                ++total_negative_value;
            else
                dwi[index][data.voxel_index] = hardi_data[index];
            ++total_value;
        }
    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        voxel.recon_report
                << " The percentage of the negative signals was " << (float)100.0*total_negative_value/(float)total_value << "%.";
        std::vector<float> b_table(4); // space for b0
        for (unsigned int index = 0;index < bvectors.size();++index)
        {
            b_table.push_back(bvalues.front());
            std::copy(bvectors[index].begin(),bvectors[index].end(),std::back_inserter(b_table));
        }
        mat_writer.write("b_table",&b_table[0],4,b_table.size()/4);
        unsigned int image_num = 0;
        if(!b0.empty())
        {
            mat_writer.write("image0",&b0[0],1,b0.size());
            ++image_num;
        }
        for (unsigned int index = 0;index < dwi.size();++index)
        {
            std::ostringstream out;
            out << "image" << image_num;
            mat_writer.write(out.str().c_str(),&(dwi[index][0]),1,dwi[index].size());
            ++image_num;
        }
    }
};

template<class value_type>
value_type sinint(value_type x)
{
    bool sgn = x > 0;
    x = std::fabs(x);
    value_type eps = 1e-15f;
    value_type x2 = x*x;
    value_type si;
    if(x == 0.0)
        return 0.0;
    if(x <= 16.0)
    {
        value_type xr = x;
        si = x;
        for(unsigned int k = 1;k <= 40;++k)
        {
            si += (xr *= -0.5*(value_type)(2*k-1)/(value_type)k/(value_type)(4*k*(k+1)+1)*x2);
            if(std::fabs(xr) < std::fabs(si)*eps)
                break;
        }
        return sgn ? si:-si;
    }

    if(x <= 32.0)
    {
        unsigned int m = std::floor(47.2+0.82*x);
        std::vector<double> bj(m+1);
        value_type xa1 = 0.0f;
        value_type xa0 = 1.0e-100f;
        for(unsigned int k=m;k>=1;--k)
        {
            value_type xa = 4.0*(value_type)k*xa0/x-xa1;
            bj[k-1] = xa;
            xa1 = xa0;
            xa0 = xa;
        }
        value_type xs = bj[0];
        for(unsigned int k=3;k <= m;k += 2)
            xs += 2.0*bj[k-1];
        for(unsigned int k=0;k < m;++k)
            bj[k] /= xs;
        value_type xr = 1.0;
        value_type xg1 = bj[0];
        for(int k=2;k <= m;++k)
            xg1 += bj[k-1]*(xr *= 0.25*(2*k-3)*(2*k-3)/((k-1)*(2*k-1)*(2*k-1))*x);
        xr = 1.0;
        value_type xg2 = bj[0];
        for(int k=2;k <= m;++k)
            xg2 += bj[k-1]*(xr *= 0.25*(2*k-5)*(2*k-5)/((k-1)*(2*k-3)*(2*k-3))*x);
        si = x*std::cos(x/2.0)*xg1+2.0*std::sin(x/2.0)*xg2-std::sin(x);
        return sgn ? si:-si;
    }

    value_type xr = 1.0;
    value_type xf = 1.0;
    for(unsigned int k=1;k <= 9;++k)
        xf += (xr *= -2.0*k*(2*k-1)/x2);
    xr = 1.0/x;
    value_type xg = xr;
    for(unsigned int k=1;k <= 8;++k)
        xg += (xr *= -2.0*(2*k+1)*k/x2);
    si = 1.570796326794897-xf*std::cos(x)/x-xg*std::sin(x)/x;
    return sgn ? si:-si;
}

class QSpaceSpectral  : public BaseProcess
{
public:
    static const int max_length = 50; // 50 microns
    std::vector<unsigned int> b0_images;
    std::vector<std::vector<float> > cdf,dis,cdfw,disw;
public:
    virtual void init(Voxel& voxel)
    {
        b0_images.clear();
        for(unsigned int index = 0;index < voxel.bvalues.size();++index)
            if(voxel.bvalues[index] == 0)
                b0_images.push_back(index);
        if(b0_images.size() > 1)
            throw std::runtime_error("Correct B0 failed. Two b0 images found in src file");

        float diffusion_time = voxel.param[1];
        float diffusion_length = std::sqrt(6.0*3.0*diffusion_time); // sqrt(6Dt)
        dis.clear();
        cdf.clear();
        disw.clear();
        cdfw.clear();
        dis.resize(max_length);
        cdf.resize(max_length);
        disw.resize(max_length);
        cdfw.resize(max_length);
        for(unsigned int n = 0;n < max_length;++n) // from 0 micron to 49 microns
        {
            // calculate the diffusion length ratio
            float sigma = ((float)n)/diffusion_length;

            dis[n].resize(voxel.dim.size());
            cdf[n].resize(voxel.dim.size());

            disw[n].resize(voxel.bvalues.size());
            cdfw[n].resize(voxel.bvalues.size());
            for(unsigned int index = 0;index < voxel.bvalues.size();++index)
            {
                // 2pi*L*q = sigma*sqrt(6D*b_value)
                double lq_2pi = sigma*std::sqrt(voxel.bvalues[index]*0.018);
                disw[n][index] = boost::math::sinc_pi(lq_2pi);
                cdfw[n][index] = (voxel.bvalues[index] == 0.0 ?
                                 sigma : sinint(lq_2pi)/std::sqrt(voxel.bvalues[index]*0.018));
            }
        }
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        if(b0_images.size() == 1 && voxel.half_sphere)
            data.space[b0_images.front()] *= 0.5;
        for(unsigned int index = 0;index < max_length;++index)
        {
            dis[index][data.voxel_index] =
                    image::vec::dot(data.space.begin(),data.space.end(),disw[index].begin());
            cdf[index][data.voxel_index] =
                    image::vec::dot(data.space.begin(),data.space.end(),cdfw[index].begin());
            // make sure that cdf is increamental
            if(index && cdf[index][data.voxel_index] < cdf[index-1][data.voxel_index])
                cdf[index][data.voxel_index] = cdf[index-1][data.voxel_index];
            // make sure that dis is positive
            if(dis[index][data.voxel_index] < 0.0)
                dis[index][data.voxel_index] = 0.0;
        }
    }
    virtual void end(Voxel& voxel,gz_mat_write& mat_writer)
    {
        for(unsigned int index = 0;index < max_length;++index)
        {
            std::ostringstream out;
            out << "pdf_" << index << "um";
            mat_writer.write(out.str().c_str(),&*dis[index].begin(),1,dis[index].size());

        }
        for(unsigned int index = 0;index < max_length;++index)
        {
            std::ostringstream out;
            out << "cdf_" << index << "um";
            mat_writer.write(out.str().c_str(),&*cdf[index].begin(),1,cdf[index].size());
        }
        mat_writer.write("fa0",&*dis[0].begin(),1,dis[0].size());
        std::vector<short> index0(voxel.dim.size());
        mat_writer.write("index0",&*index0.begin(),1,index0.size());
    }
};

class RestrictedDiffusionImaging  : public BaseProcess
{
private:
    std::vector<std::vector<float> > rdi;
public:
    virtual void init(Voxel& voxel)
    {
        float sigma = voxel.param[0]; //optimal 1.24
        if(!voxel.output_rdi)
            return;
        for(float L = 0.2f;L <= sigma;L+= 0.2f)
        {
            rdi.push_back(std::vector<float>());
            rdi.back().resize(voxel.bvalues.size());
            for(unsigned int index = 0;index < voxel.bvalues.size();++index)
            {
                float q = std::sqrt(voxel.bvalues[index]*0.018);
                rdi.back()[index] = (q > 0)? sinint(L*q)/q: L;
            }
        }
    }
    virtual void run(Voxel& voxel, VoxelData& data)
    {
        if(!voxel.output_rdi)
            return;
        float last_value = 0;
        std::vector<float> rdi_values(rdi.size());
        for(unsigned int index = 0;index < rdi.size();++index)
        {
            // force incremental
            rdi_values[index] = std::max<float>(last_value,image::vec::dot(rdi[index].begin(),rdi[index].end(),data.space.begin()));
            last_value = rdi_values[index];
        }
        data.rdi.swap(rdi_values);
    }
};
#endif//DDI_PROCESS_HPP