/* p_integrand.cpp */
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/interpolators/cubic_hermite.hpp>
#include <algorithm>

extern "C"
{
/* Survival function for R^phi*W */
int RW_marginal_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*xval[i]);
        result[i] = boost::math::gamma_p(0.5L,tmp1) + boost::math::tgamma((long double)(a+1),tmp1)*tmp0-pow(tmp1,a)*exp(-tmp1)*tmp0;
    }
    return 1;
}

/* Marginal distribution function for R^phi*W + epsilon */
int pRW_me_interp_C(double *xval, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    bool tau_bool = (tau_sqd > 0.05);
    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<820)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * surv_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = 0.5*erfc(-xval[i]/sd_const)-tmp_sum/sd_const_pi;
            tmp_sum = 0;

            /* CDF value must be greater than 0 */
            if(tmp_res < 0){
                tmp_res = 0;
            }
            result[i] = tmp_res;
        }
        else{
            tmp_int = RW_marginal_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = 1-tmp_res;
        }
    }

    return 1;
}

/* Transform to uniform scales from RW mixtures */
int RW_me_2_unifs(double *X, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    int n_s, int n_grid, int n_t, double *unifs){
    int tmp_int, X_lookup, Surv_lookup;
    for (int i = 0; i<n_s;i++){
        X_lookup = i*n_t;
        Surv_lookup = i*n_grid;
        tmp_int = pRW_me_interp_C(X+X_lookup, xp, Surv+Surv_lookup, tau_sqd, phi[i], gamma, n_t, n_grid, unifs+X_lookup);
    }

    return 1;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_me_C(double min_p, double max_p, double min_x, double max_x, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_grid, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    while (p_min_x > min_p){
        min_x = min_x-40/phi;
        tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    while (p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* PCHIP 1-D monotonic cubic interpolation */
using boost::math::interpolators::cubic_hermite;
int pchip(double *x_input, double *y_input, double *x_pred, double *y_pred_res, int n, int n_pred){
    std::vector<double> x(n);
    memcpy(&x[0], x_input, n*sizeof(double));
    std::vector<double> y(n);
    memcpy(&y[0], y_input, n*sizeof(double));

    double left_endpoint_derivative = std::numeric_limits<double>::quiet_NaN();
    double right_endpoint_derivative = std::numeric_limits<double>::quiet_NaN();
    std::vector<double> s(x.size(), std::numeric_limits<double>::quiet_NaN());
    if (isnan(left_endpoint_derivative)){
        // O(h) finite difference derivative:
        // This, I believe, is the only derivative guaranteed to be monotonic:
        s[0] = (y[1]-y[0])/(x[1]-x[0]);
    }else{
        s[0] = left_endpoint_derivative;
    }

    for (int k = 1; k < n-1; ++k) {
        double hkm1 = x[k] - x[k-1];
        double dkm1 = (y[k] - y[k-1])/hkm1;

        double hk = x[k+1] - x[k];
        double dk = (y[k+1] - y[k])/hk;
        double w1 = 2*hk + hkm1;
        double w2 = hk + 2*hkm1;
        if ( (dk > 0 && dkm1 < 0) || (dk < 0 && dkm1 > 0) || dk == 0 || dkm1 == 0){
            s[k] = 0;
        }else{
            s[k] = (w1+w2)/(w1/dkm1 + w2/dk);
        }

    }
    // Quadratic extrapolation at the other end:
    if (isnan(right_endpoint_derivative)){
                s[n-1] = (y[n-1]-y[n-2])/(x[n-1] - x[n-2]);
    }else{
                s[n-1] = right_endpoint_derivative;
    }

    auto spline = cubic_hermite<std::vector<double>>(std::move(x), std::move(y), std::move(s));

    for(int j=0; j<n_pred; j++){
        y_pred_res[j] = spline(x_pred[j]);
    }
    return 1;
}

/* Quantile inverse function for R^phi*W + epsilon*/
int qRW_me_interp(double *p, double *xp, double *surv_p, double tau_sqd, double phi, double gamma,
                  int n_p, int n_grid, double *result,
                  double *cdf_vals, double *x_vals, int n_x=400, double lower=5, double upper=20
                ){
    int tmp_int, i, zeros;
    double min_p, max_p, delta, tmp_double;
    bool large_delta_large_x = false;

    // (1) When phi is varying over space, we need to calculate one quantile for each phi value.
    // Given more accuarte [lower,upper] values for the single p, we can decrease n_x.
    if(n_p<2){ n_x=floor(100*(p[0]+0.1));}
    double x_vals1[n_x+150];
    double cdf_vals1[n_x+150];

    // (2) Generate x_vals and cdf_vals to interpolate
    if(isnan(x_vals[0])){
        double x_range[2];
        max_p = *std::max_element(p, p+n_p);
        min_p = *std::min_element(p, p+n_p);
        tmp_int = find_xrange_pRW_me_C(min_p, max_p, lower, upper, xp, surv_p, tau_sqd,
                                       phi, gamma, n_grid, x_range);
        if (isinf(x_range[1])){   // Upper is set to 20 initially
            x_range[1] = std::pow(10,20);
            large_delta_large_x = true;
        }
        if (x_range[0]<=0){
            delta = (0.0001 - x_range[0]) / 149;
            for(i=0; i<150; i++){
                x_vals1[i] = x_range[0] + delta * i;
            }
            delta = (log(x_range[1])-log(0.0001001)) / (n_x-1);
            for(i=0; i<n_x; i++){
                x_vals1[i+150] = exp(log(0.0001001) + delta * i);
            }
            n_x=150+n_x;
        }else{
            delta = (log(x_range[1])-log(x_range[0])) / (n_x-1);
            for(i=0; i<n_x; i++){
                x_vals1[i] = exp(log(x_range[0]) + delta * i);
            }
        }
        tmp_int = pRW_me_interp_C(x_vals1, xp, surv_p, tau_sqd, phi, gamma, n_x, n_grid, cdf_vals1);
    }else{
        double x_vals1[n_x];
        double cdf_vals1[n_x];
        memcpy(x_vals1, x_vals, n_x*sizeof(double));
        if(isnan(cdf_vals[0])){
            tmp_int = pRW_me_interp_C(x_vals1, xp, surv_p, tau_sqd, phi, gamma, n_x, n_grid, cdf_vals1);
        }else{
            memcpy(cdf_vals1, cdf_vals, n_x*sizeof(double));
        }
    }

    // (3) Obtain the quantile level using the interpolated function
    if(!large_delta_large_x){
        zeros = 0;
        tmp_double = cdf_vals1[zeros];
        while (tmp_double==0) {
            tmp_double = cdf_vals1[++zeros];
        }
        tmp_int = pchip(cdf_vals1+zeros, x_vals1+zeros, p, result, n_x-zeros, n_p);
    }else{
        int exceeds = 0;
        for (i=0;i<n_p;i++){
            if(p[i]>cdf_vals1[n_x-1]) exceeds+=1;
        }
        tmp_int = pchip(cdf_vals1, x_vals1, p, result, n_x, n_p-exceeds);
        for (i=n_p-exceeds;i<n_p;i++){
            result[i] = x_vals[n_x-1];
        }
    }
    return 1;
}

/* Density function for R^phi*W */
int RW_density_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*pow(xval[i],2));
        result[i] = (boost::math::tgamma((long double)(a+1),tmp1)-pow(tmp1,a)*exp(-tmp1))*tmp0;
    }
    return 1;
}

/* Marginal density function for R^phi*W + epsilon */
int dRW_me_interp_C(double *xval, double *xp, double *den_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    double thresh_large = 820;
    if(tau_sqd < 1) {
        thresh_large = 50;
    }
    bool tau_bool = (tau_sqd > 0.05);

    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<thresh_large)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * den_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = tmp_sum/sd_const_pi;
            tmp_sum = 0;
            result[i] = tmp_res;
        }else if((tau_bool & (xval[i]>=thresh_large))|(!tau_bool & (xval[i]>0))){
            tmp_int = RW_density_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = tmp_res;
        }else{
            result[i] = 0;
        }
    }

    return 1;
}

