#include "TextureAnalysis.hpp"

#include <math.h>

#include <Eigen/Eigenvalues>
#include <algorithm>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iomanip>

const int white_color = 255;
const int black_color = 0;

using namespace glcm;

namespace fs = std::filesystem;

TextureAnalysis::TextureAnalysis(int Ng) : _Ng(Ng) {
    if (Ng > 0) {
        // initialize probability matrices
        _P_H.resize(_Ng, std::vector<int>(_Ng));
        _P_V.resize(_Ng, std::vector<int>(_Ng));
        _P_LD.resize(_Ng, std::vector<int>(_Ng));
        _P_RD.resize(_Ng, std::vector<int>(_Ng));

        _p_H.resize(_Ng, std::vector<double>(_Ng));
        _p_V.resize(_Ng, std::vector<double>(_Ng));
        _p_LD.resize(_Ng, std::vector<double>(_Ng));
        _p_RD.resize(_Ng, std::vector<double>(_Ng));

        // initialize probability vectors
        _px_H.resize(_Ng);
        _px_V.resize(_Ng);
        _px_LD.resize(_Ng);
        _px_RD.resize(_Ng);

        _py_H.resize(_Ng);
        _py_V.resize(_Ng);
        _py_LD.resize(_Ng);
        _py_RD.resize(_Ng);

        _p_xpy_H.resize(2 * _Ng - 1);
        _p_xpy_V.resize(2 * _Ng - 1);
        _p_xpy_LD.resize(2 * _Ng - 1);
        _p_xpy_RD.resize(2 * _Ng - 1);

        _p_xny_H.resize(_Ng);
        _p_xny_V.resize(_Ng);
        _p_xny_LD.resize(_Ng);
        _p_xny_RD.resize(_Ng);

        // reset factors as zeros
        ResetFactors();
    } else {
        std::cerr << "Invalid Ng assignment (Ng < 0)!\n";
    }
}

void TextureAnalysis::ProcessRectImage(const cv::Mat &image, int distance) {
    // Clear the cache
    ResetCache();

    // Calculate matrices elements: central pixel coord (m ,n), where "m" is the row index, and "n" is the column index
    for (int m = 0; m < image.rows; ++m) {
        for (int n = 0; n < image.cols; ++n) {
            // Nearest neighborhood pixel coord (k ,l), where "k" is the row index, and "l" is the column index
            for (int k = m - distance; k <= m + distance; ++k) {
                for (int l = n - distance; l <= n + distance; ++l) {
                    if ((k >= 0) && (l >= 0) && (k < image.rows) && (l < image.cols)) {
                        //// ToDo: need to check are pixel values in coordinates (m,n) and (k,l) nan or masked!!
                        int j = (int) (image.at<uchar>(m, n)); // I(m,n)
                        int i = (int) (image.at<uchar>(k, l)); // I(k,l)
                        if (((k - m) == 0) && (abs(l - n) == distance)) {
                            CountElemH(i, j);
                        } else if ((((k - m) == distance) && ((l - n) == -distance)) ||
                                   (((k - m) == -distance) && ((l - n) == distance))) {
                            CountElemRD(i, j);
                        } else if ((abs(k - m) == distance) && (l - n == 0)) {
                            CountElemV(i, j);
                        } else if ((((k - m) == distance) && ((l - n) == distance)) ||
                                   (((k - m) == -distance) && ((l - n) == -distance))) {
                            CountElemLD(i, j);
                        } else if ((m == k) && (n == l)) {
                            PushPixelValue(i);
                        } else {
                            // if ((m != k) || (n != l)) {
                            //    cerr << "unknown element:" << endl;
                            //    cerr << "central element: (m, n) = (" << m << "," << n << ")" << endl;
                            //    cerr << "neighborhood element: (k, l) = (" << k << "," << l << ")" << endl;
                            //}
                        }
                    }
                }
            }
        }
    }

    // Normalize the matrices
    Normalization();
}

void TextureAnalysis::ProcessPolygonImage(const cv::Mat &original_image, const cv::Mat &mask_image, int distance) {
    // Clear the cache
    ResetCache();

    // Calculate matrices elements: central pixel coord (m ,n), where "m" is the row index, and "n" is the column index
    int masked = 0;
    int non_masked = 0;
    for (int m = 0; m < original_image.rows; ++m) {
        for (int n = 0; n < original_image.cols; ++n) {
            // Nearest neighborhood pixel coord (k ,l), where "k" is the row index, and "l" is the column index
            for (int k = m - distance; k <= m + distance; ++k) {
                for (int l = n - distance; l <= n + distance; ++l) {
                    if ((k >= 0) && (l >= 0) && (k < original_image.rows) && (l < original_image.cols)) {
                        // Check is the nearest neighborhood pixel coord (k ,l) masked
                        int mask_pixel_value = (int) (mask_image.at<uchar>(k, l));
                        if (mask_pixel_value ==
                            white_color) {             // if nearest neighborhood pixel coord (k ,l) is not masked
                            int j = (int) (original_image.at<uchar>(m, n)); // I(m,n)
                            int i = (int) (original_image.at<uchar>(k, l)); // I(k,l)
                            if (((k - m) == 0) && (abs(l - n) == distance)) {
                                CountElemH(i, j);
                            } else if ((((k - m) == distance) && ((l - n) == -distance)) ||
                                       (((k - m) == -distance) && ((l - n) == distance))) {
                                CountElemRD(i, j);
                            } else if ((abs(k - m) == distance) && (l - n == 0)) {
                                CountElemV(i, j);
                            } else if ((((k - m) == distance) && ((l - n) == distance)) ||
                                       (((k - m) == -distance) && ((l - n) == -distance))) {
                                CountElemLD(i, j);
                            } else if ((m == k) && (n == l)) {
                                PushPixelValue(i);
                            } else {
                                // if ((m != k) || (n != l)) {
                                //    cerr << "unknown element:" << endl;
                                //    cerr << "central element: (m, n) = (" << m << "," << n << ")" << endl;
                                //    cerr << "neighborhood element: (k, l) = (" << k << "," << l << ")" << endl;
                                //}
                            }
                            ++non_masked;
                        } else {
                            // cerr << "masked coord: (k, l) = (" << k << "," << l << "), pixel value = " << mask_pixel_value << endl;
                            ++masked;
                        }
                    }
                }
            }
        }
    }

    // Normalize the matrices
    Normalization();
}

void TextureAnalysis::ResetCache() {
    // reset probability matrices as zeros
    for (int i = 0; i < _Ng; ++i) {
        std::fill(_P_H[i].begin(), _P_H[i].end(), 0);
        std::fill(_P_V[i].begin(), _P_V[i].end(), 0);
        std::fill(_P_LD[i].begin(), _P_LD[i].end(), 0);
        std::fill(_P_RD[i].begin(), _P_RD[i].end(), 0);

        std::fill(_p_H[i].begin(), _p_H[i].end(), 0);
        std::fill(_p_V[i].begin(), _p_V[i].end(), 0);
        std::fill(_p_LD[i].begin(), _p_LD[i].end(), 0);
        std::fill(_p_RD[i].begin(), _p_RD[i].end(), 0);
    }

    _pixel_values.clear();
    _pixel_values_mean = std::numeric_limits<double>::quiet_NaN();
    _pixel_values_STD = std::numeric_limits<double>::quiet_NaN();

    // reset probability vectors as zeros
    std::fill(_px_H.begin(), _px_H.end(), 0);
    std::fill(_px_V.begin(), _px_V.end(), 0);
    std::fill(_px_LD.begin(), _px_LD.end(), 0);
    std::fill(_px_RD.begin(), _px_RD.end(), 0);

    std::fill(_py_H.begin(), _py_H.end(), 0);
    std::fill(_py_V.begin(), _py_V.end(), 0);
    std::fill(_py_LD.begin(), _py_LD.end(), 0);
    std::fill(_py_RD.begin(), _py_RD.end(), 0);

    std::fill(_p_xpy_H.begin(), _p_xpy_H.end(), 0);
    std::fill(_p_xpy_V.begin(), _p_xpy_V.end(), 0);
    std::fill(_p_xpy_LD.begin(), _p_xpy_LD.end(), 0);
    std::fill(_p_xpy_RD.begin(), _p_xpy_RD.end(), 0);

    std::fill(_p_xny_H.begin(), _p_xny_H.end(), 0);
    std::fill(_p_xny_V.begin(), _p_xny_V.end(), 0);
    std::fill(_p_xny_LD.begin(), _p_xny_LD.end(), 0);
    std::fill(_p_xny_RD.begin(), _p_xny_RD.end(), 0);

    // reset factors as zeros
    ResetFactors();
}

void TextureAnalysis::ResetFactors() {
    // reset normalization factors as zeros
    _R_H = 0;
    _R_V = 0;
    _R_LD = 0;
    _R_RD = 0;

    // initialize entropy factors
    _HX_H = 0;
    _HX_V = 0;
    _HX_LD = 0;
    _HX_RD = 0;

    _HY_H = 0;
    _HY_V = 0;
    _HY_LD = 0;
    _HY_RD = 0;

    _HXY_H = 0;
    _HXY_V = 0;
    _HXY_LD = 0;
    _HXY_RD = 0;

    _HXY1_H = 0;
    _HXY1_V = 0;
    _HXY1_LD = 0;
    _HXY1_RD = 0;

    _HXY2_H = 0;
    _HXY2_V = 0;
    _HXY2_LD = 0;
    _HXY2_RD = 0;
}

void TextureAnalysis::CountElemH(int i, int j) {
    ++_P_H[i][j];
    ++_R_H;
}

void TextureAnalysis::CountElemV(int i, int j) {
    ++_P_V[i][j];
    ++_R_V;
}

void TextureAnalysis::CountElemLD(int i, int j) {
    ++_P_LD[i][j];
    ++_R_LD;
}

void TextureAnalysis::CountElemRD(int i, int j) {
    ++_P_RD[i][j];
    ++_R_RD;
}

void TextureAnalysis::PushPixelValue(int pixel_value) {
    _pixel_values.push_back((double) pixel_value);
}

void TextureAnalysis::Normalization() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            _p_H[i][j] = (double) _P_H[i][j] / (double) _R_H;
            _p_V[i][j] = (double) _P_V[i][j] / (double) _R_V;
            _p_LD[i][j] = (double) _P_LD[i][j] / (double) _R_LD;
            _p_RD[i][j] = (double) _P_RD[i][j] / (double) _R_RD;
        }
    }

    // calculate probability vectors
    Calculate_px();
    Calculate_py();
    Calculate_p_xpy();
    Calculate_p_xny();

    // calculate pixels mean and STD in the region
    CalculatePixelSTD(_pixel_values);
}

