//
//  main.cpp
//  HRMeasure
//
//  Created by WuH on 2017/6/26.
//  Copyright © 2017年 WuH. All rights reserved.
//
#include "opencv2/core/core_c.h"
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_io.h>
#include <dlib/image_processing.h>
#include <dlib/opencv/cv_image.h>

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <queue>
#include <cmath>
#include <time.h>
#include <chrono>
#include <pthread.h>
#include <armadillo>
#include <itpp/signal/fastica.h>
#include <stdlib.h>
#include "matplotlibcpp.h"

using namespace dlib;
using namespace cv;
using namespace std;
using namespace std::chrono;
using namespace arma;
using namespace itpp;
namespace plt = matplotlibcpp;

struct Frame {
public:
    cv::Mat frame;
    double time;
};

struct ROI {
public:
    cv::Mat roi;
    double time;
};

#define MAXT 201
milliseconds start_time;
bool isCalculating;
bool isDetecting;
pthread_t cal_thread;
pthread_t roi_thread;
std::queue<Frame> frames;

CascadeClassifier faces_cascade;

struct RGB_color
{
public:
    double r, g, b;
    RGB_color() {}
    RGB_color(double _r,double _g, double _b):r(_r), g(_g), b(_b) {}
    RGB_color(const RGB_color & A) {r = A.r, g = A.g, b = A.b;}
};

class Signal
{
private:
    RGB_color sig[MAXT];
    double times[MAXT];
    int nowT, headT;
    bool flag;
public:
    Signal() {nowT = headT = 0; flag = 0;}
    /* Insert avg. color of ROI */
    void insert(const RGB_color & a, const double & time)
    {
        sig[nowT] = a;
        times[nowT] = time;
        nowT = (nowT + 1) % MAXT;
        if(nowT == headT) headT = (headT + 1) % MAXT, flag = 1;
    }
    bool full() {return flag;}
    /* Calculate BVP */
    double calc_BVP()
    {
        /* filter */
        /* undo */
        /* SOBI */
        int nows = (nowT - headT + MAXT) % MAXT;
        if(nows < 100) return 0.0;
        double sel = -10000.0;
        int sel_i = 0;
        arma::mat tmp;
        rowvec even_times = linspace<rowvec>(times[headT], times[nowT - 1], nows);
        rowvec timesv = zeros<rowvec>(nows);
        for(int i = headT, j = 0; i != nowT; i = (i + 1) % MAXT, j++) timesv(j) = times[i];
        tmp.set_size(3, nows);
        for(int i = headT, j = 0; i != nowT; i = (i + 1) % MAXT, j++)
        {
            tmp(0, j) = sig[i].r;
            tmp(1, j) = sig[i].g;
            tmp(2, j) = sig[i].b;
        }

        std::vector<double> rgbx(nows), rgby(nows);
        for (int i = 0; i < nows; i++) {
            rgbx.at(i) = i;
            rgby.at(i) = tmp(0,i);
        }

        plt::plot(rgbx, rgby);
        plt::title("rgb red");
        plt::legend();
        plt::save("/Users/apple/Desktop/testpics/rgbred.png");

        for (int i = 0; i < nows; i++) {
            rgbx.at(i) = i;
            rgby.at(i) = tmp(1,i);
        }

        plt::figure();
        plt::plot(rgbx, rgby);
        plt::title("rgb green");
        plt::legend();
        plt::save("/Users/apple/Desktop/testpics/rgbgreen.png");

        for (int i = 0; i < nows; i++) {
            rgbx.at(i) = i;
            rgby.at(i) = tmp(2,i);
        }

        plt::figure();
        plt::plot(rgbx, rgby);
        plt::title("rgb blue");
        plt::legend();
        plt::save("/Users/apple/Desktop/testpics/rgbblue.png");

        for(int i = 0; i < 3; ++i)
        {
            double avg = mean(tmp.row(i)), std = stddev(tmp.row(i));
            tmp.row(i).transform([avg, std](double x) {return (x - avg) / (std + 0.0001);});
        }
        cout << "START SOBI!" << endl;
        arma::mat H;
        SOBI(tmp, 3, 20, H);
        cout << "END SOBI!" << endl;
        arma::mat sur = H * tmp;
        cx_mat raws;
        for(int i = 0; i < 3; ++i)
        {
            rowvec tmp3;
            interp1(timesv, sur.row(i), even_times, tmp3);
            sur.row(i) = tmp3;
            raws = fft(sur.row(i));
            double tmp2 = kurt(abs(raws));
            if(tmp2 > sel)
                sel = tmp2, sel_i = i;
        }
        rowvec sor = sur.row(sel_i);
        cout << "End Select" << endl;
        /* End of SOBI */
        /* Calculate BVP */
        raws = fft(sor);

        cout<<"&&&&&&&&&"<< abs(raws) <<endl;
        double ans = 0, BVP = 0;
        for(int i = 0; i < nows / 2 + 1; ++i)
        {
            if (abs(raws(i)) > ans && ((double)i) / (even_times[nows - 1] - even_times[0]) * 60 > 40
                        && ((double)i) / (even_times[nows - 1] - even_times[0]) * 60< 200)
                ans = abs(raws(i)), BVP = ((double)i) / (even_times[nows - 1] - even_times[0]) * 60;
        }

        cout << "BVP is: " << BVP << endl;
        return BVP;
        /* End of Calculate */
    }

    /* Helper Function */
    double sqr(double x)
    {
        return x * x;
    }
    double kurt(const rowvec & A)
    {
        int n = A.size();
        double B4 = 0.0, B2 = 0.0;
        for(int i = 0; i < n; ++i)
            B4 += sqr(sqr(A(i))), B2 += sqr(A(i));
        return B4 * n / (B2 * B2) - 3;
    }

    void stdcov(const arma::mat & X, int tau, arma::mat & C)
    {
        int N = X.n_cols, m = X.n_rows;
        arma::vec m1 = zeros<arma::vec>(m), m2 = zeros<arma::vec>(m);
        arma::mat R = X.cols(0, N - tau - 1) * X.cols(tau, N - 1).t() / (N - tau);
        for(int i = 0; i < m; ++i)
        {
            m1[i] = mean(X.row(i).cols(0, N - tau - 1));
            m2[i] = mean(X.row(i).cols(tau, N - 1));
        }
        C = R - m1 * m2.t();
        C = (C + C.t()) / 2;
    }
    void joint_diag(const arma::mat & A, double jthresh, cx_mat & V, cx_mat & D)
    {
        int m = A.n_rows, nm = A.n_cols;
        arma::mat b1 = zeros<arma::mat>(3, 3), b2 = zeros<arma::mat>(3, 3);
        b1 << 1 << 0 << 0 << endr << 0 << 1 << 1 << endr << 0 << 0 << 0 << endr;
        b2 << 0 << 0 << 0 << endr << 0 << 0 << 0 << endr << 0 << -1 << 1 << endr;
        cx_mat B = cx_mat(b1, b2);
        cx_mat Bt = B.t();
        cx_mat Ip = zeros<cx_mat>(1, nm);
        cx_mat Iq = zeros<cx_mat>(1, nm);
        cx_mat g = zeros<cx_mat>(3, m);
        cx_mat G = zeros<cx_mat>(2, 2);
        arma::vec ev = zeros<arma::vec>(3);
        arma::mat vcp = zeros<arma::mat>(3, 3);
        double c = 0;
        cx_double s = 0;
        V = eye<cx_mat>(m, m);
        D = zeros<cx_mat>(m, nm);
        D.set_real(A);
        for(int encore = 1; encore;)
        {
            encore = 0;
            for(int p = 0; p < m - 1; ++p)
                for(int q = p + 1; q < m; ++q)
                {
                    cx_rowvec t1 = zeros<cx_rowvec>(nm / m), t2 = zeros<cx_rowvec>(nm / m);
                    cx_rowvec t3 = zeros<cx_rowvec>(nm / m), t4 = zeros<cx_rowvec>(nm / m);
                    for(int i = p; i < nm; i += m) t1((i - p) / m) = D(p, i), t2((i - p) / m) = D(q, i);
                    for(int i = q; i < nm; i += m) t3((i - q) / m) = D(q, i), t4((i - q) / m) = D(p, i);
                    g = join_vert(t1 - t3, join_vert(t4, t2));
                    eig_sym(ev, vcp, real((B * (g * g.t())) * B.t()));
                    arma::vec angles = vcp.col(2);
                    if(angles[0] < 0)
                        angles = angles * (-1);
                    c = sqrt(0.5 + angles[0] / 2);
                    s = cx_double(angles[1], -angles[2]) * 0.5 / c;
                    if(abs(s) > jthresh)
                    {
                        encore = 1;
                        G << c << -conj(s) << endr << s << c << endr;
                        cx_mat tmp = join_horiz(V.col(p), V.col(q)) * G;
                        V.col(p) = tmp.col(0), V.col(q) = tmp.col(1);
                        tmp = G.t() * join_vert(D.row(p), D.row(q));
                        D.row(p) = tmp.row(0), D.row(q) = tmp.row(1);
                        for(int ip = p, iq = q; ip < nm && iq < nm; ip += m, iq += m)
                        {
                            cx_colvec dip = D.col(ip), diq = D.col(iq);
                            D.col(ip) = (dip * c) + (diq * s);
                            D.col(iq) = (diq * c) - (dip * conj(s));
                        }
                    }
                }
        }
    }
    void SOBI(const arma::mat & A, int n, int num_tau, arma::mat & H)
    {
        int N = A.n_cols, m = A.n_rows;
        double tiny = 1e-8;
        arma::mat Rx, tmp;
        stdcov(A, 0, Rx);
        arma::mat uu, vv;
        arma::vec dd;
        svd(uu, dd, vv, Rx);
        arma::mat d = diagmat(dd);
        arma::mat Q = sqrtmat_sympd(pinv(d)) * uu.t();
        arma::mat z = Q * A;
        arma::mat Rz = zeros<arma::mat>(n, num_tau * n);

        for(int i = 1; i <= num_tau; ++i)
        {
            stdcov(z, i - 1, tmp);
            Rz.cols((i - 1) * n, i * n - 1) = tmp.cols(0, n - 1);
        }
        cx_mat v, d2;
        joint_diag(Rz, tiny, v, d2);
        H = real(v.t()) * Q;
    }
}HR;