int density_interp_grid(double *xp, double *phi, double gamma, int n_phi, int n_grid, double *Den, double *Surv){
    int counter = 0;
    int i,j, tmp_int;
    double tmp_surv, tmp_den;
    double tmp2, tmp1, tmp0, a, tmp_incomp, tmp_phi, tmp_phi_inv, tmp_xp;
    double gamma_half = gamma/2;

    for(i=0; i<n_phi; i++){
        tmp_phi = phi[i];
        a = 0.5-tmp_phi;
        tmp2 = std::pow(gamma_half, tmp_phi)/(a*sqrt(M_PI));
        tmp_phi_inv = 1/tmp_phi;
        for(j=0; j<n_grid; j++){
            tmp_xp = xp[j];
            tmp1 = gamma_half/std::pow(tmp_xp,tmp_phi_inv);
            tmp0 = tmp2/tmp_xp;
            tmp_incomp = (boost::math::tgamma((long double)(a+1),tmp1)-std::pow(tmp1,a)*exp(-tmp1))*tmp0;
            Surv[counter] = boost::math::gamma_p(0.5L,tmp1) + tmp_incomp;
            Den[counter++] = tmp_incomp/tmp_xp;
        }
    }
    return 1;
}

double dgev_C(double y, double loc, double scale, double shape, bool log_out){
    double t = std::pow(1+shape*((y-loc)/scale), -1/shape);
    double result;
    if(log_out){
        result = -log(scale)+(shape+1)*log(t)-t;
    }else{
        result = std::pow(t, shape+1)*exp(-t)/scale;
    }
    return result;
}