void TextureAnalysis::Calculate_px() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            _px_H[i] += _p_H[i][j];
            _px_V[i] += _p_V[i][j];
            _px_LD[i] += _p_LD[i][j];
            _px_RD[i] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_py() {
    for (int j = 0; j < _Ng; ++j) {
        for (int i = 0; i < _Ng; ++i) {
            _py_H[j] += _p_H[i][j];
            _py_V[j] += _p_V[i][j];
            _py_LD[j] += _p_LD[i][j];
            _py_RD[j] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_p_xpy() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int k = i + j;
            _p_xpy_H[k] += _p_H[i][j];
            _p_xpy_V[k] += _p_V[i][j];
            _p_xpy_LD[k] += _p_LD[i][j];
            _p_xpy_RD[k] += _p_RD[i][j];
        }
    }
}

void TextureAnalysis::Calculate_p_xny() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int k = abs(i - j);
            _p_xny_H[k] += _p_H[i][j];
            _p_xny_V[k] += _p_V[i][j];
            _p_xny_LD[k] += _p_LD[i][j];
            _p_xny_RD[k] += _p_RD[i][j];
        }
    }
}

double TextureAnalysis::CalculateMean(const std::vector<double> &vec) {
    double sum = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        sum += i * vec[i];
    }
    return sum;
}

double TextureAnalysis::CalculateSTD(const std::vector<double> &vec) {
    double mean = CalculateMean(vec);
    double sum = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        sum += (i - mean) * (i - mean) * vec[i];
    }
    return sqrt(sum);
}

double TextureAnalysis::CalculateGLCMMean_i(const std::vector<std::vector<double>> &mat) {
    double mean = 0.0;
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            mean += i * mat[i][j];
        }
    }
    return mean;
}

double TextureAnalysis::CalculateGLCMMean_j(const std::vector<std::vector<double>> &mat) {
    double mean = 0.0;
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            mean += j * mat[i][j];
        }
    }
    return mean;
}

double TextureAnalysis::CalculateGLCMSTD_i(const std::vector<std::vector<double>> &mat) {
    double mu_x = CalculateGLCMMean_i(mat);
    double sigma_x = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            sigma_x += (i - mu_x) * (i - mu_x) * mat[i][j];
        }
    }

    return sqrt(sigma_x);
}

double TextureAnalysis::CalculateGLCMSTD_j(const std::vector<std::vector<double>> &mat) {
    double mu_y = CalculateGLCMMean_j(mat);
    double sigma_y = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            sigma_y += (j - mu_y) * (j - mu_y) * mat[i][j];
        }
    }

    return sqrt(sigma_y);
}

void TextureAnalysis::CalculateHX() {
    for (int i = 0; i < _Ng; ++i) {
        if (_px_H[i] > 0) {
            _HX_H -= _px_H[i] * log(_px_H[i]);
        }
        if (_px_V[i] > 0) {
            _HX_V -= _px_V[i] * log(_px_V[i]);
        }
        if (_px_LD[i] > 0) {
            _HX_LD -= _px_LD[i] * log(_px_LD[i]);
        }
        if (_px_RD[i] > 0) {
            _HX_RD -= _px_RD[i] * log(_px_RD[i]);
        }
    }
}

void TextureAnalysis::CalculateHY() {
    for (int i = 0; i < _Ng; ++i) {
        if (_py_H[i] > 0) {
            _HY_H -= _py_H[i] * log(_py_H[i]);
        }
        if (_py_V[i] > 0) {
            _HY_V -= _py_V[i] * log(_py_V[i]);
        }
        if (_py_LD[i] > 0) {
            _HY_LD -= _py_LD[i] * log(_py_LD[i]);
        }
        if (_py_RD[i] > 0) {
            _HY_RD -= _py_RD[i] * log(_py_RD[i]);
        }
    }
}

void TextureAnalysis::CalculateHXY() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > 0) {
                _HXY_H -= _p_H[i][j] * log(_p_H[i][j]);
            }
            if (_p_V[i][j] > 0) {
                _HXY_V -= _p_V[i][j] * log(_p_V[i][j]);
            }
            if (_p_LD[i][j] > 0) {
                _HXY_LD -= _p_LD[i][j] * log(_p_LD[i][j]);
            }
            if (_p_RD[i][j] > 0) {
                _HXY_RD -= _p_RD[i][j] * log(_p_RD[i][j]);
            }
        }
    }
}

void TextureAnalysis::CalculateHXY1() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_px_H[i] * _py_H[j] > 0) {
                _HXY1_H -= _p_H[i][j] * log(_px_H[i] * _py_H[j]);
            }
            if (_px_V[i] * _py_V[j] > 0) {
                _HXY1_V -= _p_V[i][j] * log(_px_V[i] * _py_V[j]);
            }
            if (_px_LD[i] * _py_LD[j] > 0) {
                _HXY1_LD -= _p_LD[i][j] * log(_px_LD[i] * _py_LD[j]);
            }
            if (_px_RD[i] * _py_RD[j] > 0) {
                _HXY1_RD -= _p_RD[i][j] * log(_px_RD[i] * _py_RD[j]);
            }
        }
    }
}

void TextureAnalysis::CalculateHXY2() {
    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_px_H[i] * _py_H[j] > 0) {
                _HXY2_H -= _px_H[i] * _py_H[j] * log(_px_H[i] * _py_H[j]);
            }
            if (_px_V[i] * _py_V[j] > 0) {
                _HXY2_V -= _px_V[i] * _py_V[j] * log(_px_V[i] * _py_V[j]);
            }
            if (_px_LD[i] * _py_LD[j] > 0) {
                _HXY2_LD -= _px_LD[i] * _py_LD[j] * log(_px_LD[i] * _py_LD[j]);
            }
            if (_px_RD[i] * _py_RD[j] > 0) {
                _HXY2_RD -= _px_RD[i] * _py_RD[j] * log(_px_RD[i] * _py_RD[j]);
            }
        }
    }
}

Features TextureAnalysis::CalculateQ(int i, int j) {
    double Q_H = 0.0;
    double Q_V = 0.0;
    double Q_LD = 0.0;
    double Q_RD = 0.0;

    for (int k = 0; k < _Ng; ++k) {
        if ((_px_H[i] * _py_H[k]) != 0) {
            Q_H += (_p_H[i][k] * _p_H[j][k]) / (_px_H[i] * _py_H[k]);
        }
        if ((_px_V[i] * _py_V[k]) != 0) {
            Q_V += (_p_V[i][k] * _p_V[j][k]) / (_px_V[i] * _py_V[k]);
        }
        if ((_px_LD[i] * _py_LD[k]) != 0) {
            Q_LD += (_p_LD[i][k] * _p_LD[j][k]) / (_px_LD[i] * _py_LD[k]);
        }
        if ((_px_RD[i] * _py_RD[k]) != 0) {
            Q_RD += (_p_RD[i][k] * _p_RD[j][k]) / (_px_RD[i] * _py_RD[k]);
        }
    }

    return {Q_H, Q_V, Q_LD, Q_RD};
}

//===============================================================================================================
// Calculate texture feature coefficients
//===============================================================================================================