Scalar roiMean(cv::Mat roi){
    Scalar avg;
    Scalar std;
    cv::meanStdDev(roi, avg, std);
    return avg;
}
cv::Mat getROI(cv::Mat img, Rect FOI) {

    Rect ROI;
    ROI.x = FOI.x+FOI.width*0.5-FOI.width*0.25/2;
    ROI.y = FOI.y+FOI.height*0.18-FOI.height*0.15/2;
    ROI.width = FOI.width*0.25;
    ROI.height = FOI.height*0.15;

    Point p1(ROI.x, ROI.y);
    Point p2(ROI.x+ROI.width, ROI.y+ROI.height);
    cv::rectangle(img, p1, p2, Scalar(255, 0, 0));

    cv::Mat ROIm = img(ROI);

    return ROIm;
}

void saveROI(ROI roi_tmp){

    double r, g, b;
    Scalar mean = roiMean(roi_tmp.roi);
    r = mean[2];
    g = mean[1];
    b = mean[0];
    HR.insert(RGB_color(r, g, b), roi_tmp.time);
}

void * calculate(void * arg) {
    pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
    while(true) {
        pthread_testcancel();
        cout << "Heart Rate" << HR.calc_BVP() << endl;
//        sleep(2);
    }
    return NULL;
}

void changeState() {
    isCalculating = !isCalculating;
}

void keyHandler(VideoCapture &cap) {
    char key = (char) cv::waitKey(10);
    if( key == 27 ) {
        pthread_cancel(roi_thread);
        if (isCalculating) {
            pthread_cancel(cal_thread);
        }
        cv::destroyAllWindows();
        cap.release();
    }
    if( key == 32 ) {
        changeState();
        if(isCalculating == true) {
            int succ = pthread_create(&cal_thread, NULL, calculate, NULL);
            if(succ == 0) {
                printf("start calculating in new thread\n");
            }
        }
        else {
            printf("stop calculating\n");
            pthread_cancel(cal_thread);
        }
    }
}

Rect getfacerect(cv::Mat frame){

    cv::Mat img = frame;
    cv::Mat img_gray;
    cv::cvtColor(img, img_gray, COLOR_BGR2GRAY);
    equalizeHist(img_gray, img_gray);

    std::vector<Rect> faces;
    faces_cascade.detectMultiScale(img_gray,faces,1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(50, 50) );

    Rect FOI = Rect(1,1,2,2);

    if(faces.size() > 1) {
        int maxsize = faces[0].area();

        FOI = faces[0];
        for ( size_t i = 1; i < faces.size(); ++i) {
            int tmp = faces[i].area();
            if( tmp > maxsize ) {
                maxsize = tmp;
                FOI = faces[i];
            }
        }
    }
    else if(faces.size() == 1) {
        FOI = faces[0];
    }

    return FOI;
}

void gettenface(Frame tenframe[]) {
    Rect facearea;
    int hasface = 0;

    for(int i = 0; i < 10; i++) {

        cv::Mat face = tenframe[i].frame;
        facearea = getfacerect(face);

        if(facearea != Rect(1,1,2,2)) {
            hasface = i;
            break;
        }
    }
    ROI roi_tmp;
    for(int i = hasface; i < 10; i++) {
        roi_tmp.time = tenframe[i].time;
        roi_tmp.roi = getROI(tenframe[i].frame, facearea);
        saveROI(roi_tmp);

        Point p1(facearea.x, facearea.y);
        Point p2(facearea.x+facearea.width, facearea.y+facearea.height);
        cv::rectangle(roi_tmp.roi, p1, p2, Scalar(255, 0, 0));

        char strchar[10];
        gcvt(roi_tmp.time, 15, strchar);
        string str = strchar;

        imwrite("/Users/apple/Library/Mobile\ Documents/com~apple~CloudDocs/Desktop/HRMeasure/roi"+str+".jpg", roi_tmp.roi);
    }
}
// In this function, take out the first frame in the queue and process it.
// Get the location of the top of eyebrow to calculate
// Call ROI calculate function in the end
ROI getRoiWithDlib(Frame frameparam) {

    cout << "getRoiWithDlib" << endl;

    dlib::frontal_face_detector face_detector = get_frontal_face_detector();

    shape_predictor sp;

    deserialize("/Users/apple/Library/Mobile\ Documents/com~apple~CloudDocs/Documents/ceca/projects/HRMeasure/shape_predictors/shape_predictor_68_face_landmarks.dat") >> sp;

    Frame frametmp = frameparam;

    cv::Mat frame = frametmp.frame;
    array2d<dlib::rgb_pixel> img;
    assign_image(img, dlib::cv_image<rgb_pixel>(frame));

    //pyramid_up(img);

    image_window win;
    win.clear_overlay();
    win.set_image(img);

    cout << "test5" <<endl;

    // Now tell the face detector to give us a list of bounding boxes
    // around all the faces in the image.
    std::vector<dlib::rectangle> dets = face_detector(img);

    cout << "test6" <<endl;

    // Choose the largest face to process
    unsigned long facenum = dets.size();

    cout << "face num" << facenum << endl;

    dlib::rectangle face;
    if(facenum > 1) {
        unsigned long maxsize = dets[0].area();

        face = dets[0];

        for(unsigned long j = 1; j < facenum; ++j) {
            unsigned long tmp = dets[j].area();
            if(tmp > maxsize) {
                maxsize = tmp;
                face = dets[j];
            }
         }
    }
    else if(facenum == 1) {
        face = dets[0];
    }

    full_object_detection shape = sp(img, face);

    dlib::point rightpoint = shape.part(20);
    dlib::point leftpoint = shape.part(23);
    Rect ROI;

    ROI.x = rightpoint.x();
    ROI.width = leftpoint.x() - rightpoint.x();

    if (rightpoint.y() <= leftpoint.y()) {
        ROI.y = 0.66 *face.top() + 0.33 * rightpoint.y();
        ROI.height = ROI.y - rightpoint.y();
    }
    else {
        ROI.y = 0.66 *face.top() + 0.33 * leftpoint.y();
        ROI.height = ROI.y - leftpoint.y();
    }

    struct ROI ROItmp;
    ROItmp.time = frametmp.time;
    ROItmp.roi = frame(ROI);

    // For debug
    Point p1(ROI.x, ROI.y);
    Point p2(ROI.x+ROI.width, ROI.y+ROI.height);
    cv::rectangle(frame, p1, p2, Scalar(255, 0, 0));
    imshow("frame2", frame);

    char strchar[10];
    gcvt(frametmp.time, 15, strchar);
    string str = strchar;

    imwrite("/Users/apple/Desktop/testpics/rois/roi"+str+".jpg", ROItmp.roi);
    // For debug

    return ROItmp;
}
void * processThreadDlib(void * arg) {
    while (frames.size() > 0) {
        cout << "frame num" << frames.size() << endl;
        Frame frame = frames.front();
        frames.pop();

        ROI roi_tmp;
        roi_tmp = getRoiWithDlib(frame);
        saveROI(roi_tmp);
    }
    return NULL;
}
void * processThreadTenFrames(void * arg) {
    while (frames.size() > 0) {
        //cout << "frame num" << frames.size() << endl;
        if(frames.size() > 10) {
            Frame tenframe[10];
            Frame tenframeout[10];
            for(int i = 0; i < 10; ++i) {
                tenframe[i] = frames.front();
                frames.pop();
            }
            gettenface(tenframe);
        }
    }
    return NULL;
}
int saveFrame() {
    VideoCapture cap(0);
    if(!cap.isOpened())
        return -1;

    while(cap.isOpened()) {
        Frame frame_in;
        cap >> frame_in.frame;

        milliseconds ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
        frame_in.time = ((double)(ms.count() - start_time.count())) / 1000;

        frames.push(frame_in);
        imshow("frame", frame_in.frame);
        keyHandler(cap);
        if (isDetecting == false) {
            isDetecting = true;
            int out_thread;
            //out_thread = pthread_create(&roi_thread, NULL, processThreadTenFrames, NULL);
            out_thread = pthread_create(&roi_thread, NULL, processThreadDlib, NULL);
            if (out_thread == 0) {
                printf("start show thread\n");
            }
        }
    }
    return 1;
}

int main(int argc, const char * argv[]) {
    start_time = duration_cast< milliseconds >(system_clock::now().time_since_epoch());
    isCalculating = false;
    isDetecting = false;

    faces_cascade.load("/Users/apple/Library/Mobile\ Documents/com~apple~CloudDocs/Documents/ceca/projects/HRMeasure/Haarcascades/haarcascade_frontalface_alt2.xml");

    if(faces_cascade.empty()) {
        printf("fail loading cascade\n");
    }

    int openVideo = saveFrame();
    if( openVideo < 0 ) {
        printf("error open camera\n");
    }
    return 0;
}

//cv::Mat getFace(cv::Mat frame_in) {
//
//
//    cv::Mat img = frame_in;
//    cv::Mat img_gray;
//    cv::cvtColor(img, img_gray, COLOR_BGR2GRAY);
//    equalizeHist(img_gray, img_gray);
//
//    vector<Rect> faces;
//    faces_cascade.detectMultiScale(img_gray,faces,1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(50, 50) );
//
//    Rect FOI;
//
//    if(faces.size() > 1) {
//        int maxsize = faces[0].area();
//
//        FOI = faces[0];
//        for ( size_t i = 1; i < faces.size(); ++i) {
//            int tmp = faces[i].area();
//            if( tmp > maxsize ) {
//                maxsize = tmp;
//                FOI = faces[i];
//            }
//        }
//    }
//    else if(faces.size() == 1) {
//        FOI = faces[0];
//    }
//
//    cv::Mat ROI = getROI(img, FOI);
//    saveROI(ROI);
//
//    Point p1(FOI.x, FOI.y);
//    Point p2(FOI.x+FOI.width, FOI.y+FOI.height);
//    rectangle(img, p1, p2, Scalar(255, 0, 0));
//
//    return img;
//}