double dnorm_C(double y, double mean, double sd, bool log_out){
    double t=(y-mean)/sd;
    double result;
    if(log_out){
        result = -0.5*log(2*M_PI)-log(sd)-0.5*t*t;
    }else{
        result = exp(-0.5*t*t)/(sqrt(2*M_PI)*sd);
    }
    return result;
}

/* Thresh_X and Thresh_X_above are required */
/* xp, den_p and surv_p are required */
/* Calculate column_wise in C order OR one time  */
double marg_transform_data_mixture_me_likelihood_C(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int, Den_lookup;

    for (i=0; i<n_s; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            Den_lookup = i*n_grid;
            tmp_int = dRW_me_interp_C(&X[i], xp, &Den[Den_lookup], tau_sqd, phi[i], gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate row_wise in F order OR one location */
double marg_transform_data_mixture_me_likelihood_F(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double phi, double gamma,
                                                double *xp, double *den_p, int n_t, int n_grid){
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double ll=0;
    double RW_den;
    int i, tmp_int;

    for (i=0; i<n_t; i++){
        if(cen[i]){
            ll += log(0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else if(cen_above[i]){
            ll += log(1-0.5*erfc(-(X[i]-X_s[i])/sd_const));
        }else{
            tmp_int = dRW_me_interp_C(&X[i], xp, den_p, tau_sqd, phi, gamma, 1, n_grid, &RW_den);
            ll += dnorm_C(X[i], X_s[i], sd, true)+dgev_C(Y[i], Loc[i], Scale[i], Shape[i], true)-log(RW_den);
        }
    }
    return ll;
}

/* Calculate all locations and all times */
double marg_transform_data_mixture_me_likelihood_global(double *Y, double *X, double *X_s, bool *cen, bool *cen_above,
                                                double *Loc, double *Scale, double *Shape,
                                                double tau_sqd, double *phi, double gamma,
                                                double *xp, double *Den, int n_s, int n_t, int n_grid){

    double ll=0;
    int site, tmp_int, Den_lookup, X_lookup;

    for (site=0; site<n_s; site++){
        X_lookup = site*n_t;
        Den_lookup = site*n_grid;
        ll += marg_transform_data_mixture_me_likelihood_F(&Y[X_lookup], &X[X_lookup], &X_s[X_lookup], &cen[X_lookup], &cen_above[X_lookup], &Loc[X_lookup], &Scale[X_lookup], &Shape[X_lookup], tau_sqd, phi[site], gamma, xp, &Den[Den_lookup], n_t, n_grid);
    }
    return ll;
}

/* Calculate thresh_X and thresh_X_above for an array of phi values */
// First way: iterate through each prob at each location (Not efficient)
int Thresh_X_try(double *phi, double *xp, double *Surv, double prob_below, double prob_above, double tau_sqd, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, double *Thresh_X, double *Thresh_X_above){
    int Surv_lookup, tmp_int;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    double tmp_res;
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        tmp_int = qRW_me_interp(&prob_below, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          1, n_grid, &tmp_res,
                          cdf_vals, x_vals, n_x, below_approx-10, below_approx+10);
        Thresh_X[i] = tmp_res;
        tmp_int = qRW_me_interp(&prob_above, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          1, n_grid, &tmp_res,
                          cdf_vals, x_vals, n_x, above_approx-100, above_approx+200);
        Thresh_X_above[i] = tmp_res;
    }

    return 1;
}

// Second way: calculate probs altogether at each location
int X_update(double *phi, double *xp, double *Surv, double *probs, double tau_sqd, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, int n_probs, double *X_res){
    int Surv_lookup, tmp_int, X_lookup;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        X_lookup = i*n_probs;
        tmp_int = qRW_me_interp(probs, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          n_probs, n_grid, X_res + X_lookup,
                          cdf_vals, x_vals, n_x, below_approx-10, above_approx+200);
    }

    return 1;
}

int unifs_2_RW_me(double *unifs, double *xp, double *Surv, double tau_sqd, double *phi, double gamma,
                    double below_approx, double above_approx,
                    int n_phi, int n_grid, int n_probs_each_loc, double *X_res){
    int Surv_lookup, tmp_int, X_lookup;
    int n_x=100;
    double cdf_vals[n_x], x_vals[n_x];
    cdf_vals[0] = std::numeric_limits<double>::quiet_NaN(); x_vals[0] = std::numeric_limits<double>::quiet_NaN();

    for (int i = 0; i<n_phi;i++){
        Surv_lookup = i*n_grid;
        X_lookup = i*n_probs_each_loc;
        tmp_int = qRW_me_interp(unifs + X_lookup, xp, &Surv[Surv_lookup], tau_sqd, phi[i], gamma,
                          n_probs_each_loc, n_grid, X_res + X_lookup,
                          cdf_vals, x_vals, n_x, below_approx-10, above_approx+200);
    }

    return 1;
}

void print_c(double *Y, int n_grid){
    printf("%4.2f %4.2f\n",*Y,*(Y+n_grid-1));
}

double print_Vec(double *Y, int n_grid, int n_s){
    int Den_lookup;
    for(int i=0; i<n_s; i++){
        Den_lookup = i*n_grid;
        print_c(&Y[Den_lookup], n_grid);
    }

    return Y[0];
}

}


//
// ConfusionMatrix.cpp
// Represents a confusion matrix for multi-class evaluation
//

#include "evaluation/ConfusionMatrix.hpp"

#include <algorithm>

#ifdef __APPLE__
    #include <eigen3/Eigen/Dense>
#else
    #include <Eigen/Dense>
#endif

namespace Evaluation
{
    // Constructor
    ConfusionMatrix::ConfusionMatrix(const std::vector<std::string> &classLabels) : m_ClassLabels(classLabels), m_Matrix(classLabels.size(), classLabels.size())
    {
        // initialise matrix
        for (size_t i = 0; i < m_ClassLabels.size(); i++)
        {
            for (size_t j = 0; j < m_ClassLabels.size(); j++) {
                m_Matrix(i, j) = 0;
            }
        }
    }

    // Destructor
    ConfusionMatrix::~ConfusionMatrix() = default;

    void ConfusionMatrix::AddClassifications(const std::string &predictedClass, const std::string &actualClass, int count)
    {
        int x = IndexOfClass(predictedClass);
        int y = IndexOfClass(actualClass);

        m_Matrix(y, x) = m_Matrix(y, x) + count;
    }

    // Get the index of class label
    int ConfusionMatrix::IndexOfClass(const std::string& classLabel) const
    {
        auto iter = std::find(m_ClassLabels.begin(), m_ClassLabels.end(), classLabel);
        if (iter != m_ClassLabels.end()) {
            return static_cast<int>(std::distance(m_ClassLabels.begin(), iter));
        }
        else {
            return -1;
        }
    }

    // Precision
    float ConfusionMatrix::GetPrecision(const std::string& classLabel) const
    {
        int index = IndexOfClass(classLabel);
        if (index < 0) {
            return 0.0;
        }

        float truePositives = m_Matrix(index, index);
        float total = m_Matrix.colwise().sum()(0);

        if (total == 0.0) {
            total = 1.0;
        }

        return (truePositives / total);
    }

    // Recall
    float ConfusionMatrix::GetRecall(const std::string &classLabel) const
    {
        int index = IndexOfClass(classLabel);
        if (index < 0) {
            return 0.0;
        }

        float truePositives = m_Matrix(index, index);

        float total = m_Matrix.rowwise().sum()(0);
        if (total == 0.0) {
            total = 1.0;
        }

        return (truePositives / total);
    }

    // Accuracy
    float ConfusionMatrix::GetAccuracy() const
    {
        float correct = m_Matrix.diagonal().sum();
        float total = m_Matrix.sum();

        if (total == 0) {
            total = 1.0;
        }

        return (correct / total);
    }

    // Output stream
    std::ostream& operator<<(std::ostream& os, const ConfusionMatrix& cm)
    {
        // output headers
        os << ",";
        for (size_t i = 0; i < cm.m_ClassLabels.size(); i++)
        {
            os << cm.m_ClassLabels[i];
            if (i < cm.m_ClassLabels.size() - 1) {
                os << ",";
            }
        }
        os << std::endl;

        // output confusion matrix
        for (int i = 0; i < cm.m_ClassLabels.size(); i++)
        {
            os << cm.m_ClassLabels[i] << ",";
            for (int j = 0; j < cm.m_ClassLabels.size(); j++)
            {
                os << cm.m_Matrix(i, j);
                if (j < cm.m_ClassLabels.size() - 1) {
                    os << ",";
                }
            }
            os << std::endl;
        }

        return os;
    }

    // Append other confusion matrix data
    ConfusionMatrix& ConfusionMatrix::operator+=(const Evaluation::ConfusionMatrix &other) {
        this->m_Matrix += other.m_Matrix;
        return *this;
    }
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing max capabilities

    Computes the largest of its parameter.

    @par semantic:
    For any given value @c x and @c y of type @c T:

    @code
    T r = max(x, y);
    @endcode

    is similar to:

    @code
    T r =  (x > y) ? x : y;
    @endcode

    @par Note:

    With this definition max(x, @ref Nan) should return x...

    On some systems (namely for example vmx in simd mode) the intrinsic used returns Nan as soon x or y is a nan.
    So the real definition of our max function must add: but if y is Nan the result is system dependent.

    This can be corrected using the conformant_ decorator that ensures the standard behaviour at a cost.

    @see maxnum, maxnummag,  maxmag

  **/
  Value max(Value const& x, Value const& y);
} }
#endif

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

#endif


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__DIFF_HPP_
#define SMOOTH__DIFF_HPP_

/**
 * @file
 * @brief Differentiation on Manifolds.
 */

#include <Eigen/Core>
#include <type_traits>

#include "internal/utils.hpp"
#include "manifold.hpp"
#include "wrt.hpp"

namespace smooth {

// differentiation module
namespace diff {
namespace detail {

/**
 * @brief Numerical first-order differentiation in tangent space.
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K = 1>
  requires(K >= 1 && K <= 2)
auto dr_numerical(auto && f, auto && x)
{
  using Wrt    = decltype(x);
  using Result = decltype(std::apply(f, x));
  using Scalar = ::smooth::Scalar<Result>;

  static constexpr auto NumArgs = std::tuple_size_v<std::decay_t<Wrt>>;

  static_assert(Manifold<Result>, "f(x) is not a Manifold");

  const Scalar eps = std::sqrt(Eigen::NumTraits<Scalar>::epsilon());

  // arguments are modified below, so we create a copy of those that come in as const
  auto x_nc = wrt_copy_if_const(std::forward<Wrt>(x));
  Result F  = std::apply(f, x_nc);

  // static sizes
  static constexpr Eigen::Index Nx = wrt_Dof<Wrt>();
  static constexpr Eigen::Index Ny = Dof<Result>;

  // dynamic sizes
  Eigen::Index nx = std::apply([](auto &&... args) { return (dof(args) + ...); }, x_nc);
  Eigen::Index ny = dof<Result>(F);

  // output variable
  Eigen::Matrix<Scalar, Ny, Nx> J(ny, nx);

  if constexpr (K == 1) {
    Eigen::Index I0 = 0;
    utils::static_for<NumArgs>([&](auto i) {
      auto & w = std::get<i>(x_nc);
      using W  = std::decay_t<decltype(w)>;

      static constexpr Eigen::Index Nx_j = Dof<W>;
      const int nx_j                     = dof<W>(w);

      for (auto j = 0; j != nx_j; ++j) {
        Scalar eps_j = eps;
        if constexpr (std::is_base_of_v<Eigen::MatrixBase<W>, W>) {
          // scale step size if we are in Rn
          eps_j *= abs(w[j]);
          if (eps_j == 0.) { eps_j = eps; }
        }
        w             = rplus<W>(w, (eps_j * Eigen::Vector<Scalar, Nx_j>::Unit(nx_j, j)).eval());
        J.col(I0 + j) = rminus<Result>(std::apply(f, x_nc), F) / eps_j;
        w             = rplus<W>(w, (-eps_j * Eigen::Vector<Scalar, Nx_j>::Unit(nx_j, j)).eval());
      }
      I0 += nx_j;
    });

    return std::make_pair(std::move(F), std::move(J));
  }

  if constexpr (K == 2) {
    static_assert(Ny == 1, "2nd derivative only implemented for scalar functions");

    const auto sqrteps = std::sqrt(eps);

    Eigen::Matrix<Scalar, Nx, Nx> H(nx, nx);

    Eigen::Index I0 = 0;
    utils::static_for<NumArgs>([&](auto i0) {
      auto & w0                           = std::get<i0>(x_nc);
      using W0                            = std::decay_t<decltype(w0)>;
      static constexpr Eigen::Index Nx_i0 = Dof<W0>;
      const int nx_i0                     = dof<W0>(w0);

      Eigen::Index I1 = 0;
      utils::static_for<NumArgs>([&](auto i1) {
        if (i1 > i0) { return; }

        auto & w1                           = std::get<i1>(x_nc);
        using W1                            = std::decay_t<decltype(w1)>;
        static constexpr Eigen::Index Nx_i1 = Dof<W1>;
        const int nx_i1                     = dof<W1>(w1);

        for (auto k0 = 0; k0 != nx_i0; ++k0) {
          Scalar eps0 = sqrteps;
          if constexpr (std::is_base_of_v<Eigen::MatrixBase<W0>, W0>) {
            eps0 *= abs(w0[k0]);
            if (eps0 == 0.) { eps0 = sqrteps; }
          }

          w0               = rplus<W0>(w0, eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
          const Result F10 = std::apply(f, x_nc);
          w0               = rplus<W0>(w0, -eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));

          J(0, I0 + k0) = (F10 - F) / eps0;

          for (auto k1 = 0; k1 < (i0 == i1 ? k0 + 1 : nx_i1); ++k1) {
            Scalar eps1 = sqrteps;
            if constexpr (std::is_base_of_v<Eigen::MatrixBase<W1>, W1>) {
              eps1 *= abs(w1[k1]);
              if (eps1 == 0.) { eps1 = sqrteps; }
            }

            // do this in order to ensure we return to same point on spaces with non-zero brackets
            w1               = rplus<W1>(w1, eps1 * Eigen::Vector<Scalar, Nx_i1>::Unit(nx_i1, k1));
            const Result F01 = std::apply(f, x_nc);
            w0               = rplus<W0>(w0, eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
            const Result F11 = std::apply(f, x_nc);
            w0               = rplus<W0>(w0, -eps0 * Eigen::Vector<Scalar, Nx_i0>::Unit(nx_i0, k0));
            w1               = rplus<W1>(w1, -eps1 * Eigen::Vector<Scalar, Nx_i1>::Unit(nx_i1, k1));

            // hessian is symmetric
            H(I0 + k0, I1 + k1) = H(I1 + k1, I0 + k0) = (F11 - F01 - F10 + F) / eps0 / eps1;
          }
        }
        I1 += nx_i1;
      });
      I0 += nx_i0;
    });

    return std::make_tuple(std::move(F), std::move(J), std::move(H));
  }
}

}  // namespace detail

/**
 * @brief Available differentiation methods
 */
enum class Type {
  Numerical,  ///< Numerical (forward) derivatives
  Autodiff,   ///< Uses the autodiff (https://autodiff.github.io) library; requires  \p
              ///< compat/autodiff.hpp
  Ceres,      ///< Uses the Ceres (http://ceres-solver.org) built-in autodiff; requires \p
              ///< compat/ceres.hpp
  Analytic,   ///< Hand-coded derivative, requires that function returns \p std::pair \f$(f(x),
              ///< \mathrm{d}^r f_x) \f$
  Default     ///< Automatically select type based on availability
};

static constexpr Type DefaultType =
#ifdef SMOOTH_DIFF_AUTODIFF
  Type::Autodiff;
#elif defined SMOOTH_DIFF_CERES
  Type::Ceres;
#else
  Type::Numerical;
#endif

/**
 * @brief Differentiation in tangent space
 *
 * @tparam K differentiation order (1 or 2)
 * @tparam D differentiation method to use
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return {f(x), dr f(x)} for K = 1, {f(x), dr f(x), d2r f(x)} for K = 2
 *
 * @note Only scalar functions suppored for K = 2
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K, Type D>
auto dr(auto && f, auto && x)
{
  using F   = decltype(f);
  using Wrt = decltype(x);

  if constexpr (D == Type::Numerical) {
    return detail::dr_numerical<K>(std::forward<F>(f), std::forward<Wrt>(x));
  } else if constexpr (D == Type::Autodiff) {
#ifdef SMOOTH_DIFF_AUTODIFF
    return dr_autodiff<K>(std::forward<F>(f), std::forward<Wrt>(x));
#else
    static_assert(D != Type::Autodiff, "compat/autodiff.hpp header not included");
#endif
  } else if constexpr (D == Type::Ceres) {
    static_assert(K == 1, "Only K = 1 supported with Ceres");
#ifdef SMOOTH_DIFF_CERES
    return dr_ceres(std::forward<F>(f), std::forward<Wrt>(x));
#else
    static_assert(D != Type::Ceres, "compat/ceres.hpp header not included");
#endif
  } else if constexpr (D == Type::Analytic) {
    auto F  = std::apply(f, x);
    auto dF = std::apply(std::bind_front(std::mem_fn(&std::decay_t<decltype(f)>::jacobian), f), x);
    if constexpr (K == 1) {
      return std::make_tuple(std::move(F), std::move(dF));
    } else if constexpr (K == 2) {
      auto d2F =
        std::apply(std::bind_front(std::mem_fn(&std::decay_t<decltype(f)>::hessian), f), x);
      return std::make_tuple(F, dF, d2F);
    }
  } else if constexpr (D == Type::Default) {
    return dr<K, DefaultType>(std::forward<F>(f), std::forward<Wrt>(x));
  }
}

/**
 * @brief Differentiation in tangent space using default method
 *
 * @tparam K differentiation order
 *
 * @param f function to differentiate
 * @param x reference tuple of function arguments
 * @return \p std::pair containing value and right derivative: \f$(f(x), \mathrm{d}^r f_x)\f$
 *
 * @note All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<std::size_t K = 1>
auto dr(auto && f, auto && x)
{
  return dr<K, Type::Default>(std::forward<decltype(f)>(f), std::forward<decltype(x)>(x));
}

}  // namespace diff
}  // namespace smooth

#endif  // SMOOTH__DIFF_HPP_


#pragma once

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <sensor_msgs/Imu.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <std_msgs/Float64MultiArray.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include "scp_planner.hpp"
#include <mission.hpp>
#include <param.hpp>

namespace SwarmPlanning {
    class SCPPublisher {
    public:
        SCPPublisher(const ros::NodeHandle &_n,
                     std::shared_ptr<SCPPlanner> _SCPPlanner_obj,
                     Mission _mission,
                     Param _param)
                : n(_n),
                  SCPPlanner_obj(_SCPPlanner_obj),
                  mission(_mission),
                  param(_param) {
            N = round(SCPPlanner_obj->msgs_traj_info.data[0]);
            K = round(SCPPlanner_obj->msgs_traj_info.data[1]);
            h = SCPPlanner_obj->msgs_traj_info.data[2];
            outdim = 3;

            traj_pubs.resize(N);
            msgs_traj.resize(N);
            for (int qi = 0; qi < N; qi++) {
                std::string mav_name = "/mav" + std::to_string(qi);
                traj_pubs[qi] = n.advertise<nav_msgs::Path>("/desired_trajectory" + mav_name, 1);
            }
            colBox_pub = n.advertise<visualization_msgs::MarkerArray>("/collision_model", 1);

            p_curr.resize(N);
            build_mapping_mtx();
            std::vector<double> data = SCPPlanner_obj->msgs_traj_input.data;
            u = Eigen::Map<Eigen::MatrixXd>(data.data(), outdim * N * K, 1);
            p = P * u + p_start;
            v = V * u;
            a = A * u;
        }

        void update(double current_time) {
            update_traj(current_time);
            update_colBox();
        }

        void publish() {
            for (int qi = 0; qi < N; qi++) {
                traj_pubs[qi].publish(msgs_traj[qi]);
            }
            colBox_pub.publish(msgs_colBox);
        }

    private:
        ros::NodeHandle n;
        std::shared_ptr<SCPPlanner> SCPPlanner_obj;
        Mission mission;
        Param param;

        int N, K, outdim;
        double h;
        tf::TransformBroadcaster br;
        Eigen::MatrixXd P, V, A, J, p_start, p_goal, u, p, v, a;
        std::vector<Eigen::MatrixXd> p_curr;

        std::vector<ros::Publisher> traj_pubs;
        ros::Publisher colBox_pub;

        std::vector<nav_msgs::Path> msgs_traj;
        visualization_msgs::MarkerArray msgs_colBox;

        void build_mapping_mtx() {
            P = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // position matrix p = Pu + p_start
            V = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // velocity matrix v = Vu, assume v_start = 0
            A = Eigen::MatrixXd::Identity(outdim * N * K, outdim * N * K); // accelation matrix a = Au
            J = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // accelation matrix a = Au

            p_start = Eigen::MatrixXd::Zero(outdim * N * K, 1);
            p_goal = Eigen::MatrixXd::Zero(outdim * N, 1);

            for (int dim = 0; dim < outdim; dim++) {
                for (int qi = 0; qi < N; qi++) {
                    int offset = dim * N * K + qi * K;
                    for (int k = 0; k < K; k++) {
                        for (int j = 0; j < k; j++) {
                            P(offset + k, offset + j) = 0.5 * h * h * (2 * (k - j) - 1);
                            V(offset + k, offset + j) = h;
                        }
                        if (k != 0) {
                            J(offset + k, offset + k) = 1 / h;
                            J(offset + k, offset + k - 1) = -1 / h;
                        }

                        p_start(offset + k, 0) = mission.startState[qi][dim];
                    }
                    p_goal(dim * N + qi, 0) = mission.goalState[qi][dim];
                }
            }
        }

        void update_traj(double current_time) {
            int k = floor(current_time / h);
            if (k >= K - 1) {
                return;
            }

            for (int qi = 0; qi < N; qi++) {
                msgs_traj[qi].header.frame_id = "/world";
                msgs_traj[qi].header.stamp.sec = current_time;

                p_curr[qi] = Eigen::MatrixXd::Zero(outdim, 1);
                Eigen::MatrixXd p_0 = Eigen::MatrixXd::Zero(outdim, 1);
                Eigen::MatrixXd p_1 = Eigen::MatrixXd::Zero(outdim, 1);
                for (int dim = 0; dim < outdim; dim++) {
                    p_0(dim, 0) = p(dim * N * K + qi * K + k, 0);
                    p_1(dim, 0) = p(dim * N * K + qi * K + k + 1, 0);
                }
                p_curr[qi] = p_0 + (current_time - k * h) / h * (p_1 - p_0);

                geometry_msgs::PoseStamped pos_des;
                pos_des.header.frame_id = "/world";
                pos_des.pose.position.x = p_curr[qi](0, 0);
                pos_des.pose.position.y = p_curr[qi](1, 0);
                pos_des.pose.position.z = p_curr[qi](2, 0);
                msgs_traj[qi].poses.emplace_back(pos_des);

                tf::Transform transform;
                transform.setOrigin(
                        tf::Vector3(pos_des.pose.position.x, pos_des.pose.position.y, pos_des.pose.position.z));
                tf::Quaternion q;
                q.setRPY(0, 0, 0);
                transform.setRotation(q);
                br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                      "world", "/mav" + std::to_string(qi) + "/base_link"));
            }
        }

        // obstacle-collision model
        void update_colBox() {
            visualization_msgs::MarkerArray mk_array;
            for (int qi = 0; qi < N; qi++) {
                visualization_msgs::Marker mk;
                mk.header.frame_id = "world";
                mk.ns = "colBox";
                mk.type = visualization_msgs::Marker::SPHERE;
                mk.action = visualization_msgs::Marker::ADD;

                mk.pose.orientation.x = 0;
                mk.pose.orientation.y = 0;
                mk.pose.orientation.z = 0;
                mk.pose.orientation.w = 1.0;

                mk.id = qi;
                mk.pose.position.x = p_curr[qi](0, 0);
                mk.pose.position.y = p_curr[qi](1, 0);
                mk.pose.position.z = p_curr[qi](2, 0);

                mk.scale.x = 2 * mission.quad_size[qi];
                mk.scale.y = 2 * mission.quad_size[qi];
                mk.scale.z = 2 * mission.quad_size[qi];

                mk.color.a = 0.7;
                mk.color.r = param.color[qi][0];
                mk.color.g = param.color[qi][1];
                mk.color.b = param.color[qi][2];

                mk_array.markers.emplace_back(mk);

                mk.ns = "initTraj";
                for (int k = 0; k < K; k++) {
                    mk.id = 1000 + qi * K + k;
                    Eigen::MatrixXd picker_i = position_picker(qi, k);
                    Eigen::MatrixXd p_i = picker_i * p;
                    mk.pose.position.x = p_i(0, 0);
                    mk.pose.position.y = p_i(1, 0);
                    mk.pose.position.z = p_i(2, 0);

                    mk.scale.x = 0.1;
                    mk.scale.y = 0.1;
                    mk.scale.z = 0.1;

                    mk.color.a = 0.5;
                    mk.color.r = param.color[qi][0];
                    mk.color.g = param.color[qi][1];
                    mk.color.b = param.color[qi][2];

                    mk_array.markers.emplace_back(mk);
                }
            }
            msgs_colBox = mk_array;
        }

        Eigen::MatrixXd position_picker(int qi, int k) {
            Eigen::MatrixXd P_pick = Eigen::MatrixXd::Zero(outdim, outdim * N * K);
            for (int dim = 0; dim < outdim; dim++) {
                P_pick(dim, dim * N * K + qi * K + k) = 1;
            }

            return P_pick;
        }
    };
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < typename A0, typename sA0>
    struct expm1_kernel;

    template < typename A0 >
    struct expm1_kernel < A0, float >
    {
      // computes expm1 for float or float vectors
      static BOOST_FORCEINLINE A0 expm1(const A0& a0) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        using s_t = bd::scalar_of_t<A0>;
        A0 k  = nearbyint(Invlog_2<A0>()*a0);
        A0 x = fnms(k, Log_2hi<A0>(), a0);
        x =  fnms(k, Log_2lo<A0>(), x);
        A0 hx  = x*Half<A0>();
        A0 hxs = x*hx;
        A0 r1 = horn<A0,
                     0X3F800000UL,// 1
                     0XBD08887FUL, // -3.3333298E-02
                     0X3ACF6DB4UL  // 1.5825541E-03
                     > (hxs);
        A0 t  = fnms(r1, hx, A0(3));
        A0 e  = hxs*((r1-t)/(A0(6) - x*t));
        e  = fms(x, e, hxs);
        i_t ik =  toint(k);
        A0 two2mk = bitwise_cast<A0>(shift_left(Maxexponent<A0>()-ik,Nbmantissabits<s_t>()));
        A0 y = oneminus(two2mk)-(e-x);
        return ldexp(y, ik);
      }
    };

    template < typename A0 >
    struct expm1_kernel < A0, double >
    {
      // computes expm1 for  double or double vectors
      static  BOOST_FORCEINLINE A0 expm1(const A0& a0) BOOST_NOEXCEPT
      {
        using i_t = bd::as_integer_t<A0>;
        using s_t = bd::scalar_of_t<A0>;
        A0 k  = nearbyint(Invlog_2<A0>()*a0);
        A0 hi = fnms(k, Log_2hi<A0>(), a0);
        A0 lo = k*Log_2lo<A0>();
        A0 x  = hi-lo;
        A0 hxs = sqr(x)*Half<A0>();
        A0 r1 = horn<A0,
                     0X3FF0000000000000ULL,
                     0XBFA11111111110F4ULL,
                     0X3F5A01A019FE5585ULL,
                     0XBF14CE199EAADBB7ULL,
                     0X3ED0CFCA86E65239ULL,
                     0XBE8AFDB76E09C32DULL
                     > (hxs);
        A0 t  = A0(3)-r1*Half<A0>()*x;
        A0 e  = hxs*((r1-t)/(A0(6) - x*t));
        A0 c = (hi-x)-lo;
        e  = (x*(e-c)-c)-hxs;
        i_t ik =  toint(k);
        A0 two2mk = bitwise_cast<A0>(shift_left(Maxexponent<A0>()-ik,Nbmantissabits<s_t>()));
        A0 ct1= oneminus(two2mk)-(e-x);
        A0 ct2= inc((x-(e+two2mk)));
        A0 y = if_else((k < A0(20)),ct1,ct2);
        return ldexp(y, ik);
      }
    };
  }
} }
#endif