void TextureAnalysis::GetEnergy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] * _p_H[i][j];
            f_V += _p_V[i][j] * _p_V[i][j];
            f_LD += _p_LD[i][j] * _p_LD[i][j];
            f_RD += _p_RD[i][j] * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetContrast(Features &f) {
    std::vector<double> sub_H(_Ng);
    std::vector<double> sub_V(_Ng);
    std::vector<double> sub_LD(_Ng);
    std::vector<double> sub_RD(_Ng);

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            int n = abs(i - j);
            sub_H[n] += _p_H[i][j];
            sub_V[n] += _p_V[i][j];
            sub_LD[n] += _p_LD[i][j];
            sub_RD[n] += _p_RD[i][j];
        }
    }

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int n = 0; n < _Ng; ++n) {
        f_H += (n * n) * sub_H[n];
        f_V += (n * n) * sub_V[n];
        f_LD += (n * n) * sub_LD[n];
        f_RD += (n * n) * sub_RD[n];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetContrastAnotherWay(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - j) * (i - j) * _p_H[i][j];
            f_V += (i - j) * (i - j) * _p_V[i][j];
            f_LD += (i - j) * (i - j) * _p_LD[i][j];
            f_RD += (i - j) * (i - j) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationI(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mu_x_H) * (j - mu_y_H) * _p_H[i][j] / (sigma_x_H * sigma_y_H);
            f_V += (i - mu_x_V) * (j - mu_y_V) * _p_V[i][j] / (sigma_x_V * sigma_y_V);
            f_LD += (i - mu_x_LD) * (j - mu_y_LD) * _p_LD[i][j] / (sigma_x_LD * sigma_y_LD);
            f_RD += (i - mu_x_RD) * (j - mu_y_RD) * _p_RD[i][j] / (sigma_x_RD * sigma_y_RD);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIAnotherWay(Features &f) {
    // Calculate means
    double mu_x_H = CalculateGLCMMean_i(_p_H);
    double mu_x_V = CalculateGLCMMean_i(_p_V);
    double mu_x_LD = CalculateGLCMMean_i(_p_LD);
    double mu_x_RD = CalculateGLCMMean_i(_p_RD);

    double mu_y_H = CalculateGLCMMean_j(_p_H);
    double mu_y_V = CalculateGLCMMean_j(_p_V);
    double mu_y_LD = CalculateGLCMMean_j(_p_LD);
    double mu_y_RD = CalculateGLCMMean_j(_p_RD);

    // Calculate STDs
    double sigma_x_H = CalculateGLCMSTD_i(_p_H);
    double sigma_x_V = CalculateGLCMSTD_i(_p_V);
    double sigma_x_LD = CalculateGLCMSTD_i(_p_LD);
    double sigma_x_RD = CalculateGLCMSTD_i(_p_RD);

    double sigma_y_H = CalculateGLCMSTD_j(_p_H);
    double sigma_y_V = CalculateGLCMSTD_j(_p_V);
    double sigma_y_LD = CalculateGLCMSTD_j(_p_LD);
    double sigma_y_RD = CalculateGLCMSTD_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mu_x_H) * (j - mu_y_H) * _p_H[i][j] / (sigma_x_H * sigma_y_H);
            f_V += (i - mu_x_V) * (j - mu_y_V) * _p_V[i][j] / (sigma_x_V * sigma_y_V);
            f_LD += (i - mu_x_LD) * (j - mu_y_LD) * _p_LD[i][j] / (sigma_x_LD * sigma_y_LD);
            f_RD += (i - mu_x_RD) * (j - mu_y_RD) * _p_RD[i][j] / (sigma_x_RD * sigma_y_RD);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationII(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIIAnotherWay(Features &f) {
    // Calculate means
    double mu_x_H = CalculateGLCMMean_i(_p_H);
    double mu_x_V = CalculateGLCMMean_i(_p_V);
    double mu_x_LD = CalculateGLCMMean_i(_p_LD);
    double mu_x_RD = CalculateGLCMMean_i(_p_RD);

    double mu_y_H = CalculateGLCMMean_j(_p_H);
    double mu_y_V = CalculateGLCMMean_j(_p_V);
    double mu_y_LD = CalculateGLCMMean_j(_p_LD);
    double mu_y_RD = CalculateGLCMMean_j(_p_RD);

    // Calculate STDs
    double sigma_x_H = CalculateGLCMSTD_i(_p_H);
    double sigma_x_V = CalculateGLCMSTD_i(_p_V);
    double sigma_x_LD = CalculateGLCMSTD_i(_p_LD);
    double sigma_x_RD = CalculateGLCMSTD_i(_p_RD);

    double sigma_y_H = CalculateGLCMSTD_j(_p_H);
    double sigma_y_V = CalculateGLCMSTD_j(_p_V);
    double sigma_y_LD = CalculateGLCMSTD_j(_p_LD);
    double sigma_y_RD = CalculateGLCMSTD_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetCorrelationIII(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    // Calculate STDs
    double sigma_x_H = CalculateSTD(_px_H);
    double sigma_x_V = CalculateSTD(_px_V);
    double sigma_x_LD = CalculateSTD(_px_LD);
    double sigma_x_RD = CalculateSTD(_px_RD);

    double sigma_y_H = CalculateSTD(_py_H);
    double sigma_y_V = CalculateSTD(_py_V);
    double sigma_y_LD = CalculateSTD(_py_LD);
    double sigma_y_RD = CalculateSTD(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i * j) * _p_H[i][j];
            f_V += (i * j) * _p_V[i][j];
            f_LD += (i * j) * _p_LD[i][j];
            f_RD += (i * j) * _p_RD[i][j];
        }
    }

    f_H = (f_H - (mu_x_H * mu_y_H)) / (sigma_x_H * sigma_y_H * sigma_x_H * sigma_y_H);
    f_V = (f_V - (mu_x_V * mu_y_V)) / (sigma_x_V * sigma_y_V * sigma_x_V * sigma_y_V);
    f_LD = (f_LD - (mu_x_LD * mu_y_LD)) / (sigma_x_LD * sigma_y_LD * sigma_x_LD * sigma_y_LD);
    f_RD = (f_RD - (mu_x_RD * mu_y_RD)) / (sigma_x_RD * sigma_y_RD * sigma_x_RD * sigma_y_RD);

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares(Features &f) {
    double mean_H_x = CalculateGLCMMean_i(_p_H);
    double mean_V_x = CalculateGLCMMean_i(_p_V);
    double mean_LD_x = CalculateGLCMMean_i(_p_LD);
    double mean_RD_x = CalculateGLCMMean_i(_p_RD);

    double mean_H_y = CalculateGLCMMean_j(_p_H);
    double mean_V_y = CalculateGLCMMean_j(_p_V);
    double mean_LD_y = CalculateGLCMMean_j(_p_LD);
    double mean_RD_y = CalculateGLCMMean_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mean_H_x) * (i - mean_H_x) * _p_H[i][j] + (j - mean_H_y) * (j - mean_H_y) * _p_H[i][j];
            f_V += (i - mean_V_x) * (i - mean_V_x) * _p_V[i][j] + (j - mean_V_y) * (j - mean_V_y) * _p_V[i][j];
            f_LD += (i - mean_LD_x) * (i - mean_LD_x) * _p_LD[i][j] + (j - mean_LD_y) * (j - mean_LD_y) * _p_LD[i][j];
            f_RD += (i - mean_RD_x) * (i - mean_RD_x) * _p_RD[i][j] + (j - mean_RD_y) * (j - mean_RD_y) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares_i(Features &f) {
    double mean_H = CalculateGLCMMean_i(_p_H);
    double mean_V = CalculateGLCMMean_i(_p_V);
    double mean_LD = CalculateGLCMMean_i(_p_LD);
    double mean_RD = CalculateGLCMMean_i(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (i - mean_H) * (i - mean_H) * _p_H[i][j];
            f_V += (i - mean_V) * (i - mean_V) * _p_V[i][j];
            f_LD += (i - mean_LD) * (i - mean_LD) * _p_LD[i][j];
            f_RD += (i - mean_RD) * (i - mean_RD) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumOfSquares_j(Features &f) {
    double mean_H = CalculateGLCMMean_j(_p_H);
    double mean_V = CalculateGLCMMean_j(_p_V);
    double mean_LD = CalculateGLCMMean_j(_p_LD);
    double mean_RD = CalculateGLCMMean_j(_p_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += (j - mean_H) * (j - mean_H) * _p_H[i][j];
            f_V += (j - mean_V) * (j - mean_V) * _p_V[i][j];
            f_LD += (j - mean_LD) * (j - mean_LD) * _p_LD[i][j];
            f_RD += (j - mean_RD) * (j - mean_RD) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetHomogeneityII(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + (i - j) * (i - j));
            f_V += _p_V[i][j] / (1 + (i - j) * (i - j));
            f_LD += _p_LD[i][j] / (1 + (i - j) * (i - j));
            f_RD += _p_RD[i][j] / (1 + (i - j) * (i - j));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumAverage(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        f_H += i * _p_xpy_H[i];
        f_V += i * _p_xpy_V[i];
        f_LD += i * _p_xpy_LD[i];
        f_RD += i * _p_xpy_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumVariance(Features &f) {
    Features f8;
    GetSumEntropy(f8);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        f_H += (i - f8.H) * (i - f8.H) * _p_xpy_H[i];
        f_V += (i - f8.V) * (i - f8.V) * _p_xpy_V[i];
        f_LD += (i - f8.LD) * (i - f8.LD) * _p_xpy_LD[i];
        f_RD += (i - f8.RD) * (i - f8.RD) * _p_xpy_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetSumEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < (2 * _Ng - 1); ++i) {
        if (_p_xpy_H[i] > 0) {
            f_H -= _p_xpy_H[i] * log(_p_xpy_H[i]);
        }
        if (_p_xpy_V[i] > 0) {
            f_V -= _p_xpy_V[i] * log(_p_xpy_V[i]);
        }
        if (_p_xpy_LD[i] > 0) {
            f_LD -= _p_xpy_LD[i] * log(_p_xpy_LD[i]);
        }
        if (_p_xpy_RD[i] > 0) {
            f_RD -= _p_xpy_RD[i] * log(_p_xpy_RD[i]);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > 0) {
                f_H -= _p_H[i][j] * log(_p_H[i][j]);
            }
            if (_p_V[i][j] > 0) {
                f_V -= _p_V[i][j] * log(_p_V[i][j]);
            }
            if (_p_LD[i][j] > 0) {
                f_LD -= _p_LD[i][j] * log(_p_LD[i][j]);
            }
            if (_p_RD[i][j] > 0) {
                f_RD -= _p_RD[i][j] * log(_p_RD[i][j]);
            }
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDifferenceVariance(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        f_H += i * i * _p_xny_H[i];
        f_V += i * i * _p_xny_V[i];
        f_LD += i * i * _p_xny_LD[i];
        f_RD += i * i * _p_xny_RD[i];
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDifferenceEntropy(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        if (_p_xny_H[i] > 0) {
            f_H -= _p_xny_H[i] * log(_p_xny_H[i]);
        }
        if (_p_xny_V[i] > 0) {
            f_V -= _p_xny_V[i] * log(_p_xny_V[i]);
        }
        if (_p_xny_LD[i] > 0) {
            f_LD -= _p_xny_LD[i] * log(_p_xny_LD[i]);
        }
        if (_p_xny_RD[i] > 0) {
            f_RD -= _p_xny_RD[i] * log(_p_xny_RD[i]);
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInformationMeasuresOfCorrelation(Features &f1, Features &f2) {
    // calculate entropy factors
    CalculateHX();
    CalculateHY();
    CalculateHXY();
    CalculateHXY1();
    CalculateHXY2();

    // calculate the first Information Measures of Correlation
    double f1_H;
    double f1_V;
    double f1_LD;
    double f1_RD;

    f1_H = (_HXY_H - _HXY1_H) / std::max(_HX_H, _HY_H);
    f1_V = (_HXY_V - _HXY1_V) / std::max(_HX_V, _HY_V);
    f1_LD = (_HXY_LD - _HXY1_LD) / std::max(_HX_LD, _HY_LD);
    f1_RD = (_HXY_RD - _HXY1_RD) / std::max(_HX_RD, _HY_RD);

    f1(f1_H, f1_V, f1_LD, f1_RD);

    // calculate the second Information Measures of Correlation
    double f2_H = sqrt(1.0 - exp(-2.0 * (_HXY2_H - _HXY_H)));
    double f2_V = sqrt(1.0 - exp(-2.0 * (_HXY2_V - _HXY_V)));
    double f2_LD = sqrt(1.0 - exp(-2.0 * (_HXY2_LD - _HXY_LD)));
    double f2_RD = sqrt(1.0 - exp(-2.0 * (_HXY2_RD - _HXY_RD)));

    f2(f2_H, f2_V, f2_LD, f2_RD);
}

void TextureAnalysis::GetMaximalCorrelationCoefficient(Features &f) {
    // fill in Q matrices
    Eigen::MatrixXd Q_H(_Ng, _Ng);
    Eigen::MatrixXd Q_V(_Ng, _Ng);
    Eigen::MatrixXd Q_LD(_Ng, _Ng);
    Eigen::MatrixXd Q_RD(_Ng, _Ng);

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            Features q = CalculateQ(i, j);
            // std::cout << "q_H = " << q.H << ", q_V = " << q.V << ", q_LD = " << q.LD << ", q_RD = " << q.RD << std::endl;
            Q_H(i, j) = q.H;
            Q_V(i, j) = q.V;
            Q_LD(i, j) = q.LD;
            Q_RD(i, j) = q.RD;
        }
    }

    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_H(Q_H);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_V(Q_V);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_LD(Q_LD);
    Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver_Q_RD(Q_RD);

    // get eigenvalues
    std::vector<double> eigens_H;
    std::vector<double> eigens_V;
    std::vector<double> eigens_LD;
    std::vector<double> eigens_RD;

    for (int i = 0; i < _Ng; ++i) {
        std::complex<double> E_H = eigen_solver_Q_H.eigenvalues().col(0)[i];
        std::complex<double> E_V = eigen_solver_Q_V.eigenvalues().col(0)[i];
        std::complex<double> E_LD = eigen_solver_Q_LD.eigenvalues().col(0)[i];
        std::complex<double> E_RD = eigen_solver_Q_RD.eigenvalues().col(0)[i];

        eigens_H.push_back(E_H.real());
        eigens_V.push_back(E_V.real());
        eigens_LD.push_back(E_LD.real());
        eigens_RD.push_back(E_RD.real());
    }

    // get second largest eigenvalues
    std::nth_element(eigens_H.begin(), eigens_H.begin() + 1, eigens_H.end(), std::greater<double>());
    std::nth_element(eigens_V.begin(), eigens_V.begin() + 1, eigens_V.end(), std::greater<double>());
    std::nth_element(eigens_LD.begin(), eigens_LD.begin() + 1, eigens_LD.end(), std::greater<double>());
    std::nth_element(eigens_RD.begin(), eigens_RD.begin() + 1, eigens_RD.end(), std::greater<double>());

    f(eigens_H[1], eigens_V[1], eigens_LD[1], eigens_RD[1]);
}

void TextureAnalysis::GetMean(Features &f) {
    double f_H = _pixel_values_mean;
    double f_V = _pixel_values_mean;
    double f_LD = _pixel_values_mean;
    double f_RD = _pixel_values_mean;
    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetStd(Features &f) {
    double f_H = _pixel_values_STD;
    double f_V = _pixel_values_STD;
    double f_LD = _pixel_values_STD;
    double f_RD = _pixel_values_STD;
    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetAutoCorrelation(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += i * j * _p_H[i][j];
            f_V += i * j * _p_V[i][j];
            f_LD += i * j * _p_LD[i][j];
            f_RD += i * j * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetClusterProminence(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += pow((i + j - mu_x_H - mu_y_H), 4) * _p_H[i][j];
            f_V += pow((i + j - mu_x_V - mu_y_V), 4) * _p_V[i][j];
            f_LD += pow((i + j - mu_x_LD - mu_y_LD), 4) * _p_LD[i][j];
            f_RD += pow((i + j - mu_x_RD - mu_y_RD), 4) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetClusterShade(Features &f) {
    // Calculate means
    double mu_x_H = CalculateMean(_px_H);
    double mu_x_V = CalculateMean(_px_V);
    double mu_x_LD = CalculateMean(_px_LD);
    double mu_x_RD = CalculateMean(_px_RD);

    double mu_y_H = CalculateMean(_py_H);
    double mu_y_V = CalculateMean(_py_V);
    double mu_y_LD = CalculateMean(_py_LD);
    double mu_y_RD = CalculateMean(_py_RD);

    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += pow((i + j - mu_x_H - mu_y_H), 3) * _p_H[i][j];
            f_V += pow((i + j - mu_x_V - mu_y_V), 3) * _p_V[i][j];
            f_LD += pow((i + j - mu_x_LD - mu_y_LD), 3) * _p_LD[i][j];
            f_RD += pow((i + j - mu_x_RD - mu_y_RD), 3) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetDissimilarity(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += fabs(i - j) * _p_H[i][j];
            f_V += fabs(i - j) * _p_V[i][j];
            f_LD += fabs(i - j) * _p_LD[i][j];
            f_RD += fabs(i - j) * _p_RD[i][j];
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetHomogeneityI(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + fabs(i - j));
            f_V += _p_V[i][j] / (1 + fabs(i - j));
            f_LD += _p_LD[i][j] / (1 + fabs(i - j));
            f_RD += _p_RD[i][j] / (1 + fabs(i - j));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetMaximumProbability(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            if (_p_H[i][j] > f_H) {
                f_H = _p_H[i][j];
            }
            if (_p_V[i][j] > f_V) {
                f_V = _p_V[i][j];
            }
            if (_p_LD[i][j] > f_LD) {
                f_LD = _p_LD[i][j];
            }
            if (_p_RD[i][j] > f_RD) {
                f_RD = _p_RD[i][j];
            }
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInverseDifferenceNormalized(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_V += _p_V[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_LD += _p_LD[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
            f_RD += _p_RD[i][j] / (1 + (abs(i - j) * abs(i - j) / _Ng));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

void TextureAnalysis::GetInverseDifferenceMomentNormalized(Features &f) {
    double f_H = 0.0;
    double f_V = 0.0;
    double f_LD = 0.0;
    double f_RD = 0.0;

    for (int i = 0; i < _Ng; ++i) {
        for (int j = 0; j < _Ng; ++j) {
            f_H += _p_H[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_V += _p_V[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_LD += _p_LD[i][j] / (1 + ((i - j) * (i - j) / _Ng));
            f_RD += _p_RD[i][j] / (1 + ((i - j) * (i - j) / _Ng));
        }
    }

    f(f_H, f_V, f_LD, f_RD);
}

std::map<Type, Features> TextureAnalysis::Calculate(const std::set<Type> &types) {
    std::map<Type, Features> results;
    bool information_measures_of_correlation_done = false;
    for (auto type : types) {
        switch (type) {
            case Type::Mean:
                GetMean(results[Type::Mean]);
                break;
            case Type::Std:
                GetStd(results[Type::Std]);
                break;
            case Type::AutoCorrelation:
                GetAutoCorrelation(results[Type::AutoCorrelation]);
                break;
            case Type::Contrast:
                GetContrast(results[Type::Contrast]);
                break;
            case Type::ContrastAnotherWay:
                GetContrastAnotherWay(results[Type::ContrastAnotherWay]);
                break;
            case Type::CorrelationI:
                GetCorrelationI(results[Type::CorrelationI]);
                break;
            case Type::CorrelationIAnotherWay:
                GetCorrelationIAnotherWay(results[Type::CorrelationIAnotherWay]);
                break;
            case Type::CorrelationII:
                GetCorrelationII(results[Type::CorrelationII]);
                break;
            case Type::CorrelationIIAnotherWay:
                GetCorrelationIIAnotherWay(results[Type::CorrelationIIAnotherWay]);
                break;
            case Type::CorrelationIII:
                GetCorrelationIII(results[Type::CorrelationIII]);
                break;
            case Type::ClusterProminence:
                GetClusterProminence(results[Type::ClusterProminence]);
                break;
            case Type::ClusterShade:
                GetClusterShade(results[Type::ClusterShade]);
                break;
            case Type::Dissimilarity:
                GetDissimilarity(results[Type::Dissimilarity]);
                break;
            case Type::Energy:
                GetEnergy(results[Type::Energy]);
                break;
            case Type::Entropy:
                GetEntropy(results[Type::Entropy]);
                break;
            case Type::HomogeneityI:
                GetHomogeneityI(results[Type::HomogeneityI]);
                break;
            case Type::HomogeneityII:
                GetHomogeneityII(results[Type::HomogeneityII]);
                break;
            case Type::MaximumProbability:
                GetMaximumProbability(results[Type::MaximumProbability]);
                break;
            case Type::SumOfSquares:
                GetSumOfSquares(results[Type::SumOfSquares]);
                break;
            case Type::SumOfSquaresI:
                GetSumOfSquares_i(results[Type::SumOfSquaresI]);
                break;
            case Type::SumOfSquaresJ:
                GetSumOfSquares_j(results[Type::SumOfSquaresJ]);
                break;
            case Type::SumAverage:
                GetSumAverage(results[Type::SumAverage]);
                break;
            case Type::SumEntropy:
                GetSumEntropy(results[Type::SumEntropy]);
                break;
            case Type::SumVariance:
                GetSumVariance(results[Type::SumVariance]);
                break;
            case Type::DifferenceVariance:
                GetDifferenceVariance(results[Type::DifferenceVariance]);
                break;
            case Type::DifferenceEntropy:
                GetDifferenceEntropy(results[Type::DifferenceEntropy]);
                break;
            case Type::InformationMeasuresOfCorrelationI:
                if (!information_measures_of_correlation_done) {
                    GetInformationMeasuresOfCorrelation(
                            results[Type::InformationMeasuresOfCorrelationI],
                            results[Type::InformationMeasuresOfCorrelationII]);
                    information_measures_of_correlation_done = true;
                }
                break;
            case Type::InformationMeasuresOfCorrelationII:
                if (!information_measures_of_correlation_done) {
                    GetInformationMeasuresOfCorrelation(
                            results[Type::InformationMeasuresOfCorrelationI],
                            results[Type::InformationMeasuresOfCorrelationII]);
                    information_measures_of_correlation_done = true;
                }
                break;
            case Type::InverseDifferenceNormalized:
                GetInverseDifferenceNormalized(results[Type::InverseDifferenceNormalized]);
                break;
            case Type::InverseDifferenceMomentNormalized:
                GetInverseDifferenceMomentNormalized(results[Type::InverseDifferenceMomentNormalized]);
                break;
            default:
                std::cerr << "Unknown feature type!\n";
                break;
        }
    }

    return results;
}

void TextureAnalysis::CalculateScore(double age, std::map<Type, Features> &features_map) {
    if ((age > 0) && features_map.count(Type::Mean) && features_map.count(Type::Entropy) &&
        features_map.count(Type::Contrast)) {
        std::vector<double> params = {1.138, -1.814, 1.416, 1.714};

        if (age < 60) {
            age = 0;
        } else {
            age = 1;
        }

        double intensity_H = features_map.at(Type::Mean).H;
        double intensity_V = features_map.at(Type::Mean).V;
        double intensity_LD = features_map.at(Type::Mean).LD;
        double intensity_RD = features_map.at(Type::Mean).RD;
        double intensity = (intensity_H + intensity_V + intensity_LD + intensity_RD) / 4.0;

        if (intensity < 51.39) {
            intensity = 0;
        } else {
            intensity = 1;
        }

        double entropy_H = features_map.at(Type::Entropy).H;
        double entropy_V = features_map.at(Type::Entropy).V;
        double entropy_LD = features_map.at(Type::Entropy).LD;
        double entropy_RD = features_map.at(Type::Entropy).RD;
        double entropy = (entropy_H + entropy_V + entropy_LD + entropy_RD) / 4.0;

        if (entropy < 7.119) {
            entropy = 0;
        } else {
            entropy = 1;
        }

        double contrast_H = features_map.at(Type::Contrast).H;
        double contrast_V = features_map.at(Type::Contrast).V;
        double contrast_LD = features_map.at(Type::Contrast).LD;
        double contrast_RD = features_map.at(Type::Contrast).RD;
        double contrast = (contrast_H + contrast_V + contrast_LD + contrast_RD) / 4.0 / 10.0;

        if (contrast < 7.1491) {
            contrast = 0;
        } else {
            contrast = 1;
        }

        double f_H = params[0] * age + params[1] * intensity + params[2] * entropy +
                     params[3] * contrast;
        double f_V = params[0] * age + params[1] * intensity + params[2] * entropy +
                     params[3] * contrast;
        double f_LD = params[0] * age + params[1] * intensity + params[2] * entropy +
                      params[3] * contrast;
        double f_RD = params[0] * age + params[1] * intensity + params[2] * entropy +
                      params[3] * contrast;

        features_map[Type::Score](f_H, f_V, f_LD, f_RD);
        features_map[Type::Age](age, age, age, age);
    } else {
        std::cerr << "Can not calculate the Score!\n";
    }
}

std::string TextureAnalysis::TypeToString(const Type &type) {
    std::string result;
    switch (type) {
        case Type::Mean:
            result = "Mean";
            break;
        case Type::Std:
            result = "STD";
            break;
        case Type::AutoCorrelation:
            result = "Auto Correlation";
            break;
        case Type::Contrast:
            result = "Contrast";
            break;
        case Type::ContrastAnotherWay:
            result = "Contrast (Check)";
            break;
        case Type::CorrelationI:
            result = "Correlation I";
            break;
        case Type::CorrelationIAnotherWay:
            result = "Correlation I (Check)";
            break;
        case Type::CorrelationII:
            result = "Correlation II";
            break;
        case Type::CorrelationIIAnotherWay:
            result = "Correlation II (Check)";
            break;
        case Type::CorrelationIII:
            result = "Correlation III";
            break;
        case Type::ClusterProminence:
            result = "Cluster Prominence";
            break;
        case Type::ClusterShade:
            result = "Cluster Shade";
            break;
        case Type::Dissimilarity:
            result = "Dissimilarity";
            break;
        case Type::Energy:
            result = "Energy";
            break;
        case Type::Entropy:
            result = "Entropy";
            break;
        case Type::HomogeneityI:
            result = "Homogeneity I";
            break;
        case Type::HomogeneityII:
            result = "Homogeneity II (Inverse Difference Moment)";
            break;
        case Type::MaximumProbability:
            result = "Maximum Probability";
            break;
        case Type::SumOfSquares:
            result = "Sum of Squares (in x and y)";
            break;
        case Type::SumOfSquaresI:
            result = "Sum of Squares (in x)";
            break;
        case Type::SumOfSquaresJ:
            result = "Sum of Squares (in y)";
            break;
        case Type::SumAverage:
            result = "Sum Average";
            break;
        case Type::SumEntropy:
            result = "Sum Entropy";
            break;
        case Type::SumVariance:
            result = "Sum Variance";
            break;
        case Type::DifferenceVariance:
            result = "Difference Variance";
            break;
        case Type::DifferenceEntropy:
            result = "Difference Entropy";
            break;
        case Type::InformationMeasuresOfCorrelationI:
            result = "Information Measures of Correlation I";
            break;
        case Type::InformationMeasuresOfCorrelationII:
            result = "Information Measures of Correlation II";
            break;
        case Type::InverseDifferenceNormalized:
            result = "Inverse Difference Normalized";
            break;
        case Type::InverseDifferenceMomentNormalized:
            result = "Inverse Difference Moment Normalized";
            break;
        case Type::Score:
            result = "Score";
            break;
        case Type::Age:
            result = "Age";
            break;
        default:
            std::cerr << "Unknown feature type!\n";
            break;
    }
    return result;
}

std::string TextureAnalysis::DirectionToString(const Direction &direction) {
    std::string result;
    switch (direction) {
        case Direction::H:
            result = "H (0 deg)";
            break;
        case Direction::V:
            result = "V (90 deg)";
            break;
        case Direction::LD:
            result = "LD (135 deg)";
            break;
        case Direction::RD:
            result = "RD (45 deg)";
            break;
        case Direction::Avg:
            result = "Average";
            break;
        default:
            std::cerr << "Unknown feature type!\n";
            break;
    }

    return result;
}

void TextureAnalysis::Print(const std::map<Type, Features> &features) {
    for (auto feature : features) {
        std::cout << TypeToString(feature.first) << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::H) << " = " << feature.second.H << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::V) << " = " << feature.second.V << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::LD) << " = " << feature.second.LD << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::RD) << " = " << feature.second.RD << std::endl;
        std::cout << std::setw(30) << DirectionToString(Direction::Avg) << " = " << feature.second.Avg() << std::endl;
    }
}

void TextureAnalysis::SaveAsCSV(const std::string &image_name, std::map<Type, Features> features,
                                const std::string &csv_name) {
    // check whether the csv file exists or not
    bool csv_file_exists = fs::exists(csv_name);

    // get image file base name
    std::string image_base_name = fs::path(image_name).filename().string();

    // get current time
    std::string current_time = GetCurrentTime();

    // open the csv file
    std::ofstream csv_file;
    csv_file.open(csv_name,
                  std::ios::out | std::ios::app); // open as the writing mode and append the csv file at the end

    if (!csv_file) {
        std::cerr << "Can't the open file!" << std::endl;
        return;
    }

    if (!csv_file_exists) {
        // write a row of titles
        csv_file << "Date,";
        csv_file << "Image,";
        csv_file << "Direction,";
        for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
            csv_file << TypeToString(it->first) << ",";
        }
        csv_file << "\n";
    }

    // write a row of H values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::H) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.H << ",";
    }
    csv_file << "\n";

    // write a row of V values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::V) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.V << ",";
    }
    csv_file << "\n";

    // write a row of LD values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::LD) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.LD << ",";
    }
    csv_file << "\n";

    // write a row of RD values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::RD) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.RD << ",";
    }
    csv_file << "\n";

    // write a row of Avg values
    csv_file << current_time << ",";
    csv_file << image_base_name << ",";
    csv_file << DirectionToString(Direction::Avg) << ",";
    for (std::map<Type, Features>::iterator it = features.begin(); it != features.end(); ++it) {
        csv_file << it->second.Avg() << ",";
    }
    csv_file << "\n";

    // close the csv file
    csv_file.close();
}

std::string TextureAnalysis::GetCurrentTime() {
    time_t rawtime;
    struct tm *timeinfo;
    char buffer[80];

    time(&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", timeinfo);
    std::string str(buffer);

    return str;
}

void TextureAnalysis::CalculatePixelMean(const std::vector<double> &vec) {
    _pixel_values_mean = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        _pixel_values_mean += vec[i];
    }
    _pixel_values_mean /= vec.size();
}

void TextureAnalysis::CalculatePixelSTD(const std::vector<double> &vec) {
    CalculatePixelMean(vec);
    _pixel_values_STD = 0.0;
    for (int i = 0; i < vec.size(); ++i) {
        _pixel_values_STD += (vec[i] - _pixel_values_mean) * (vec[i] - _pixel_values_mean);
    }
    _pixel_values_STD = _pixel_values_STD / (vec.size() - 1.0);
    _pixel_values_STD = sqrt(_pixel_values_STD);
}


#pragma once

#include <boost/mpl/identity.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <type_traits>

#include <cmath>

// own includes ---------------------------------------------------------
#include "traits/type_traits.hpp"

namespace math {

// ----------------------------------------------------------------------
// SQRT
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
sqrt(const T& t)
{
  return std::sqrt(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
sqrt(const T& t)
{
  return boost::multiprecision::sqrt(t);
}

// ----------------------------------------------------------------------
// EXP
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
exp(const T& t)
{
  return std::exp(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
exp(const T& t)
{
  return boost::multiprecision::exp(t);
}

// ----------------------------------------------------------------------
// LOG
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
log(const T& t)
{
  return std::log(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
log(const T& t)
{
  return boost::multiprecision::log(t);
}

// ----------------------------------------------------------------------
// POW
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
pow(const T& base, const typename boost::mpl::identity<T>::type& exp)
{
  return std::pow(base, exp);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
pow(const T& base, const typename boost::mpl::identity<T>::type& exp)
{
  return boost::multiprecision::pow(base, exp);
}

// ----------------------------------------------------------------------
// ABS
template <typename T>
inline constexpr typename std::enable_if<std::is_floating_point<T>::value, T>::type
abs(const T& t)
{
  return std::abs(t);
}

template <typename T>
inline constexpr typename std::enable_if<!std::is_floating_point<T>::value, T>::type
abs(const T& t)
{
  return boost::multiprecision::abs(t);
}

}  // end namespace math


/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/** Example partys_tallest_guests.cpp \file partys_tallest_guests.cpp
    \brief Using <i>aggregate on overlap</i> the heights of the party's tallest
           guests are computed.

    In partys_tallest_guests.cpp we use a different instantiation of
    interval map templates to compute maxima of guest heights.

    Instead of aggregating groups of people attending the party in time
    we aggregate the maximum of guest height for the time intervals.

    Using a joining interval_map results in a smaller map: All interval
    value pairs are joined if the maximum does not change in time. Using
    a split_interval_map results in a larger map: All splits of intervals
    that occur due to entering and leaving of guests are preserved in
    the split_interval_map.

    \include partys_tallest_guests_/partys_tallest_guests.cpp
*/
//[example_partys_tallest_guests
// The next line includes <boost/date_time/posix_time/posix_time.hpp>
// and a few lines of adapter code.
#include <boost/icl/ptime.hpp>
#include <iostream>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::icl;

// A party's height shall be defined as the maximum height of all guests ;-)
// The last parameter 'inplace_max' is a functor template that calls a max
// aggregation on overlap.
typedef interval_map<ptime, int, partial_absorber, less, inplace_max>
    PartyHeightHistoryT;

// Using a split_interval_map we preserve interval splittings that occurred via insertion.
typedef split_interval_map<ptime, int, partial_absorber, less, inplace_max>
    PartyHeightSplitHistoryT;

void partys_height()
{
    PartyHeightHistoryT tallest_guest;

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        180); // Mary & Harry: Harry is 1,80 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        170); // Diana & Susan: Diana is 1,70 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        200); // Peters height is 2,00 m

    PartyHeightHistoryT::iterator height_ = tallest_guest.begin();
    cout << "-------------- History of maximum guest height -------------------\n";
    while(height_ != tallest_guest.end())
    {
        discrete_interval<ptime> when = height_->first;
        // Of what height are the tallest guests within the time interval 'when' ?
        int height = (*height_++).second;
        cout << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height <<" cm = " << height/30.48 << " ft" << endl;
    }

}

// Next we are using a split_interval_map instead of a joining interval_map
void partys_split_height()
{
    PartyHeightSplitHistoryT tallest_guest;

    // adding an element can be done wrt. simple aggregate functions
    // like e.g. min, max etc. in their 'inplace' or op= incarnation
    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 19:30"),
          time_from_string("2008-05-20 23:00")),
        180); // Mary & Harry: Harry is 1,80 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 20:10"),
          time_from_string("2008-05-21 00:00")),
        170); // Diana & Susan: Diana is 1,70 m tall.

    tallest_guest +=
      make_pair(
        discrete_interval<ptime>::right_open(
          time_from_string("2008-05-20 22:15"),
          time_from_string("2008-05-21 00:30")),
        200); // Peters height is 2,00 m

    PartyHeightSplitHistoryT::iterator height_ = tallest_guest.begin();
    cout << "\n";
    cout << "-------- Split History of maximum guest height -------------------\n";
    cout << "--- Same map as above but split for every interval insertion.  ---\n";
    while(height_ != tallest_guest.end())
    {
        discrete_interval<ptime> when = height_->first;
        // Of what height are the tallest guests within the time interval 'when' ?
        int height = (*height_++).second;
        cout << "[" << first(when) << " - " << upper(when) << ")"
             << ": " << height <<" cm = " << height/30.48 << " ft" << endl;
    }

}

int main()
{
    cout << ">>Interval Container Library: Sample partys_tallest_guests.cpp  <<\n";
    cout << "------------------------------------------------------------------\n";
    partys_height();
    partys_split_height();
    return 0;
}

// Program output:
/*-----------------------------------------------------------------------------
>>Interval Container Library: Sample partys_tallest_guests.cpp  <<
------------------------------------------------------------------
-------------- History of maximum guest height -------------------
[2008-May-20 19:30:00 - 2008-May-20 22:15:00): 180 cm = 5.90551 ft
[2008-May-20 22:15:00 - 2008-May-21 00:30:00): 200 cm = 6.56168 ft

-------- Split History of maximum guest height -------------------
--- Same map as above but split for every interval insertion.  ---
[2008-May-20 19:30:00 - 2008-May-20 20:10:00): 180 cm = 5.90551 ft
[2008-May-20 20:10:00 - 2008-May-20 22:15:00): 180 cm = 5.90551 ft
[2008-May-20 22:15:00 - 2008-May-20 23:00:00): 200 cm = 6.56168 ft
[2008-May-20 23:00:00 - 2008-May-21 00:00:00): 200 cm = 6.56168 ft
[2008-May-21 00:00:00 - 2008-May-21 00:30:00): 200 cm = 6.56168 ft
-----------------------------------------------------------------------------*/
//]


#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "TrackstersPCA.h"
#include "TPrincipal.h"

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

void ticl::assignPCAtoTracksters(std::vector<Trackster> &tracksters,
                                 const std::vector<reco::CaloCluster> &layerClusters,
                                 double z_limit_em,
                                 bool energyWeight) {
  LogDebug("TrackstersPCA_Eigen") << "------- Eigen -------" << std::endl;

  for (auto &trackster : tracksters) {
    Eigen::Vector3d point;
    point << 0., 0., 0.;
    Eigen::Vector3d barycenter;
    barycenter << 0., 0., 0.;

    auto fillPoint = [&](const reco::CaloCluster &c, const float weight = 1.f) {
      point[0] = weight * c.x();
      point[1] = weight * c.y();
      point[2] = weight * c.z();
    };

    // Initialize this trackster with default, dummy values
    trackster.setRawEnergy(0.f);
    trackster.setRawEmEnergy(0.f);
    trackster.setRawPt(0.f);
    trackster.setRawEmPt(0.f);

    size_t N = trackster.vertices().size();
    float weight = 1.f / N;
    float weights2_sum = 0.f;
    Eigen::Vector3d sigmas;
    sigmas << 0., 0., 0.;
    Eigen::Vector3d sigmasEigen;
    sigmasEigen << 0., 0., 0.;
    Eigen::Matrix3d covM = Eigen::Matrix3d::Zero();

    for (size_t i = 0; i < N; ++i) {
      auto fraction = 1.f / trackster.vertex_multiplicity(i);
      trackster.addToRawEnergy(layerClusters[trackster.vertices(i)].energy() * fraction);
      if (std::abs(layerClusters[trackster.vertices(i)].z()) <= z_limit_em)
        trackster.addToRawEmEnergy(layerClusters[trackster.vertices(i)].energy() * fraction);

      // Compute the weighted barycenter.
      if (energyWeight)
        weight = layerClusters[trackster.vertices(i)].energy() * fraction;
      fillPoint(layerClusters[trackster.vertices(i)], weight);
      for (size_t j = 0; j < 3; ++j)
        barycenter[j] += point[j];
    }
    if (energyWeight && trackster.raw_energy())
      barycenter /= trackster.raw_energy();

    // Compute the Covariance Matrix and the sum of the squared weights, used
    // to compute the correct normalization.
    // The barycenter has to be known.
    for (size_t i = 0; i < N; ++i) {
      fillPoint(layerClusters[trackster.vertices(i)]);
      if (energyWeight && trackster.raw_energy())
        weight =
            (layerClusters[trackster.vertices(i)].energy() / trackster.vertex_multiplicity(i)) / trackster.raw_energy();
      weights2_sum += weight * weight;
      for (size_t x = 0; x < 3; ++x)
        for (size_t y = 0; y <= x; ++y) {
          covM(x, y) += weight * (point[x] - barycenter[x]) * (point[y] - barycenter[y]);
          covM(y, x) = covM(x, y);
        }
    }
    covM *= 1. / (1. - weights2_sum);

    // Perform the actual decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>::RealVectorType eigenvalues_fromEigen;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>::EigenvectorsType eigenvectors_fromEigen;
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covM);
    if (eigensolver.info() != Eigen::Success) {
      eigenvalues_fromEigen = eigenvalues_fromEigen.Zero();
      eigenvectors_fromEigen = eigenvectors_fromEigen.Zero();
    } else {
      eigenvalues_fromEigen = eigensolver.eigenvalues();
      eigenvectors_fromEigen = eigensolver.eigenvectors();
    }

    // Compute the spread in the both spaces.
    for (size_t i = 0; i < N; ++i) {
      fillPoint(layerClusters[trackster.vertices(i)]);
      sigmas += weight * (point - barycenter).cwiseAbs2();
      Eigen::Vector3d point_transformed = eigenvectors_fromEigen * (point - barycenter);
      if (energyWeight && trackster.raw_energy())
        weight =
            (layerClusters[trackster.vertices(i)].energy() / trackster.vertex_multiplicity(i)) / trackster.raw_energy();
      sigmasEigen += weight * (point_transformed.cwiseAbs2());
    }
    sigmas /= (1. - weights2_sum);
    sigmasEigen /= (1. - weights2_sum);

    // Add trackster attributes
    trackster.setBarycenter(ticl::Trackster::Vector(barycenter));
    trackster.fillPCAVariables(
        eigenvalues_fromEigen, eigenvectors_fromEigen, sigmas, sigmasEigen, 3, ticl::Trackster::PCAOrdering::ascending);

    LogDebug("TrackstersPCA") << "Use energy weighting: " << energyWeight << std::endl;
    LogDebug("TrackstersPCA") << "\nTrackster characteristics: " << std::endl;
    LogDebug("TrackstersPCA") << "Size: " << N << std::endl;
    LogDebug("TrackstersPCA") << "Energy: " << trackster.raw_energy() << std::endl;
    LogDebug("TrackstersPCA") << "raw_pt: " << trackster.raw_pt() << std::endl;
    LogDebug("TrackstersPCA") << "Means:          " << barycenter[0] << ", " << barycenter[1] << ", " << barycenter[2]
                              << std::endl;
    LogDebug("TrackstersPCA") << "EigenValues from Eigen/Tr(cov): " << eigenvalues_fromEigen[2] / covM.trace() << ", "
                              << eigenvalues_fromEigen[1] / covM.trace() << ", "
                              << eigenvalues_fromEigen[0] / covM.trace() << std::endl;
    LogDebug("TrackstersPCA") << "EigenValues from Eigen:         " << eigenvalues_fromEigen[2] << ", "
                              << eigenvalues_fromEigen[1] << ", " << eigenvalues_fromEigen[0] << std::endl;
    LogDebug("TrackstersPCA") << "EigenVector 3 from Eigen: " << eigenvectors_fromEigen(0, 2) << ", "
                              << eigenvectors_fromEigen(1, 2) << ", " << eigenvectors_fromEigen(2, 2) << std::endl;
    LogDebug("TrackstersPCA") << "EigenVector 2 from Eigen: " << eigenvectors_fromEigen(0, 1) << ", "
                              << eigenvectors_fromEigen(1, 1) << ", " << eigenvectors_fromEigen(2, 1) << std::endl;
    LogDebug("TrackstersPCA") << "EigenVector 1 from Eigen: " << eigenvectors_fromEigen(0, 0) << ", "
                              << eigenvectors_fromEigen(1, 0) << ", " << eigenvectors_fromEigen(2, 0) << std::endl;
    LogDebug("TrackstersPCA") << "Original sigmas:          " << sigmas[0] << ", " << sigmas[1] << ", " << sigmas[2]
                              << std::endl;
    LogDebug("TrackstersPCA") << "SigmasEigen in PCA space: " << sigmasEigen[2] << ", " << sigmasEigen[1] << ", "
                              << sigmasEigen[0] << std::endl;
    LogDebug("TrackstersPCA") << "covM:     \n" << covM << std::endl;
  }
}


#include <vector>
#include <algorithm>
#include <set>
#include <iostream>
#include <armadillo>

#include "Util.h"
#include "PostCalMultiPop.h"

using namespace arma;

void printGSLPrint(mat &A, int row, int col) {
	for(int i = 0; i < row; i++) {
		for(int j = 0; j < col; j++)
			printf("%g ", A(i, j));
		printf("\n");
	}
}

string PostCalMultiPop::convertConfig2String(int * config, int size) {
	string result = "0";
	for(int i = 0; i < size; i++)
		if(config[i]==1)
			result+= "_" + convertInt(i);
	return result;
}

// We compute dmvnorm(Zcc, mean=rep(0,nrow(Rcc)), Rcc + Rcc %*% Rcc) / dmvnorm(Zcc, rep(0, nrow(Rcc)), Rcc))
// // togheter to avoid numerical over flow
double PostCalMultiPop::fracdmvnorm(mat Z, mat mean, mat R, mat diagC, int pop, double NCP) {
        mat newR = R + R * diagC  * R;
        mat ZcenterMean = Z - mean;
        mat res1 = trans(ZcenterMean) * inv(R) * (ZcenterMean);
        mat res2 = trans(ZcenterMean) * inv(newR) *  (ZcenterMean);

        double v1 = res1(0,0)/2-res2(0,0)/2-baseValue[pop]/2;
        return(exp(v1)/sqrt(det(newR))* sqrt(det(R)));
}

double PostCalMultiPop::dmvnorm(mat Z, mat mean, mat R) {
        mat ZcenterMean = Z - mean;
        mat res = trans(ZcenterMean) * inv(R) * (ZcenterMean);
        double v1 = res(0,0);
        double v2 = log(sqrt(det(R)));
        return (exp(-v1/2-v2));
}

// cc=causal SNPs
// Rcc = LD of causal SNPs
// Zcc = Z-score of causal SNPs
// dmvnorm(Zcc, mean=rep(0,nrow(Rcc)), Rcc + Rcc %*% Rcc) / dmvnorm(Zcc, rep(0, nrow(Rcc)), Rcc))
//
double PostCalMultiPop::fastLikelihood(int * configure, int pop, double NCP) {
	int causalCount = 0;
	vector <int> causalIndex;

	for(int i = 0; i < snpCount; i++) {
		causalCount += configure[i];
		if(configure[i] == 1)
			causalIndex.push_back(i);
	}

	if (causalCount == 0) {
		int maxVal = 0;
		for(int i = 0; i < snpCount; i++) {
			if (maxVal < abs(stat[pop][i]))
				maxVal = stat[pop][i];
		}
		baseValue[pop] = maxVal * maxVal;
	}

	mat Rcc(causalCount, causalCount, fill::zeros);
	mat Zcc(causalCount, 1, fill::zeros);
	mat mean(causalCount, 1, fill::zeros);
	mat diagC(causalCount, causalCount, fill::zeros);

	for (int i = 0; i < causalCount; i++){
		for(int j = 0; j < causalCount; j++) {
			Rcc(i,j) = sigmaMatrix[pop](causalIndex[i], causalIndex[j]);
		}
		Zcc(i,0) = stat[pop][causalIndex[i]];
		diagC(i,i) = NCP;
	}
	return fracdmvnorm(Zcc, mean, Rcc, diagC,pop, NCP);
}

int PostCalMultiPop::nextBinary(int * data, int size) {
	int i = 0;
	int total_one = 0;
	int index = size-1;
        int one_countinus_in_end = 0;

        while(index >= 0 && data[index] == 1) {
                index = index - 1;
                one_countinus_in_end = one_countinus_in_end + 1;
	}
	if(index >= 0) {
        	while(index >= 0 && data[index] == 0) {
               	 index = index - 1;
		}
	}
        if(index == -1) {
                while(i <  one_countinus_in_end+1 && i < size) {
                        data[i] = 1;
                        i=i+1;
		}
                i = 0;
                while(i < size-one_countinus_in_end-1) {
                        data[i+one_countinus_in_end+1] = 0;
                        i=i+1;
		}
	}
        else if(one_countinus_in_end == 0) {
                data[index] = 0;
                data[index+1] = 1;
	} else {
                data[index] = 0;
                while(i < one_countinus_in_end + 1) {
                        data[i+index+1] = 1;
			if(i+index+1 >= size)
				printf("ERROR3 %d\n", i+index+1);
                        i=i+1;
		}
                i = 0;
                while(i < size - index - one_countinus_in_end - 2) {
                        data[i+index+one_countinus_in_end+2] = 0;
			if(i+index+one_countinus_in_end+2 >= size) {
				printf("ERROR4 %d\n", i+index+one_countinus_in_end+2);
			}
                        i=i+1;
		}
	}
	i = 0;
	total_one = 0;
	for(i = 0; i < size; i++)
		if(data[i] == 1)
			total_one = total_one + 1;

	return(total_one);
}

double PostCalMultiPop::computeTotalLikelihood(double NCP) {
	int num = 0;
	double sumLikelihood = 0;
	double tmp_likelihood = 1;
	long int total_iteration = 0 ;
	int * configure = (int *) malloc (snpCount * sizeof(int *)); // original data

	for(long int i = 0; i <= maxCausalSNP; i++)
		total_iteration = total_iteration + nCr(snpCount, i);
	cout << snpCount << endl;
	cout << "Max Causal=" << maxCausalSNP << endl;
	cout << "Total="      << total_iteration << endl;
	for(long int i = 0; i < snpCount; i++)
		configure[i] = 0;
	for(long int i = 0; i < total_iteration; i++) {
		tmp_likelihood = 1;
		for (int pop =0 ; pop < popNum; pop++)
                	tmp_likelihood = tmp_likelihood * fastLikelihood(configure, pop, NCP) * (pow(gamma, num))*(pow(1-gamma, snpCount-num));
		sumLikelihood += tmp_likelihood;
		for(int j = 0; j < snpCount; j++) {
                        postValues[j] = postValues[j] + tmp_likelihood * configure[j];
		}
		histValues[num] = histValues[num] + tmp_likelihood;
                num = nextBinary(configure, snpCount);
       		if(i % 100000 == 0)
			cout << i << " "  << sumLikelihood << endl;
	}
	for(int i = 0; i <= maxCausalSNP; i++)
		histValues[i] = histValues[i]/sumLikelihood;
        free(configure);
        return(sumLikelihood);
}

/*
	stat is the z-scpres
	sigma is the correaltion matrix
	G is the map between snp and the gene (snp, gene)
*/
double PostCalMultiPop::findOptimalSetGreedy(double NCP, char * pcausalSet, int *rank,  double inputRho, string outputFileName) {
	int index = 0;
        double rho = 0;
        double total_post = 0;

        totalLikeLihoodLOG = computeTotalLikelihood(NCP);

	export2File(outputFileName+".log", totalLikeLihoodLOG); //Output the total likelihood to the log File
	for(int i = 0; i < snpCount; i++)
		total_post += postValues[i];
	printf("Total Likelihood= %e SNP=%d \n", total_post, snpCount);

        std::vector<data> items;
        std::set<int>::iterator it;
	//output the poster to files
        for(int i = 0; i < snpCount; i++) {
             //printf("%d==>%e ",i, postValues[i]/total_likelihood);
             items.push_back(data(postValues[i]/total_post, i, 0));
        }
        printf("\n");
        std::sort(items.begin(), items.end(), by_number());
        for(int i = 0; i < snpCount; i++)
                rank[i] = items[i].index1;

        for(int i = 0; i < snpCount; i++)
                pcausalSet[i] = '0';
        do{
                rho += postValues[rank[index]]/total_post;
                pcausalSet[rank[index]] = '1';
                printf("%d %e\n", rank[index], rho);
                index++;
        } while( rho < inputRho);

        printf("\n");
	return(0);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2004, 2007 StatPro Italia srl

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/instruments/dividendvanillaoption.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/pricingengines/vanilla/analyticdividendeuropeanengine.hpp>
#include <ql/pricingengines/vanilla/fdblackscholesvanillaengine.hpp>
#include <ql/utilities/dataformatters.hpp>
#include <ql/cashflows/cashflowvectors.hpp>
#include <ql/exercise.hpp>
#include <boost/scoped_ptr.hpp>

namespace QuantLib {

    DividendVanillaOption::DividendVanillaOption(
                           const ext::shared_ptr<StrikedTypePayoff>& payoff,
                           const ext::shared_ptr<Exercise>& exercise,
                           const std::vector<Date>& dividendDates,
                           const std::vector<Real>& dividends)
    : OneAssetOption(payoff, exercise),
      cashFlow_(DividendVector(dividendDates, dividends)) {}

    Volatility DividendVanillaOption::impliedVolatility(
             Real targetValue,
             const ext::shared_ptr<GeneralizedBlackScholesProcess>& process,
             Real accuracy,
             Size maxEvaluations,
             Volatility minVol,
             Volatility maxVol) const {

        QL_REQUIRE(!isExpired(), "option expired");

        ext::shared_ptr<SimpleQuote> volQuote(new SimpleQuote);

        ext::shared_ptr<GeneralizedBlackScholesProcess> newProcess =
            detail::ImpliedVolatilityHelper::clone(process, volQuote);

        // engines are built-in for the time being
        boost::scoped_ptr<PricingEngine> engine;
        switch (exercise_->type()) {
          case Exercise::European:
            engine.reset(new AnalyticDividendEuropeanEngine(newProcess));
            break;
          case Exercise::American:
            engine.reset(new FdBlackScholesVanillaEngine(newProcess));
            break;
          case Exercise::Bermudan:
            QL_FAIL("engine not available for Bermudan option with dividends");
            break;
          default:
            QL_FAIL("unknown exercise type");
        }

        return detail::ImpliedVolatilityHelper::calculate(*this,
                                                          *engine,
                                                          *volQuote,
                                                          targetValue,
                                                          accuracy,
                                                          maxEvaluations,
                                                          minVol, maxVol);
    }

    void DividendVanillaOption::setupArguments(
                                       PricingEngine::arguments* args) const {
        OneAssetOption::setupArguments(args);

        DividendVanillaOption::arguments* arguments =
            dynamic_cast<DividendVanillaOption::arguments*>(args);
        QL_REQUIRE(arguments != 0, "wrong engine type");

        arguments->cashFlow = cashFlow_;
    }

    void DividendVanillaOption::arguments::validate() const {
        OneAssetOption::arguments::validate();

        Date exerciseDate = exercise->lastDate();

        for (Size i = 0; i < cashFlow.size(); i++) {
            QL_REQUIRE(cashFlow[i]->date() <= exerciseDate,
                       "the " << io::ordinal(i+1) << " dividend date ("
                       << cashFlow[i]->date()
                       << ") is later than the exercise date ("
                       << exerciseDate << ")");
        }
    }

}


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	int nV = heMesh->NumVertices();
	vector<Eigen::Triplet<double> > triplets;

	for (int i = 0; i < nV; ++i) {
		V* vi = heMesh->Vertices()[i];
		triplets.push_back(Eigen::Triplet<double>(i, i, 1));

		if (!vi->IsBoundary()) {
			double adjVertexSize = vi->AdjVertices().size();
			for(int j = 0; j < adjVertexSize; ++j){
				triplets.push_back(Eigen::Triplet<double>(i, heMesh->Index(vi->AdjVertices()[j]), -1.0 / adjVertexSize));
			}
		}
	}

	Eigen::SparseMatrix<double> A(nV, nV);
	A.setZero();
	A.setFromTriplets(triplets.begin(), triplets.end());
	Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
	solver.compute(A);

	if (solver.info() != Eigen::Success) {
		cout << "compute is error" << endl;
		return;
	}

	Eigen::VectorXd resX(nV), resY(nV), resZ(nV), bX(nV), bY(nV), bZ(nV);
	bX.setZero(); bY.setZero(); bZ.setZero();
	for (int i = 0; i < nV; ++i) {
		V* vi = heMesh->Vertices()[i];
		if (vi->IsBoundary()) {
			bX(i) = vi->pos.at(0);
			bY(i) = vi->pos.at(1);
			bZ(i) = vi->pos.at(2);
		}
	}

	resX = solver.solve(bX);
	resY = solver.solve(bY);
	resZ = solver.solve(bZ);

	for (int i = 0; i < nV; ++i) {
		V* vi = heMesh->Vertices()[i];

		vi->pos.at(0) = resX(i);
		vi->pos.at(1) = resY(i);
		vi->pos.at(2) = resZ(i);
		//cout << resX(i) << " " << resY(i) << " " << resZ(i) << endl;
	}

	return;
}


#define PROFILING_MODE

#include <CGAL/basic.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/CORE_Expr.h>
#include <CGAL/Cartesian.h>
#include <CGAL/determinant.h>
#include <CGAL/Timer.h>
#include <CGAL/Circular_kernel_2.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>           Traits;
typedef Traits::FT                                                              NT;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>            Triangulation;
typedef CGAL::Hyperbolic_octagon_translation_matrix<NT>                         Octagon_matrix;
typedef Triangulation::Point                                                    Point;
typedef Triangulation::Vertex_handle                                            Vertex_handle;
typedef Traits::Side_of_original_octagon                                        Side_of_original_octagon;
typedef Triangulation::Face_iterator                                            Face_iterator;

typedef CGAL::Cartesian<double>::Point_2                                        Point_double;
typedef CGAL::Creator_uniform_2<double, Point_double >                          Creator;

long calls_apply_identity(0);
long calls_apply_non_identity(0);
long calls_append_identity(0);
long calls_append_non_identity(0);

long calls_predicate_identity(0);
long calls_predicate_non_identity(0);
double time_predicate_identity(0);
double time_predicate_non_identity(0);
double time_remove_dp(0);

int main(int argc, char** argv)
{
  int N, iters;
  if(argc < 2)
  {
    std::cout << "usage: " << argv[0] << " [number_of_points_to_insert] [optional: number_of_iterations]" << std::endl;
    std::cout << "Defaulting to 100k points, 10 iterations..." << std::endl;
    iters = 10;
    N = 100000;
  } else {
    N = atoi(argv[1]);
    if (argc < 3)
      iters = 10;
    else
      iters = atoi(argv[2]);
  }

  Side_of_original_octagon pred;

  std::cout << "---- for best results, make sure that you have compiled me in Release mode ----" << std::endl;

  double extime = 0.0;

  for(int exec = 1; exec <= iters; ++exec)
  {
    std::vector<Point> pts;
    CGAL::Random_points_in_disc_2<Point_double, Creator> g(0.85);

    int cnt = 0;
    do
    {
      Point_double pd = *(++g);
      if(pred(pd) != CGAL::ON_UNBOUNDED_SIDE)
      {
        Point pt = Point(pd.x(), pd.y());
        pts.push_back(pt);
        ++cnt;
      }
    }
    while(cnt < N);

    std::cout << "iteration " << exec << ": inserting into triangulation (rational dummy points)... "; std::cout.flush();
    Triangulation tr;

    CGAL::Timer tt;
    tt.start();
    tr.insert(pts.begin(), pts.end());
    tt.stop();
    std::cout << "DONE! (# of vertices = " << tr.number_of_vertices() << ", time = " << tt.time() << " secs)" << std::endl;
    extime += tt.time();

    int bfc = 0;
    for(Face_iterator fit = tr.faces_begin(); fit != tr.faces_end(); fit++)
    {
      if(!(fit->translation(0).is_identity() &&
           fit->translation(1).is_identity() &&
           fit->translation(2).is_identity()))
      {
        ++bfc;
      }
    }

    Triangulation::size_type Nf = tr.number_of_faces();
    double perc = double(bfc)/double(Nf) * 100.0;
    std::cout << "Total number of faces      : " << Nf << std::endl;
    std::cout << "Faces crossing the boundary: " << bfc << std::endl;
    std::cout << "Percentage                 : " << perc << std::endl;

    std::cout << "Triangulation is valid: " << (tr.is_valid() ? "YES" : "NO") << std::endl;
  }

  double avgtime = extime / double(iters);
  std::cout << "---------------------------------------" << std::endl;
  std::cout << "Average execution time over " << iters << " iterations: " << avgtime << " secs" << std::endl << std::endl;

  std::cout << "Calls to append resulting in     identity: " << calls_append_identity << std::endl;
  std::cout << "Calls to append resulting in non-identity: " << calls_append_non_identity << std::endl;
  std::cout << "Percentage                               : " << (double(calls_append_non_identity)/double(calls_append_non_identity+calls_append_identity)*100.0) << std::endl << std::endl;
  std::cout << "Calls to apply  with             identity: " << calls_apply_identity << std::endl;
  std::cout << "Calls to apply  with         non-identity: " << calls_apply_non_identity << std::endl;
  std::cout << "Percentage                               : " << double(calls_apply_non_identity)/(double(calls_apply_non_identity+calls_apply_identity)*100.0) << std::endl << std::endl;

  std::cout << "Predicate calls with     identity translations: " << calls_predicate_identity << std::endl;
  std::cout << "Predicate calls with non-identity translations: " << calls_predicate_non_identity << std::endl;
  std::cout << "Percentage                               : " << double(calls_predicate_non_identity)/double(calls_predicate_non_identity+calls_predicate_identity)*100.0 << std::endl << std::endl;

  std::cout << "Time in predicates with     identity translations: " << time_predicate_identity << std::endl;
  std::cout << "Time in predicates with non-identity translations: " << time_predicate_non_identity << std::endl;
  std::cout << "Percentage                                  : " << double(time_predicate_non_identity)/double(time_predicate_non_identity+time_predicate_identity)*100.0 << std::endl << std::endl;

  std::cout << "Time to remove dummy points                 : " << time_remove_dp << std::endl;

  return EXIT_SUCCESS;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <iostream>
#include <memory>

#include <BayesFilters/BootstrapCorrection.h>
#include <BayesFilters/DrawParticles.h>
#include <BayesFilters/GaussianLikelihood.h>
#include <BayesFilters/InitSurveillanceAreaGrid.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/Resampling.h>
#include <BayesFilters/SIS.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>
#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class SISSimulation : public SIS
{
public:
    SISSimulation
    (
        unsigned int num_particle,
        std::size_t state_size,
        unsigned int simulation_steps,
        std::unique_ptr<ParticleSetInitialization> initialization,
        std::unique_ptr<PFPrediction> prediction,
        std::unique_ptr<PFCorrection> correction,
        std::unique_ptr<Resampling> resampling
    ) noexcept :
        SIS(num_particle, state_size, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool runCondition() override
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

private:
    unsigned int simulation_steps_;
};

int main()
{
    std::cout << "Running a SIS particle filter on a simulated target." << std::endl;
    std::cout << "Data is logged in the test folder with prefix testSIS." << std::endl;

    /* A set of parameters needed to run a SIS particle filter in a simulated environment. */
    double surv_x = 1000.0;
    double surv_y = 1000.0;
    unsigned int num_particle_x = 100;
    unsigned int num_particle_y = 100;
    unsigned int num_particle = num_particle_x * num_particle_y;
    Vector4d initial_state(10.0f, 0.0f, 10.0f, 0.0f);
    unsigned int simulation_time = 100;
    std::size_t state_size = 4;

    /* Step 1 - Initialization */
    /* Initialize initialization class. */
    std::unique_ptr<ParticleSetInitialization> grid_initialization = utils::make_unique<InitSurveillanceAreaGrid>(surv_x, surv_y, num_particle_x, num_particle_y);

    /* Step 2 - Prediction */
    /* Step 2.1 - Define the state model */
    /* Initialize a white noise acceleration state model. */
    double T = 1.0f;
    double tilde_q = 10.0f;

    std::unique_ptr<StateModel> wna = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);

    /* Step 2.2 - Define the prediction step */
    /* Initialize the particle filter prediction step and pass the ownership of the state model. */
    std::unique_ptr<PFPrediction> pf_prediction = utils::make_unique<DrawParticles>();
    pf_prediction->setStateModel(std::move(wna));

    /* Step 3 - Correction */
    /* Step 3.1 - Define where the measurement are originated from (either simulated or from a real process) */
    /* Initialize simulaterd target model with a white noise acceleration. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>(T, tilde_q);
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_state, simulation_time);
    simulated_state_model->enable_log(".", "testSIS");

    /* Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<MeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));
    simulated_linear_sensor->enable_log(".", "testSIS");

    /* Step 3.3 - Define the likelihood model */
    /* Initialize the the exponential likelihood, a PFCorrection decoration of the particle filter correction step. */
    std::unique_ptr<LikelihoodModel> exp_likelihood = utils::make_unique<GaussianLikelihood>();

    /* Step 3.4 - Define the correction step */
    /* Initialize the particle filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<PFCorrection> pf_correction = utils::make_unique<BoostrapCorrection>();
    pf_correction->setLikelihoodModel(std::move(exp_likelihood));
    pf_correction->setMeasurementModel(std::move(simulated_linear_sensor));

    /* Step 4 - Resampling */
    /* Initialize a resampling algorithm */
    std::unique_ptr<Resampling> resampling = utils::make_unique<Resampling>();

    /* Step 5 - Assemble the particle filter */
    std::cout << "Constructing SIS particle filter..." << std::flush;
    SISSimulation sis_pf(num_particle, state_size, simulation_time, std::move(grid_initialization), std::move(pf_prediction), std::move(pf_correction), std::move(resampling));
    sis_pf.enable_log(".", "testSIS");
    std::cout << "done!" << std::endl;

    /* Step 6 - Prepare the filter to be run */
    std::cout << "Booting SIS particle filter..." << std::flush;
    sis_pf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 7 - Run the filter and wait until it is closed */
    /* Note that since this is a simulation, the filter will end upon simulation termination */
    std::cout << "Running SIS particle filter..." << std::flush;
    sis_pf.run();
    std::cout << "waiting..." << std::flush;
    if (!sis_pf.wait())
        return EXIT_FAILURE;
    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


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

#include "LevenbergMarquardt.h"
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

namespace Scine {
namespace Utils {

void LevenbergMarquardt::optimize(Eigen::VectorXd& parameters, UpdateFunctionManagerBase& updateFunctionManager) {
  LMFunctor functor(updateFunctionManager);
  functor.n = static_cast<int>(parameters.size());
  functor.m = functor.updateFunctionManager_.getNumberOfDataPoints(parameters);
  Eigen::LevenbergMarquardt<LMFunctor, double> lm(functor);
  // Set maximum number of function evaluations if it was set to a sensible value
  if (maxFuncEval > 0)
    lm.parameters.maxfev = maxFuncEval;
  lm.minimize(parameters);
  // Calculate and update covariance matrix if desired
  if (calculateCovarianceMatrix) {
    auto hessian = lm.fjac.transpose() * lm.fjac;
    auto inverseHessian = hessian.inverse();
    auto variance = (1.0 / (functor.m - functor.n + 1.0)) * lm.fvec.squaredNorm();
    covarianceMatrix_ = variance * inverseHessian;
  }
}

LevenbergMarquardt::LMFunctor::LMFunctor(UpdateFunctionManagerBase& updateFunctionManager)
  : updateFunctionManager_(updateFunctionManager) {
}

// Compute 'm' errors, one for each data point, for the given parameter values in 'x'
int LevenbergMarquardt::LMFunctor::operator()(const Eigen::VectorXd& parameters, Eigen::VectorXd& fvec) const {
  updateFunctionManager_.updateErrors(parameters, fvec);
  return 0;
}

// Compute the Jacobian of the errors
int LevenbergMarquardt::LMFunctor::df(const Eigen::VectorXd& parameters, Eigen::MatrixXd& fjac) const {
  updateFunctionManager_.updateJacobian(parameters, fjac);
  return 0;
}

// Returns 'm', the number of values.
int LevenbergMarquardt::LMFunctor::values() const {
  return m;
}

// Returns 'n', the number of inputs.
int LevenbergMarquardt::LMFunctor::inputs() const {
  return n;
}

const Eigen::MatrixXd& LevenbergMarquardt::getCovarianceMatrix() {
  return covarianceMatrix_;
}

} // namespace Utils
} // namespace Scine