#ifndef SKYLARK_LOCAL_COMPUTATIONS_HPP
#define SKYLARK_LOCAL_COMPUTATIONS_HPP

#include <boost/math/special_functions/bessel.hpp>

#include <unordered_map>
#include <unordered_set>
#include <queue>

extern "C" {

void EL_BLAS(sgemv)(const char*, const El::Int *, const El::Int *,
    const float *, const float *, const El::Int *,
    const float *, const El::Int *, const float *, float *, const El::Int *);

void EL_BLAS(dgemv)(const char*, const El::Int *, const El::Int *,
    const double *, const double *, const El::Int *,
    const double *, const El::Int *, const double *, double *, const El::Int *);

}

namespace skylark { namespace ml {

/**
 * Localized solution of Time-Dependent Personalized PageRank.
 *
 * For details on Time-Dependent PPR, the algorithm, the gaureentess of the
 * output, and the parameters, see:
 *
 * "Community Detection Using Time-Dependent PageRank"
 * by Haim Avron and Lior Horesh
 *
 * @tparam GraphType type of graph object. Needs to support the following:
 *                   GraphType::vertex_type - type of vertex.
 *                   GraphType::num_edges() - number of edges.
 *                   GraphType::deg(node) - debgree of a node.
 *                   GraphType::adjanct_begin(node),
 *                   GraphType::adjanct_end(node) -
 *                    begining and end iterators to adjancy container
 *                    container can be any type.
 * @tparam T datatype (e.g. double) for the function on nodes. Must be numeric.
 * @param G input graph
 * @param s seed function on nodes (i.e. map from node to numeric value).
 * @param y output function - or each node the function defines NX values
 *          for NX different time points in [0, gamma].
 * @param x NX-sized vector with the time values on which y reports the values.
 * @param alpha,gamma,epsilon, NX - parameters (see paper).
 */
template<typename GraphType, typename T>
void TimeDependentPPR(const GraphType& G,
    const std::unordered_map<typename GraphType::vertex_type, T>& s,
    std::unordered_map<typename GraphType::vertex_type, El::Matrix<T> *>& y,
    El::Matrix<T> &x, double alpha = 0.85, double gamma = 5.0,
    double epsilon = 0.001, int NX = 4) {

    typedef typename GraphType::vertex_type vertex_type;

    if (!El::Initialized())
        SKYLARK_THROW_EXCEPTION (
            base::skylark_exception()
               << base::error_msg("Elemental was not initialized") );

    // Find minimum N, caching it since it involves costly computations of
    // Bessel functions.
    static std::unordered_map<std::pair<double, double>, int,
                              utility::pair_hasher_t> Nmap;
    auto epsgamma = std::make_pair(epsilon, gamma);
    const double pi = boost::math::constants::pi<double>();
    if (Nmap.count(epsgamma) == 0) {
        int minN = 10;
        double C = 20.0 * std::sqrt(minN) * std::exp(-gamma/2);
        while (C * boost::math::cyl_bessel_i(minN, gamma) * pow(0.8, minN) >
            epsilon / (gamma * (1 + (2 / pi) * log(minN - 1))))
            minN++;
        Nmap[epsgamma] = minN;
    }
    int minN = Nmap[epsgamma];

    // N is taken to be the minimum multiple of NX that is bigger or equal
    // to minN.
    const El::Int N = minN % NX == 0 ? minN : (minN / NX + 1) * NX;
    const El::Int NR = N / NX;

    // Setup matrices associated with Chebyshev spectral diff
    // We cache them to keep costs low (can be crucial for
    // when finding the cluster is very fast).
    static std::unordered_map<std::pair<int, double>, El::Matrix<T>*,
                              utility::pair_hasher_t> Dmap;

    auto ngamma = std::make_pair(N, gamma);
    if (Dmap.count(ngamma) == 0) {
        El::Matrix<T> *D = new El::Matrix<T>(N, N);
        Dmap[ngamma] = D;

        El::Matrix<T> D0;
        nla::ChebyshevDiffMatrix(N, D0, x, 0, gamma);
        for(int i = 0; i < N; i++)
            D0.Set(i, i, D0.Get(i, i) + 1.0);

        El::Matrix<T> R(N, N);
        El::qr::Explicit(D0, R);

        for(int j = 0; j < N; j++)
            D->Set(N-1, j, D0.Get(j, N-1));

        El::Matrix<T> Q1, R1;
        base::ColumnView(Q1, D0, 0, N - 1);
        El::View(R1, R, 0, 0, N-1, N-1);

        El::Pseudoinverse(R1);

        El::Matrix<T> DU;
        base::RowView(DU, *D, 0, N-1);
        El::Gemm(El::NORMAL, El::TRANSPOSE, 1.0, R1, Q1, 0.0, DU);
    }

    const El::Matrix<T> *D_ = Dmap[ngamma];

    El::Matrix<T> x1;
    nla::ChebyshevPoints(N, x1, 0, gamma);
    x.Resize(NX, 1);
    for(int i = 0; i < NX; i++)
        x.Set(i, 0, x1.Get(i * NR, 0));

    // Constants for convergence.
    double LC = 1 + (2 / pi) * log(N - 1);
    double C = (alpha < 1) ?
        (1-alpha) * epsilon / ((1 - exp((alpha - 1) * gamma)) * LC) :
        epsilon / (gamma * LC);

    // From now on, do not use Elemental to avoid overheads.
    const T *D = D_->LockedBuffer();
    const T *u = D_->LockedBuffer() + N - 1;

    typedef std::pair<bool, T*> rypair_t;
    std::unordered_map<vertex_type, rypair_t> rymap;
    std::queue<vertex_type> violating;

    // Initialize non-zero functions, and their residual, which is not
    // fully computed yet (but we know that needs to be inserted into the queue).
    for(auto it = s.begin(); it != s.end(); it++) {
        const vertex_type &node = it->first;
        const T &v = it->second;

        T *ry = new T[N + NX];
        std::fill(ry, ry + N, -alpha * v);
        std::fill(ry + N, ry + N + NX, v);
        rymap[node] = rypair_t(true, ry);
        violating.push(node);
    }

    // Initialize to just zero for all nodes adjanct to seeds, that
    // are not seeds themselves. Residual is not fully computed yet.
    for(auto it = s.begin(); it != s.end(); it++) {
        const vertex_type &node = it->first;

        for(auto it = G.adjanct_begin(node); it != G.adjanct_end(node); it++) {
            const vertex_type &onode = *it;
            if (rymap.count(onode) == 0) {
                T *ry = new T[N + NX];
                std::fill(ry, ry + N + NX, 0);
                rymap[onode] = rypair_t(false, ry);
            }
        }
    }

    // Update the residual based on seeds
    for(auto it = s.begin(); it != s.end(); it++) {
        const vertex_type &node = it->first;

        T *ry = rymap[node].second;

        size_t deg = G.degree(node);
        T v = alpha * ry[N] / deg;
        for(auto it = G.adjanct_begin(node); it != G.adjanct_end(node); it++) {
            const vertex_type &onode = *it;
            size_t odeg = G.degree(onode);

            rypair_t& ryopair = rymap[onode];
            T *ro = ryopair.second;
            bool inq = false;
            double B = C * odeg;
            for(int j = 0; j < N; j++) {
                ro[j] += v;
                inq = inq || (std::abs(ro[j]) > B);
            }
            if (!ryopair.first && inq) {
                violating.push(onode);
                ryopair.first = true;
            }
        }
    }

    // Main loop
    T dyp[N];
    while(!violating.empty()) {
        vertex_type node = violating.front();
        violating.pop();

        // Solve locally, and update rymap[node].
        rypair_t& rpair = rymap[node];
        T *ry = rpair.second;

        // Compute correction to y, and the new residual.
        T done = 1.0, dzero = 0.0;
        El::Int ione = 1;
        if (std::is_same<T, float>::value)
            EL_BLAS(sgemv)("Normal", &N, &N, (float *)&done, (float *)D, &N,
                (float *)ry, &ione, (float *)&dzero, (float *)dyp, &ione);
        else
            EL_BLAS(dgemv)("Normal", &N, &N, (double *)&done, (double *)D, &N,
                (double *)ry, &ione, (double *)&dzero, (double *)dyp, &ione);
        for(int i = 0; i < NX; i++)
            ry[N + i] += dyp[i * NR];
        T v = dyp[N-1];
        for(int i = 0; i < N; i++)
            ry[i] = v * u[i * N];

        // No longer in queue.
        rpair.first = false;

        // Update residuals
        size_t deg = G.degree(node);
        for(auto it = G.adjanct_begin(node); it != G.adjanct_end(node); it++) {
            const vertex_type &onode = *it;
            size_t odeg = G.degree(onode);

            // Add it to rymap, if not already there.
            if (rymap.count(onode) == 0) {
                T *rynew = new T[N + NX];
                std::fill(rynew, rynew + N + NX, 0);
                rymap[onode] = rypair_t(false, rynew);
            }

            rypair_t& ryopair = rymap[onode];
            bool inq = false;
            T *ryo = ryopair.second;
            T c = alpha / deg;
            double B = C * odeg;
            for(int i = 0; i < N - 1; i++) {
                ryo[i] += c *  dyp[i];
                inq = inq || (std::abs(ryo[i]) > B);
            }
            inq = inq || (std::abs(ryo[N - 1]) > B);
            if (!ryopair.first && inq) {
                violating.push(onode);
                ryopair.first = true;
            }
        }
    }

    // Yank values to y, freeing other parts of ry in the process.
    y.clear();
    for(auto it = rymap.begin(); it != rymap.end(); it++) {
        if (it->second.second[N] != 0) {
            El::Matrix<T> *yv = new El::Matrix<T>(NX, 1);
            for(int i = 0; i < NX; i++)
                yv->Set(i, 0, it->second.second[N + i]);
            y[it->first] = yv;
        }
        delete it->second.second;
    }
}

/**
 * Find a local cluster in a graph using a set of seed nodes.
 *
 * Based on the algorithm in
 * "Community Detection Using Time-Dependent PageRank"
 * by Haim Avron and Lior Horesh
 *
 * @tparam GraphType type of graph object. Needs to support the following:
 *                   GraphType::vertex_type - type of vertex.
 *                   GraphType::num_edges() - number of edges.
 *                   GraphType::deg(node) - debgree of a node.
 *                   GraphType::adjanct_begin(node),
 *                   GraphType::adjanct_end(node) -
 *                    begining and end iterators to adjancy container
 *                    container can be any type.
 * @param G input graph
 * @param seeds seed nodes
 * @param cluster output cluster of nodes
 * @param alpha,gamma,epsilon, NX - parameters (see paper).
 * @param recursive - recursively run on output as seed until conductance
 *                    steps reducing.
 */
template<typename GraphType>
double FindLocalCluster(const GraphType& G,
    const std::unordered_set<typename GraphType::vertex_type>& seeds,
    std::unordered_set<typename GraphType::vertex_type>& cluster,
    double alpha = 0.85, double gamma = 5.0, double epsilon = 0.001, int NX = 4,
    bool recursive = false) {

    typedef typename GraphType::vertex_type vertex_type;
    double currentcond = -1;
    cluster = seeds;
    bool improve;
    El::Matrix<double> x;

    do {
        // Create seed set.
        std::unordered_map<vertex_type, double> s;
        for(auto it = cluster.begin(); it != cluster.end(); it++)
            s[*it] = 1.0 / cluster.size();

        // Run the diffusion
        std::unordered_map<vertex_type, El::Matrix<double>*> y;
        TimeDependentPPR(G, s, y, x, alpha, gamma, epsilon, NX);

        // Go over the y output at the different time samples,
        // find the best prefix and if better conductance, store it.
        improve = false;
        for (int t = 0; t < NX; t++) {
            // Sort (descending) the non-zero components based on their normalized
            // y values (normalized by degree).
            std::vector<std::pair<double, vertex_type> > vals(y.size());
            int i = 0;
            for(auto it = y.begin(); it != y.end(); it++) {
                vertex_type node = it->first;
                double val = - it->second->Get(t, 0) / G.degree(node);
                vals[i] = std::make_pair(val, node);
                i++;
            }
            std::sort(vals.begin(), vals.end());

            // Find the best prefix
            int volS = 0, cutS = 0;
            double bestcond = 1.0;
            int bestprefix = 0;
            int Gvol = G.num_edges();
            std::unordered_set<vertex_type> currentset;
            for (int i = 0; i < vals.size(); i++) {
                vertex_type node = vals[i].second;
                size_t deg = G.degree(node);
                volS += deg;
                for(auto it = G.adjanct_begin(node);
                    it != G.adjanct_end(node); it++) {
                    const vertex_type &onode = *it;
                    if (currentset.count(onode))
                        cutS--;
                    else
                        cutS++;
                }

                double condS =
                    static_cast<double>(cutS) / std::min(volS, Gvol - volS);
                if (condS < bestcond) {
                    bestcond = condS;
                    bestprefix = i;
                }
                currentset.insert(node);
            }

            if (currentcond == -1 || bestcond < 0.999999 * currentcond) {
                // We have a new best cluster - the best perfix.
                improve = true;
                cluster.clear();
                for(int i = 0; i <= bestprefix; i++)
                    cluster.insert(vals[i].second);
                currentcond = bestcond;
            }
        }

        // Clear y
        for(auto it = y.begin(); it != y.end(); it++)
            delete it->second;
    } while (recursive && improve);

    return currentcond;
}

} }   // namespace skylark::ml

#endif // SKYLARK_LOCAL_COMPUTATIONS_HPP


/*
 * Copyright (c) 2013-2019 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef AFFINE_HPP
#define AFFINE_HPP

// Affine Arithmetic

#include <iostream>
#include <stdexcept>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <vector>
#include <algorithm>
#include <cmath>

#include <map> //@hylagi

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

#include <kv/convert.hpp>

/*
 * define simplicity of affine arithmetic
 *
 *   value of AFFINE_SIMPLE                   | 0 | 1 | 2
 *  ------------------------------------------+---+---+---
 *   add dummy epsilin on linear operation    | o | x | x
 *   add dummy epsilin on nonlinear operation | o | o | x
 *
 *  default: 1
 */

#ifndef AFFINE_SIMPLE
#define AFFINE_SIMPLE 1
#endif

/*
 * select the method for multiplication
 *
 *  0: Stolfi's simple method (default, O(n))
 *  1: better multiplication (give smaller extra epsilon but slow, O(n^2))
 *  2: best multiplication (give smallest extra epsilon but slow, O(n^2))
 */

#ifndef AFFINE_MULT
#define AFFINE_MULT 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> class affine;

template <class C, class T> struct acceptable_s<C, affine<T> > {
  static const bool value = boost::is_same<C, interval<T> >::value || boost::is_convertible<C, std::string>::value;
};

template <class C, class T> struct acceptable_n<C, affine<T> > {
  static const bool value = convertible<C, T>::value && (!acceptable_s<C, affine<T> >::value);
};

template <class C, class T> struct convertible<C, affine<T> > {
  static const bool value = acceptable_n<C, affine<T> >::value || acceptable_s<C, affine<T> >::value || boost::is_same<C, affine<T> >::value;
};

#if 0
template <class C, class T> struct acceptable_n<C, affine<T> > {
  static const bool value = convertible<C, T>::value && (!boost::is_same<C, interval<T> >::value) && (!boost::is_convertible<C, std::string>::value);
};

template <class C, class T> struct convertible<C, affine<T> > {
  static const bool value = convertible<C, T>::value || boost::is_same<C, interval<T> >::value || boost::is_convertible<C, std::string>::value || boost::is_same<C, affine<T> >::value;
};
#endif

template <class T> class affine {
  public:
  ub::vector<T> a;
  #if AFFINE_SIMPLE >= 1
  T er;
  #endif

  typedef T base_type;

  static int& maxnum() {
    static int m = 0;
    #pragma omp threadprivate(m)
    return m;
  }

  friend inline T rad(const affine& x) {
    int i, xs;
    T r(0.);

    xs = x.a.size();

    rop<T>::begin();
    for (i=1; i<xs; i++) {
      using std::abs;
      r = rop<T>::add_up(r, abs(x.a(i)));
    }
    #if AFFINE_SIMPLE >= 1
    r = rop<T>::add_up(r, x.er);
    #endif
    rop<T>::end();

    return r;
  }

  friend inline interval<T> to_interval(const affine& x) {
    T t1, t2, t3;

    t1 = rad(x);
    rop<T>::begin();
    t2 = rop<T>::sub_down(x.a(0), t1);
    t3 = rop<T>::add_up(x.a(0), t1);
    rop<T>::end();
    return interval<T>(t2, t3);
  }

  interval<T> as_interval() const{ // @hylagi modified
    T t1, t2, t3;
    t1 = rad(*this);
    rop<T>::begin();
    t2 = rop<T>::sub_down(a(0), t1);
    t3 = rop<T>::add_up(a(0), t1);
    rop<T>::end();
    return interval<T>(t2, t3);
  }

  affine() {
  }

  template <class C> explicit affine(const C& x, typename boost::enable_if_c< acceptable_n<C, affine>::value >::type* =0) {
    a.resize(1);
    a(0) = x;
    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif
  }

  template <class C> explicit affine(const C& x, typename boost::enable_if_c< acceptable_s<C, affine>::value >::type* =0) {
    int i;
    interval<T> I(x);
    maxnum()++;
    a.resize(maxnum()+1);

    rop<T>::begin();
    a(0) = rop<T>::mul_up(rop<T>::add_up(I.upper(), I.lower()), T(0.5));
    a(maxnum()) = rop<T>::sub_up(a(0), I.lower());
    rop<T>::end();

    for (i=1; i<maxnum(); i++) a(i) = 0.;

    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif
  }

  template <class C> typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator=(const C& x) {
    a.resize(1);
    a(0) = x;
    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif

    return *this;
  }

  template <class C> typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator=(const C& x) {
    int i;
    interval<T> I(x);
    maxnum()++;
    a.resize(maxnum()+1);

    rop<T>::begin();
    a(0) = rop<T>::mul_up(rop<T>::add_up(I.upper(), I.lower()), T(0.5));
    a(maxnum()) = rop<T>::sub_up(a(0), I.lower());
    rop<T>::end();

    for (i=1; i<maxnum(); i++) a(i) = 0.;

    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif

    return *this;
  }

  T get_coef (int i) const {
    if (i >= a.size()) return T(0.);
    else return a(i);
  }

  T get_mid() const {
    return a(0);
  }

  T get_err() const {
    #if AFFINE_SIMPLE >= 1
    return er;
    #else
    return T(0.);
    #endif
  }

  friend affine operator+(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;
    T err(0.);

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    xs = x.a.size();
    ys = y.a.size();
    if (xs > ys) {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(xs);
      #endif
      rop<T>::begin();
      for (i=0; i<ys; i++) {
        r.a(i) = rop<T>::add_down(x.a(i), y.a(i));
      }
      for (i=0; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(ys);
      #endif
      rop<T>::begin();
      for (i=0; i<xs; i++) {
        r.a(i) = rop<T>::add_down(x.a(i), y.a(i));
      }
      for (i=0; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=xs; i<ys; i++) {
        r.a(i) = y.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(rop<T>::add_up(x.er, y.er), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  // same as operator+, but do not add extra epsilon.
  // This function can be used for adding affine variables
  // which have no common epsilons.

  friend affine append(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;

    xs = x.a.size();
    ys = y.a.size();

    if (xs > ys) {
      r.a.resize(xs);
      for (i=0; i<ys; i++) {
        r.a(i) = x.a(i) + y.a(i);
      }
      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
    } else {
      r.a.resize(ys);
      for (i=0; i<xs; i++) {
        r.a(i) = x.a(i) + y.a(i);
      }
      for (i=xs; i<ys; i++) {
        r.a(i) = y.a(i);
      }
    }
    #if AFFINE_SIMPLE >= 1
    r.er = x.er + y.er;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator+(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::add_down(x.a(0), (T)y);
    err = rop<T>::sub_up(rop<T>::add_up(x.a(0), (T)y), r.a(0));
    rop<T>::end();

    for (i=1; i<xs; i++) {
      r.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(x.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator+(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err;

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::add_down((T)x, y.a(0));
    err = rop<T>::sub_up(rop<T>::add_up((T)x, y.a(0)), r.a(0));
    rop<T>::end();

    for (i=1; i<ys; i++) {
      r.a(i) = y.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(y.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator+(const affine& x, const C& y) {
    return x + affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator+(const C& x, const affine& y) {
    return affine(x) + y;
  }

  friend affine& operator+=(affine& x, const affine& y) {
    x = x + y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator+=(affine& x, const C& y) {
    x = x + y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator+=(affine& x, const C& y) {
    x = x + y;
    return x;
  }

  friend affine operator-(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;
    T err(0.);

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    xs = x.a.size();
    ys = y.a.size();
    if (xs > ys) {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(xs);
      #endif
      rop<T>::begin();
      for (i=0; i<ys; i++) {
        r.a(i) = rop<T>::sub_down(x.a(i), y.a(i));
      }
      for (i=0; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::sub_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(ys);
      #endif
      rop<T>::begin();
      for (i=0; i<xs; i++) {
        r.a(i) = rop<T>::sub_down(x.a(i), y.a(i));
      }
      for (i=0; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::sub_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=xs; i<ys; i++) {
        r.a(i) = - y.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(rop<T>::add_up(x.er, y.er), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator-(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::sub_down(x.a(0), (T)y);
    err = rop<T>::sub_up(rop<T>::sub_up(x.a(0), (T)y), r.a(0));
    rop<T>::end();

    for (i=1; i<xs; i++) {
      r.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(x.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator-(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err;

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::sub_down((T)x, y.a(0));
    err = rop<T>::sub_up(rop<T>::sub_up((T)x, y.a(0)), r.a(0));
    rop<T>::end();

    for (i=1; i<ys; i++) {
      r.a(i) = - y.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(y.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator-(const affine& x, const C& y) {
    return x - affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator-(const C& x, const affine& y) {
    return affine(x) - y;
  }

  friend affine& operator-=(affine& x, const affine& y) {
    x = x - y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator-=(affine& x, const C& y) {
    x = x - y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator-=(affine& x, const C& y) {
    x = x - y;
    return x;
  }

  friend affine operator-(const affine& x) {
    affine r;

    r.a = - x.a;
    #if AFFINE_SIMPLE >= 1
    r.er = x.er;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator*(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err(0.);

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    for (i=0; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), (T)y);
    }
    for (i=0; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), (T)y), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(rop<T>::mul_up(x.er, T(abs(y))), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator*(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err(0.);

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    for (i=0; i<ys; i++) {
      r.a(i) = rop<T>::mul_down((T)x, y.a(i));
    }
    for (i=0; i<ys; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up((T)x, y.a(i)), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(rop<T>::mul_up(y.er, T(abs(x))), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator*(const affine& x, const C& y) {
    return x * affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator*(const C& x, const affine& y) {
    return affine(x) * y;
  }

  #if AFFINE_MULT >= 1

  static interval<T> bestmult_error(const affine& x, const affine& y)
  {
    int n, i, j, s;
    bool f1, f2;
    ub::vector<T> vx, vy;
    kv::interval<T> tmp, C, D, E, X, Y;

    n = std::max(x.a.size(), y.a.size()) - 1;

    #if AFFINE_SIMPLE >= 1
    vx.resize(n+3);
    vy.resize(n+3);
    #else
    vx.resize(n+1);
    vy.resize(n+1);
    #endif

    for (i=0; i<x.a.size(); i++) {
      vx(i) = x.a(i);
    }
    for (i=x.a.size(); i<=n; i++) {
      vx(i) = 0.;
    }
    for (i=0; i<y.a.size(); i++) {
      vy(i) = y.a(i);
    }
    for (i=y.a.size(); i<=n; i++) {
      vy(i) = 0.;
    }

    #if AFFINE_SIMPLE >= 1
    vx(n+1) = x.er;
    vy(n+1) = 0.;
    vx(n+2) = 0.;
    vy(n+2) = y.er;
    n += 2;
    #endif

    E = 0.;

    #if AFFINE_MULT == 1
    for (i=1; i<=n; i++) {
      X = vx(i);
      Y = vy(i);
      E += X * Y * interval<T>(0., 1.);
      for (j=i+1; j<=n; j++) {
        tmp = X * vy(j) + Y * vx(j);
        E += tmp * interval<T>(-1., 1.);
      }
    }
    #else // AFFINE_MULT == 1

    for (i=1; i<=n; i++) {
      X = vx(i);
      Y = vy(i);
      if (X == 0. && Y == 0.) continue;
      C = D = 0.;
      for (j=1; j<=n; j++) {
        if (j == i) continue;
        if (vy(j) == 0. && vx(j) == 0.) continue;
        tmp = X * vy(j) - Y * vx(j);
        if (tmp.lower() >= 0.) {
          C += vx(j);
          D += vy(j);
        } else if (tmp.upper() <= 0.) {
          C -= vx(j);
          D -= vy(j);
        } else {
          if ((X.upper() > 0. && vx(j) < 0.)
          || (X.lower() < 0. && vx(j) > 0.)) {
            X -= vx(j);
            Y -= vy(j);
          } else {
            X += vx(j);
            Y += vy(j);
          }
        }
      }
      E = interval<T>::hull(E, X * Y + (X * D + Y * C) + C * D);
      E = interval<T>::hull(E, X * Y - (X * D + Y * C) + C * D);
      if (!zero_in(X * Y) ){
        tmp = -0.5 * (X * D + Y * C) / (X * Y);
        if (overlap(tmp, interval<T>(-1., 1.))) {
          E = interval<T>::hull(E, X * Y * tmp * tmp + (X * D + Y * C) * tmp + C * D);
        }
      }
    }

    #endif // AFFINE_MULT == 1

    return E;
  }

  #endif // AFFINE_MULT >= 1

  friend affine operator*(const affine& x, const affine& y) {
    affine r;
    int i, j, xs, ys;
    T err;
    T tmp_u, tmp_l;

    // if (&x == &y) return square(x);

    xs = x.a.size();
    ys = y.a.size();

    #if AFFINE_SIMPLE != 2
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(std::max(xs, ys));
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::mul_down(x.a(0), y.a(0));
    err = rop<T>::sub_up(rop<T>::mul_up(x.a(0), y.a(0)), r.a(0));
    rop<T>::end();

    if (xs > ys) {
      rop<T>::begin();
      for (i=1; i<ys; i++) {
        r.a(i) = rop<T>::add_down(rop<T>::mul_down(y.a(0), x.a(i)), rop<T>::mul_down(x.a(0), y.a(i)));
      }
      for (i=ys; i<xs; i++) {
        r.a(i) = rop<T>::mul_down(y.a(0), x.a(i));
      }
      for (i=1; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(rop<T>::mul_up(y.a(0), x.a(i)), rop<T>::mul_up(x.a(0), y.a(i))), r.a(i)));
      }
      for (i=ys; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(y.a(0), x.a(i)), r.a(i)));
      }
      rop<T>::end();
      #if AFFINE_SIMPLE != 2
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      rop<T>::begin();
      for (i=1; i<xs; i++) {
        r.a(i) = rop<T>::add_down(rop<T>::mul_down(y.a(0), x.a(i)), rop<T>::mul_down(x.a(0), y.a(i)));
      }
      for (i=xs; i<ys; i++) {
        r.a(i) = rop<T>::mul_down(x.a(0), y.a(i));
      }
      for (i=1; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(rop<T>::mul_up(y.a(0), x.a(i)), rop<T>::mul_up(x.a(0), y.a(i))), r.a(i)));
      }
      for (i=xs; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(0), y.a(i)), r.a(i)));
      }
      rop<T>::end();
      #if AFFINE_SIMPLE != 2
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }

    #if AFFINE_MULT >= 1

    interval<T> E;
    E = r.a(0) + bestmult_error(x, y);
    rop<T>::begin();
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(E.upper(), E.lower()), 0.5);
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), E.lower()));
    rop<T>::end();

    #else // AFFINE_MULT >= 1

    tmp_l = rad(x);
    tmp_u = rad(y);
    rop<T>::begin();
    err = rop<T>::add_up(err, rop<T>::mul_up(tmp_l, tmp_u));
    rop<T>::end();

    #endif // AFFINE_MULT >= 1

    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::add_up(rop<T>::mul_up(abs(y.a(0)), x.er), rop<T>::mul_up(abs(x.a(0)), y.er)));
    rop<T>::end();
    #endif

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine& operator*=(affine& x, const affine& y) {
    x = x * y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator*=(affine& x, const C& y) {
    x = x * y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator*=(affine& x, const C& y) {
    x = x * y;
    return x;
  }

  friend affine inv(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();
    if (l < 0. && u > 0.) {
      throw std::domain_error("affine: division by 0");
    }

    a = -1. /(l * u);
    tmp = a; // tmp is used to force interval calculation
    if (u > 0.) {
      range = 2. * sqrt(-tmp);
    } else {
      range = -2. * sqrt(-tmp);
    }
    tmp = l;
    range = interval<T>::hull(range, 1./tmp - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, 1./tmp - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine operator/(const affine& x, const affine& y) {
    return x * inv(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator/(const C& x, const affine& y) {
    return x * inv(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator/(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err(0.);

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    for (i=0; i<xs; i++) {
      r.a(i) = rop<T>::div_down(x.a(i), (T)y);
    }
    for (i=0; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::div_up(x.a(i), (T)y), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    // r.er = x.er / abs(y) + err;
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(err, rop<T>::div_up(x.er, T(abs(y))));
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator/(const affine& x, const C& y) {
    return x / affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator/(const C& x, const affine& y) {
    return affine(x) / y;
  }

  friend affine& operator/=(affine& x, const affine& y) {
    x = x / y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator/=(affine& x, const C& y) {
    x = x / y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator/=(affine& x, const C& y) {
    x = x / y;
    return x;
  }

  friend affine sqrt(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();
    if (l < 0.) {
      throw std::domain_error("affine: sqrt of negative value");

    }

    using std::sqrt;
    a = 1. /(sqrt(l) + sqrt(u));
    tmp = a;
    range = 1. / (4. * tmp);
    tmp = l;
    range = interval<T>::hull(range, sqrt(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, sqrt(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine square(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    a = l + u;
    tmp = a;
    range = - tmp * tmp * 0.25;
    tmp = l;
    // range = hull(range, tmp * tmp - a * tmp);
    range = interval<T>::hull(range, tmp * (tmp - a));
    tmp = u;
    range = interval<T>::hull(range, tmp * (tmp - a));

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine exp(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    using std::exp;
    if (u == l) a = exp(u);
    else a = (exp(u) - exp(l)) / (u - l);
    tmp = a;
    range = tmp * (1. - log(tmp));
    tmp = l;
    range = interval<T>::hull(range, exp(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, exp(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine log(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    if (l <= 0.) {
      throw std::domain_error("affine: log of nagative value");
    }

    using std::log;
    if (u == l) a = 1. / u;
    else a = (log(u) - log(l)) / (u - l);
    tmp = a;
    range = log(1. / tmp) - 1.;
    tmp = l;
    range = interval<T>::hull(range, log(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, log(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine abs(const affine& x) {
    affine r;
    T err;
    interval<T> I, range;
    T a, b, l, u;
    int i, xs;

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    if (l >= 0.) {
      return x;
    }
    if (u <= 0.) {
      return -x;
    }

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    a = (u + l) / (u - l);

    range = 0.;
    range = interval<T>::hull(range, u - a * u);
    range = interval<T>::hull(range, -l - a * l);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  // lazy implementation of integer pow
  friend affine pow(const affine& x, int y) {
    affine r, xp;
    int a, tmp;

    if (y == 0) return affine(1.);

    a = (y >= 0) ? y : -y;

    tmp = a;
    r = 1.;
    xp = x;
    while (tmp != 0) {
      if (tmp % 2 != 0) {
        r *= xp;
      }
      tmp /= 2;
      xp = xp * xp;
    }

    if (y < 0) {
      r = 1. / r;
    }

    return r;
  }

  friend affine pow(const affine& x, const affine& y) {
    return exp(y * log(x));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value && ! boost::is_integral<C>::value, affine >::type pow(const affine& x, const C& y) {
    return pow(x, affine(y));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type pow(const affine& x, const C& y) {
    return pow(x, affine(y));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type pow(const C& x, const affine& y) {
    return pow(affine(x), y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type pow(const C& x, const affine& y) {
    return pow(affine(x), y);
  }

  // lazy implementation of sin
  friend affine sin(const affine& x) {
    // return sin((interval<T>)(x.a(0))) + cos(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = sin((interval<T>)(x.a(0)));
    r2 = cos(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of cos
  friend affine cos(const affine& x) {
    // return cos((interval<T>)(x.a(0))) - sin(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = cos((interval<T>)(x.a(0)));
    r2 = -sin(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of tan
  friend affine tan(const affine& x) {
    // return tan((interval<T>)(x.a(0))) + pow(cos(to_interval(x)), -2) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = tan((interval<T>)(x.a(0)));
    r2 = pow(cos(to_interval(x)), -2);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of asin
  friend affine asin(const affine& x) {
    // return asin((interval<T>)(x.a(0))) + (1. / sqrt(1. - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = asin((interval<T>)(x.a(0)));
    r2 = 1. / sqrt(1. - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of acos
  friend affine acos(const affine& x) {
    // return acos((interval<T>)(x.a(0))) + (- 1. / sqrt(1. - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = acos((interval<T>)(x.a(0)));
    r2 = - 1. / sqrt(1. - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of atan
  friend affine atan(const affine& x) {
    // return atan((interval<T>)(x.a(0))) + (1. / sqrt(1. + pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = atan((interval<T>)(x.a(0)));
    r2 = 1. / sqrt(1. + pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of sinh
  friend affine sinh(const affine& x) {
    // return sinh((interval<T>)(x.a(0))) + cosh(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = sinh((interval<T>)(x.a(0)));
    r2 = cosh(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of cosh
  friend affine cosh(const affine& x) {
    // return cosh((interval<T>)(x.a(0))) + sinh(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = cosh((interval<T>)(x.a(0)));
    r2 = sinh(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of tanh
  friend affine tanh(const affine& x) {
    // return tanh((interval<T>)(x.a(0))) + pow(cosh(to_interval(x)), -2) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = tanh((interval<T>)(x.a(0)));
    r2 = pow(cosh(to_interval(x)), -2);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of asinh
  friend affine asinh(const affine& x) {
    // return asinh((interval<T>)(x.a(0))) + (1 / sqrt(pow(to_interval(x), 2) + 1)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = asinh((interval<T>)(x.a(0)));
    r2 = 1 / sqrt(pow(to_interval(x), 2) + 1);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of acosh
  friend affine acosh(const affine& x) {
    // return acosh((interval<T>)(x.a(0))) + (1 / sqrt(pow(to_interval(x), 2) - 1)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = acosh((interval<T>)(x.a(0)));
    r2 = 1 / sqrt(pow(to_interval(x), 2) - 1);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of atanh
  friend affine atanh(const affine& x) {
    // return atanh((interval<T>)(x.a(0))) + (1 / (1 - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = atanh((interval<T>)(x.a(0)));
    r2 = 1 / (1 - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  friend std::ostream& operator<<(std::ostream& s, const affine& x) {
    int i;

    s << "[(" << x.a(0) << ")";
    for (i=1; i<x.a.size(); i++) {
      s << "+(" << x.a(i) << ")e" << i;
    }
    #if AFFINE_SIMPLE >= 1
    s << "+(" << x.er << ")er";
    #endif
    s << "]";

    return s;
  }

  friend inline void split(const affine& x, int n, affine& y, affine& z) {
    int i;
    int s = x.a.size();

    y.a.resize(s);
    z.a.resize(s);
    for (i=0; i<=n; i++) {
      y.a(i) = x.a(i);
      z.a(i) = 0.;
    }
    for (i=n+1; i<s; i++) {
      y.a(i) = 0.;
      z.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE >= 1
    y.er = 0.;
    z.er = x.er;
    #endif
  }

  void resize() {
    int i;
    ub::vector<T> r;

    r.resize(maxnum()+1);
    for (i=0; i<=maxnum(); i++) {
      r(i) = a(i);
    }

    a = r;
  }
};

template <class T> inline ub::vector< interval<T> > to_interval(const ub::vector< affine<T> >& x) {
  int s = x.size();
  ub::vector< interval<T> > r;
  int i;

  r.resize(s);
  for (i=0; i<s; i++) r(i) = to_interval(x(i));

  return r;
}

/*
 * epsilon_reduce(x, n, n_limit)
 *  x: vector of affine (overwrited)
 *  reduce the number of epsilons to n if the number of epsilons > n_limit.
 *
 *  s: size of x
 *  keep n-s "important" epsilons and convert other "non-important" epsilons
 *  to s-dimensional rectangular.
 */

// a class to store column vector and its "importance"

template <class T> class ep_reduce_v {
  public:
  ub::vector<T> v;
  T score;
  int index; // @hylagi to identify intervalized dummy variables
  void calc_score() {
    int s = v.size();
    int i, j;
    T m1, m2, tmp;
    using std::abs;
    if (s<2){score = abs(v(0)); return;} // @hylagi
    m1 = abs(v(0));
    if (s == 1) {
      score = m1;
      return;
    }
    m2 = abs(v(1));
    if (m2 > m1) {
      tmp = m2; m2 = m1; m1 = tmp;
    }
    for (i=2; i<s; i++) {
      tmp = abs(v(i));
      if (tmp > m1) {
        m2 = m1; m1 = tmp;
      } else if (tmp > m2) {
        m2 = tmp;
      }
    }
    if (m1 == 0.) score = 0.;
    else score = (m1*m2)/(m1+m2);
  }
};

// function to sort column vector by score

template <class T> inline bool ep_reduce_cmp(ep_reduce_v<T>* a, ep_reduce_v<T>* b) {
#ifdef EP_REDUCE_REVERSE
  return a->score < b->score;
#else
  return a->score > b->score;
#endif
}
//template <class T> inline void epsilon_reduce(ub::vector< affine<T> >& x, int n, int n_limit = 0) {
template <class T> inline std::map<int, int> epsilon_reduce(ub::vector< affine<T> >& x, int n, int n_limit = 0) { // @hylagi modified
  int s = x.size();
  int m = affine<T>::maxnum();
  int i, j;
  std::vector< ep_reduce_v<T> > a;
  std::vector< ep_reduce_v<T>* > pa;
  ub::vector< affine<T> > r;
  T tmp;
  std::map<int, int> result_map; // @hylagi

  if (n_limit < n) n_limit = n;

  // if (m <= n_limit) return;
  // if (n < s) return; // impossible
  // @hylagi modified
  if (m <= n_limit) return result_map;
  if (n < s) return result_map; // impossible

  a.resize(m);
  pa.resize(m);

  for (i=1; i<=m; i++) {
    a[i-1].v.resize(s);
    a[i-1].index = i; // @hylagi
    result_map.insert(std::make_pair(i, -1)); // @hylagi
    for (j=0; j<s; j++) {
      a[i-1].v(j) = (i < x(j).a.size()) ? x(j).a(i) : (T)0.;
    }
    a[i-1].calc_score();
    pa[i-1] = &(a[i-1]);
  }

#ifdef EP_REDUCE_REVERSE
  std::partial_sort(pa.begin(), pa.begin()+m-n+s, pa.end(), ep_reduce_cmp<T>);
#else
  std::partial_sort(pa.begin(), pa.begin()+n-s, pa.end(), ep_reduce_cmp<T>);
#endif

  r.resize(s);
  for (i=0; i<s; i++) {
    r(i).a.resize(n+1);
    r(i).a(0) = x(i).a(0);
    for (j=0; j<n-s; j++) {
#ifdef EP_REDUCE_REVERSE
      r(i).a(j+1) = pa[m-1-j]->v(i);
      result_map[pa[m-1-j]->index] = j+1; // @hylagi
#else
      r(i).a(j+1) = pa[j]->v(i);
#endif
    }
    tmp = 0.;
    rop<T>::begin();
    for (j=n-s; j<m; j++) {
      using std::abs;
#ifdef EP_REDUCE_REVERSE
      tmp = rop<T>::add_up(tmp, abs(pa[m-1-j]->v(i)));
#else
      tmp = rop<T>::add_up(tmp, abs(pa[j]->v(i)));
#endif
    }
    #if AFFINE_SIMPLE >= 1
    tmp = rop<T>::add_up(tmp, x(i).er);
    #endif
    rop<T>::end();
    for (j=n-s; j<n; j++) {
      r(i).a(j+1) = 0.;
    }
    r(i).a(n-s+i+1) = tmp;
    #if AFFINE_SIMPLE >= 1
    r(i).er = 0.;
    #endif
  }

  x = r;
  affine<T>::maxnum() = n;
  return result_map; //@hylagi
}

// simple version of epsilon_reduce
// keep ep_1...ep_n and "intervalize" epsilons newer than ep_n

template <class T> inline void epsilon_reduce2(ub::vector< affine<T> >& x, int n) {
  int s = x.size();
  int i, j;
  T tmp;

  for (i=0; i<s; i++) {
    tmp = 0.;
    rop<T>::begin();
    for (j=n+1; j<x(i).a.size(); j++) {
      using std::abs;
      tmp = rop<T>::add_up(tmp, abs(x(i).a(j)));
    }
    #if AFFINE_SIMPLE >= 1
    tmp = rop<T>::add_up(tmp, x(i).er);
    #endif
    rop<T>::end();

    x(i).a.resize(n+1+i+1, true);
    for (j=0; j<i; j++) {
      x(i).a(n+1+j) = 0.;
    }
    x(i).a(n+1+i) = tmp;
    #if AFFINE_SIMPLE >= 1
    x(i).er = 0.;
    #endif
  }

  affine<T>::maxnum() = n + s;
}

template <class T> struct constants< affine<T> > {
  static affine<T> pi() {
    static const affine<T> tmp(constants< interval<T> >::pi());
    return tmp;
  }

  static affine<T> e() {
    static const affine<T> tmp(constants< interval<T> >::e());
    return tmp;
  }

  static affine<T> ln2() {
    static const affine<T> tmp(constants< interval<T> >::ln2());
    return tmp;
  }
  static affine<T> str(const std::string& s) {
    return affine<T>(constants< interval<T> >::str(s));
  }
  static affine<T> str(const std::string& s1, const std::string& s2) {
    return affine<T>(constants< interval<T> >::str(s1, s2));
  }
};

} // namespace kv

#endif //AFFINE_HPP


/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * Written (W) 2013 Fernando J. Iglesias Garcia
 * Copyright (C) 2013 Fernando J. Iglesias Garcia
 */

#ifdef HAVE_EIGEN3

#include <shogun/distance/CustomMahalanobisDistance.h>
#include <Eigen/Dense>

using namespace shogun;
using namespace Eigen;

CCustomMahalanobisDistance::CCustomMahalanobisDistance() : CRealDistance()
{
	register_params();
}

CCustomMahalanobisDistance::CCustomMahalanobisDistance(CFeatures* l, CFeatures* r, SGMatrix<float64_t> m)
: CRealDistance()
{
	register_params();
	CRealDistance::init(l, r);
	m_mahalanobis_matrix = m;
}

void CCustomMahalanobisDistance::register_params()
{
	SG_ADD(&m_mahalanobis_matrix, "m_mahalanobis_matrix", "Mahalanobis matrix", MS_NOT_AVAILABLE)
}

CCustomMahalanobisDistance::~CCustomMahalanobisDistance()
{
	cleanup();
}

void CCustomMahalanobisDistance::cleanup()
{
}

const char* CCustomMahalanobisDistance::get_name() const
{
	return "CustomMahalanobisDistance";
}

EDistanceType CCustomMahalanobisDistance::get_distance_type()
{
	return D_CUSTOMMAHALANOBIS;
}

float64_t CCustomMahalanobisDistance::compute(int32_t idx_a, int32_t idx_b)
{
	// Get feature vectors that will be used to compute the distance; casts
	// are safe, features are checked to be dense in DenseDistance::init
	SGVector<float64_t> avec = static_cast<CDenseFeatures<float64_t>*>(lhs)->get_feature_vector(idx_a);
	SGVector<float64_t> bvec = static_cast<CDenseFeatures<float64_t>*>(rhs)->get_feature_vector(idx_b);

	REQUIRE(avec.vlen == bvec.vlen, "In CCustomMahalanobisDistance::compute the "
			"feature vectors must have the same number of elements")

	// Compute the distance between the feature vectors

	// Compute the difference vector and wrap in Eigen vector
	const VectorXd dvec = Map<const VectorXd>(avec, avec.vlen) - Map<const VectorXd>(bvec, bvec.vlen);
	// Wrap Mahalanobis distance in Eigen matrix
	Map<const MatrixXd> M(m_mahalanobis_matrix.matrix, m_mahalanobis_matrix.num_rows,
			m_mahalanobis_matrix.num_cols);

	return dvec.transpose()*M*dvec;
}

#endif /* HAVE_EIGEN3 */


//  Copyright John Maddock 2006.
//  Copyright Paul A. Bristow 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_STATS_STUDENTS_T_HPP
#define BOOST_STATS_STUDENTS_T_HPP

// http://en.wikipedia.org/wiki/Student%27s_t_distribution
// http://www.itl.nist.gov/div898/handbook/eda/section3/eda3664.htm

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for ibeta(a, b, x).
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>

#include <utility>

#ifdef BOOST_MSVC
# pragma warning(push)
# pragma warning(disable: 4702) // unreachable code (return after domain_error throw).
#endif

namespace boost{ namespace math{

template <class RealType = double, class Policy = policies::policy<> >
class students_t_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   students_t_distribution(RealType i) : m_df(i)
   { // Constructor.
      RealType result;
      detail::check_df(
         "boost::math::students_t_distribution<%1%>::students_t_distribution", m_df, &result, Policy());
   } // students_t_distribution

   RealType degrees_of_freedom()const
   {
      return m_df;
   }

   // Parameter estimation:
   static RealType find_degrees_of_freedom(
      RealType difference_from_mean,
      RealType alpha,
      RealType beta,
      RealType sd,
      RealType hint = 100);

private:
   //
   // Data members:
   //
   RealType m_df;  // degrees of freedom are a real number.
};

typedef students_t_distribution<double> students_t;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const students_t_distribution<RealType, Policy>& /*dist*/)
{ // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const students_t_distribution<RealType, Policy>& /*dist*/)
{ // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>());
}

template <class RealType, class Policy>
inline RealType pdf(const students_t_distribution<RealType, Policy>& dist, const RealType& t)
{
   BOOST_FPU_EXCEPTION_GUARD
   BOOST_MATH_STD_USING  // for ADL of std functions

   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
      "boost::math::pdf(const students_t_distribution<%1%>&, %1%)", degrees_of_freedom, &error_result, Policy()))
      return error_result;
   // Might conceivably permit df = +infinity and use normal distribution.
   RealType result;
   RealType basem1 = t * t / degrees_of_freedom;
   if(basem1 < 0.125)
   {
      result = exp(-boost::math::log1p(basem1, Policy()) * (1+degrees_of_freedom) / 2);
   }
   else
   {
      result = pow(1 / (1 + basem1), (degrees_of_freedom + 1) / 2);
   }
   result /= sqrt(degrees_of_freedom) * boost::math::beta(degrees_of_freedom / 2, RealType(0.5f), Policy());
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const students_t_distribution<RealType, Policy>& dist, const RealType& t)
{
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
      "boost::math::cdf(const students_t_distribution<%1%>&, %1%)", degrees_of_freedom, &error_result, Policy()))
      return error_result;

   if (t == 0)
   {
     return 0.5;
   }
   //
   // Calculate probability of Student's t using the incomplete beta function.
   // probability = ibeta(degrees_of_freedom / 2, 1/2, degrees_of_freedom / (degrees_of_freedom + t*t))
   //
   // However when t is small compared to the degrees of freedom, that formula
   // suffers from rounding error, use the identity formula to work around
   // the problem:
   //
   // I[x](a,b) = 1 - I[1-x](b,a)
   //
   // and:
   //
   //     x = df / (df + t^2)
   //
   // so:
   //
   // 1 - x = t^2 / (df + t^2)
   //
   RealType t2 = t * t;
   RealType probability;
   if(degrees_of_freedom > 2 * t2)
   {
      RealType z = t2 / (degrees_of_freedom + t2);
      probability = ibetac(static_cast<RealType>(0.5), degrees_of_freedom / 2, z, Policy()) / 2;
   }
   else
   {
      RealType z = degrees_of_freedom / (degrees_of_freedom + t2);
      probability = ibeta(degrees_of_freedom / 2, static_cast<RealType>(0.5), z, Policy()) / 2;
   }
   return (t > 0 ? 1   - probability : probability);
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const students_t_distribution<RealType, Policy>& dist, const RealType& p)
{
   BOOST_MATH_STD_USING // for ADL of std functions
   //
   // Obtain parameters:
   //
   RealType degrees_of_freedom = dist.degrees_of_freedom();
   RealType probability = p;
   //
   // Check for domain errors:
   //
   static const char* function = "boost::math::quantile(const students_t_distribution<%1%>&, %1%)";
   RealType error_result;
   if(false == detail::check_df(
      function, degrees_of_freedom, &error_result, Policy())
         && detail::check_probability(function, probability, &error_result, Policy()))
      return error_result;

   // Special cases, regardless of degrees_of_freedom.
   if (probability == 0)
      return -policies::raise_overflow_error<RealType>(function, 0, Policy());
   if (probability == 1)
     return policies::raise_overflow_error<RealType>(function, 0, Policy());
   if (probability == static_cast<RealType>(0.5))
     return 0;
   //
   // This next block is disabled in favour of a faster method than
   // incomplete beta inverse, code retained for future reference:
   //
#if 0
   //
   // Calculate quantile of Student's t using the incomplete beta function inverse:
   //
   probability = (probability > 0.5) ? 1 - probability : probability;
   RealType t, x, y;
   x = ibeta_inv(degrees_of_freedom / 2, RealType(0.5), 2 * probability, &y);
   if(degrees_of_freedom * y > tools::max_value<RealType>() * x)
      t = tools::overflow_error<RealType>(function);
   else
      t = sqrt(degrees_of_freedom * y / x);
   //
   // Figure out sign based on the size of p:
   //
   if(p < 0.5)
      t = -t;

   return t;
#endif
   //
   // Depending on how many digits RealType has, this may forward
   // to the incomplete beta inverse as above.  Otherwise uses a
   // faster method that is accurate to ~15 digits everywhere
   // and a couple of epsilon at double precision and in the central
   // region where most use cases will occur...
   //
   return boost::math::detail::fast_students_t_quantile(degrees_of_freedom, probability, Policy());
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<students_t_distribution<RealType, Policy>, RealType>& c)
{
   return cdf(c.dist, -c.param);
}

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<students_t_distribution<RealType, Policy>, RealType>& c)
{
   return -quantile(c.dist, c.param);
}

//
// Parameter estimation follows:
//
namespace detail{
//
// Functors for finding degrees of freedom:
//
template <class RealType, class Policy>
struct sample_size_func
{
   sample_size_func(RealType a, RealType b, RealType s, RealType d)
      : alpha(a), beta(b), ratio(s*s/(d*d)) {}

   RealType operator()(const RealType& df)
   {
      if(df <= tools::min_value<RealType>())
         return 1;
      students_t_distribution<RealType, Policy> t(df);
      RealType qa = quantile(complement(t, alpha));
      RealType qb = quantile(complement(t, beta));
      qa += qb;
      qa *= qa;
      qa *= ratio;
      qa -= (df + 1);
      return qa;
   }
   RealType alpha, beta, ratio;
};

}  // namespace detail

template <class RealType, class Policy>
RealType students_t_distribution<RealType, Policy>::find_degrees_of_freedom(
      RealType difference_from_mean,
      RealType alpha,
      RealType beta,
      RealType sd,
      RealType hint)
{
   static const char* function = "boost::math::students_t_distribution<%1%>::find_degrees_of_freedom";
   //
   // Check for domain errors:
   //
   RealType error_result;
   if(false == detail::check_probability(
      function, alpha, &error_result, Policy())
         && detail::check_probability(function, beta, &error_result, Policy()))
      return error_result;

   if(hint <= 0)
      hint = 1;

   detail::sample_size_func<RealType, Policy> f(alpha, beta, sd, difference_from_mean);
   tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
   boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
   std::pair<RealType, RealType> r = tools::bracket_and_solve_root(f, hint, RealType(2), false, tol, max_iter, Policy());
   RealType result = r.first + (r.second - r.first) / 2;
   if(max_iter == policies::get_max_root_iterations<Policy>())
   {
      policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
         " either there is no answer to how many degrees of freedom are required"
         " or the answer is infinite.  Current best guess is %1%", result, Policy());
   }
   return result;
}

template <class RealType, class Policy>
inline RealType mean(const students_t_distribution<RealType, Policy>& )
{
   return 0;
}

template <class RealType, class Policy>
inline RealType variance(const students_t_distribution<RealType, Policy>& dist)
{
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
      "boost::math::variance(students_t_distribution<%1%> const&, %1%)", dist.degrees_of_freedom(), &error_result, Policy()))
      return error_result;

   RealType v = dist.degrees_of_freedom();
   return v / (v - 2);
}

template <class RealType, class Policy>
inline RealType mode(const students_t_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType median(const students_t_distribution<RealType, Policy>& /*dist*/)
{
   return 0;
}

template <class RealType, class Policy>
inline RealType skewness(const students_t_distribution<RealType, Policy>& dist)
{
   if(dist.degrees_of_freedom() <= 3)
   {
      policies::raise_domain_error<RealType>(
         "boost::math::skewness(students_t_distribution<%1%> const&, %1%)",
         "Skewness is undefined for degrees of freedom <= 3, but got %1%.",
         dist.degrees_of_freedom(), Policy());
   }
   return 0;
}

template <class RealType, class Policy>
inline RealType kurtosis(const students_t_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   if(df <= 3)
   {
      policies::raise_domain_error<RealType>(
         "boost::math::kurtosis(students_t_distribution<%1%> const&, %1%)",
         "Skewness is undefined for degrees of freedom <= 3, but got %1%.",
         df, Policy());
   }
   return 3 * (df - 2) / (df - 4);
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const students_t_distribution<RealType, Policy>& dist)
{
   // see http://mathworld.wolfram.com/Kurtosis.html
   RealType df = dist.degrees_of_freedom();
   if(df <= 3)
   {
      policies::raise_domain_error<RealType>(
         "boost::math::kurtosis_excess(students_t_distribution<%1%> const&, %1%)",
         "Skewness is undefined for degrees of freedom <= 3, but got %1%.",
         df, Policy());
   }
   return 6 / (df - 4);
}

} // namespace math
} // namespace boost

#ifdef BOOST_MSVC
# pragma warning(pop)
#endif

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_STATS_STUDENTS_T_HPP


#ifndef CY_NNQS_UTILITY_HPP
#define CY_NNQS_UTILITY_HPP
#include <memory>
#include <random>
#include <utility>

#include <Eigen/Dense>
#include "Utilities/type_traits.hpp"

namespace yannq
{

//! stable implementation of log(cosh(x)) for real x
template<typename T>
inline typename std::enable_if<!is_complex_type<T>::value, T>::type logCosh(T x)
{
    const T xp = std::abs(x);
    if (xp <= 12.) {
        return std::log(std::cosh(xp));
    } else {
        const static T log2v = std::log(2.);
        return xp - log2v;
    }
}

//! stable implementation of log(cosh(x)) for complex x
template<typename T>
inline typename std::enable_if<is_complex_type<T>::value, T>::type logCosh(T x)
{
    const auto xr = x.real();
    const auto xi = x.imag();

    T res = logCosh(xr);
    res += std::log(T{std::cos(xi), std::tanh(xr) * std::sin(xi)});

    return res;
}

template<typename T>
typename std::enable_if<!is_complex_type<T>::value, T>::type real(const T& v)
{
	return v;
}

/*
template<typename StateT, class AuxData, class Container, class Observable>
auto calcObs(const AuxData& ad, const Container& sr, Observable& obs)
	-> typename std::result_of<Observable(StateT)>::type
{
	using ResultType = typename std::result_of<Observable(StateT)>::type;
	ResultType res{};
	for(const auto& elt: sr)
	{
		auto s = construct_state<StateT>(ad, make_rtuple(elt));
		res += obs(s);
	}
	res /= sr.size();
	return res;
}
*/

//! generate a random vector in the computational basis
template <typename RandomEngine>
Eigen::VectorXi randomSigma(int n, RandomEngine& re)
{
	Eigen::VectorXi sigma(n);
	std::uniform_int_distribution<> uid(0, 1);
	//randomly initialize currSigma
	for(int i = 0; i < n; i++)
	{
		sigma(i) = -2*uid(re)+1;
	}
	return sigma;
}
//! generate a random vector in the computational basis with the constraint that
//! the number |1>=|\sigma_z = -1> is nup.
template <typename RandomEngine>
Eigen::VectorXi randomSigma(int n, int nup, RandomEngine& re)
{
	std::vector<int> sigma(n,-1);
	//randomly initialize currSigma
	for(int i = nup; i < n; i++)
	{
		sigma[i] = 1;
	}
	std::shuffle(sigma.begin(), sigma.end(), re);
	return Eigen::Map<Eigen::VectorXi>(sigma.data(), n);
}

//! generate a vector that the binary representation is val.
Eigen::VectorXi toSigma(int length, uint32_t val)
{
	Eigen::VectorXi res(length);
	for(int i = 0; i < length; i++)
	{
		res(i) = 1-2*((val >> i) & 1);
	}
	return res;
}

//! returns the binary representation of sigma.
long long int toValue(const Eigen::VectorXi& sigma)
{
	long long int res = 0;
	for(int i = 0; i < sigma.size(); i++)
	{
		res += (1 << i)*((1-sigma(i))/2);
	}
	return res;
}

//! for complex type T, generate a vector filled with samples from normal distribution.
template<typename T, class RandomEngine, typename std::enable_if<is_complex_type<T>::value, int>::type = 0 >
Eigen::Matrix<T, Eigen::Dynamic, 1> randomVector(RandomEngine&& re,
		remove_complex_t<T> sigma, std::size_t nelt)
{
	Eigen::Matrix<T, Eigen::Dynamic, 1> res(nelt);
	std::normal_distribution<typename remove_complex<T>::type> dist(0.0, sigma);
	for(std::size_t i = 0; i < nelt; i++)
	{
		res(i) = T{dist(re),dist(re)};
	}
	return res;
}
//! for real type T, generate a vector filled with samples from normal distribution.
template<typename T, class RandomEngine, typename std::enable_if<!is_complex_type<T>::value, int>::type = 0 >
Eigen::Matrix<T, Eigen::Dynamic, 1> randomVector(RandomEngine&& re,
		remove_complex_t<T> sigma, std::size_t nelt)
{
	Eigen::Matrix<T, Eigen::Dynamic, 1> res(nelt);
	std::normal_distribution<T> dist(0.0, sigma);
	for(std::size_t i = 0; i < nelt; i++)
	{
		res(i) = dist(re);
	}
	return res;
}

}//namespace yannq

#endif//CY_NNQS_UTILITY_HPP