#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <geometry/draw.hpp>
#include <imgproc/gradient_adapter.hpp>
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/image_operator.hpp>
#include <utility/matlab_helpers.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>

#define WRITE_IMAGE_FILES
//#define SHOW_IMAGES

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

std::string path = "./orientation/", fname;
int stepsi = -1, steps = 45;
double magTh = 0.1;

constexpr int ENTRY_HALF_RANGE = 1;

template<class GT, class MT = int, class DT = double>
struct Entry {
    Entry() :  flags(0) {}
    Entry(const cv::Ptr<GradientI<uchar, GT, MT, DT>>& a, const std::string& b, int f = 0)
        : grad(a), name(b), flags(f) {}

    cv::Ptr<GradientI<uchar, GT, MT, DT>> grad;
    std::string name;
    int flags;

    inline bool hr() const {
        return flags & ENTRY_HALF_RANGE;
    }

    inline cv::Mat process(const cv::Mat src) {
        grad->process(src);
        return grad->magnitude();
    }

    cv::Mat fixGrad(const cv::Mat &m) const {
        cv::Mat mag;
        if (m.type() != CV_32F)
            m.convertTo(mag, CV_32F);
        else
            m.copyTo(mag);

        double vmin, vmax;
        cv::minMaxIdx(mag, &vmin, &vmax);
        // laplace
        if (vmin < 0) {
            vmax = std::max(vmax, -vmin);
            mag = cv::abs(mag);
            mag *= 255.0 / vmax;
        }
        else {
            mag -= vmin;
            mag *= 255.0 / (vmax - vmin);
        }
        mag.convertTo(mag, CV_8U);
        return mag;
    }
};

double error(const cv::Mat &gt, const cv::Mat &dir, bool hr = false) {
    cv::Mat res = cv::abs(gt - dir);
    cv::subtract(2 * CV_PI, res, res, res > CV_PI);
    if (hr)
        cv::subtract(CV_PI, res, res, res > CV_PI / 2);
#ifdef WRITE_IMAGE_FILES
    cv::Mat write = res / CV_PI * 2550;
    write.convertTo(write, CV_8U);
    if (stepsi < 0)
        cv::imwrite(path + fname + "-error.png", write);
    else
        cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + "-error.png", write);
#endif
#ifdef SHOW_IMAGES
    imshow("error", res / CV_PI * 10);
    cv::waitKey();
#endif
    return cv::sum(res)[0];
}

template<class GT, class MT = int, class DT = float>
double testSingle(Entry<GT, MT, DT> &e, const cv::Mat &img, const cv::Mat &mask, double rot = 0, double noise = 0) {

    RotateOperator<double> op_rot(rot, cv::Point2d(127, 127), cv::INTER_CUBIC);
    RotateOperator<double> op_rot_mask(rot, cv::Point2d(127, 127), cv::INTER_NEAREST);
    GaussianNoiseOperator op_noise(noise);

    cv::Mat gt(mask.size(),CV_64F);
    gt.setTo(0);

    gt.setTo(-rot, mask == 1);
    gt.setTo(-rot + CV_PI/2, mask == 2);

    if (e.hr()) {
        gt.setTo(-rot, mask == 3);
        gt.setTo(-rot + CV_PI / 2, mask == 4);
    }
    else
    {
        gt.setTo(-rot + CV_PI, mask == 3);
        gt.setTo(-rot -  CV_PI / 2, mask == 4);
    }

    cv::Mat nimg;
    img.copyTo(nimg);

    //cv::imshow("img", img);

    cv::Mat nmask;
    mask.copyTo(nmask);
    if (rot > 0) {
        op_rot(nimg);
        op_rot_mask(nmask);
        op_rot_mask(gt);
    }

    int num = static_cast<int>(cv::sum(gt > 0)[0]) / 255;
    if (noise > 0)
        op_noise(nimg);

    e.grad->process(nimg);
    cv::Mat dir = e.grad->direction();
    dir.setTo(0, nmask == 0);

#ifdef WRITE_IMAGE_FILES
    cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + ".png", nimg);
    cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + "-mask.png", nmask > 0);
#endif
#ifdef SHOW_IMAGES
    cv::imshow("img", nimg);
    cv::imshow("mask", nmask > 0);
    //cv::imshow("gt", gt > 0);
    cv::imshow("cA", quiverDir<DT, uchar>(nimg, gt, Range<double>(-CV_PI, CV_PI), nmask > 0, 2, 2, 1, 8, 1));
    cv::imshow("cB", quiverDir<DT, uchar>(nimg, dir, Range<double>(-CV_PI, CV_PI), nmask > 0, 2, 2, 1, 8, 1));
    //cv::imshow(e.name + "-mask", nmask > 0);
    std::cout << rot << std::endl;
#endif

    return error(gt, dir, e.hr()) / num;

}

cv::Mat boxImage() {
    cv::Mat img(256, 256, CV_8U);
    img.setTo(85);
    img.rowRange(47, 208).colRange(47, 208).setTo(170);
    return img;
}

template<class GT, class MT = int, class DT = float>
double testBox(Entry<GT,MT,DT> &e, cv::Mat img, cv::Mat mask, double blur = 0, double noise = 0) {
    GaussianBlurOperator op_blur(blur);

    if (blur > 0)
        op_blur(img);

    // remove borders dependong on blur
    int ks = cvRound(blur * 6 + 1) | 1;
    // make sure that border has at least the size of greatest filter kernel (7x7)
    if (ks < 9)
        ks = 9;
    mask.rowRange(47 - ks, 47 + ks).colRange(47 - ks, 47 + ks).setTo(0);
    mask.rowRange(207 - ks, 207 + ks).colRange(47 - ks, 47 + ks).setTo(0);
    mask.rowRange(47 - ks, 47 + ks).colRange(207 - ks, 207 + ks).setTo(0);
    mask.rowRange(207 - ks, 207 + ks).colRange(207 - ks, 207 + ks).setTo(0);

    cv::Mat tmp = mask.rowRange(47 + ks, 207 - ks).colRange(47 - ks, 47 + ks);
    tmp.setTo(1, tmp > 0);

    tmp = mask.rowRange(47 - ks, 47 + ks).colRange(47 + ks, 207 - ks);
    tmp.setTo(2, tmp > 0);

    tmp = mask.rowRange(47 + ks, 207 - ks).colRange(207 - ks, 207 + ks);
    tmp.setTo(3, tmp > 0);

    tmp = mask.rowRange(207 - ks, 207 + ks).colRange(47 + ks, 207 - ks);
    tmp.setTo(4, tmp > 0);

    double sum = 0;
    for (stepsi = 0; stepsi <= steps; ++stepsi)
        sum += testSingle(e, img, mask, stepsi * CV_PI / (4 *steps), noise);

    return sum / steps;
}

cv::Mat diskImage() {
    cv::Mat img(512, 512, CV_8U);
    img.setTo(85);
    cv::circle(img, cv::Point(256, 256), 150, 170, -1, CV_AA);
    return img;
}

template<class GT, class MT = int, class DT = double>
double testDisk(Entry<GT, MT, DT> &e, cv::Mat img, cv::Mat mask, double blur = 0, double noise = 0) {
    stepsi = -1;
    GaussianBlurOperator op_blur(blur);
    GaussianNoiseOperator op_noise(noise);

    cv::Mat gt(512, 512, CV_64F);
    for (int y = 0; y != 512; ++y)
        for (int x = 0; x != 512; ++x)
            gt.at<double>(y, x) = std::atan2(static_cast<double>(256-y), static_cast<double>(256-x));

    if (e.hr()) {
        cv::add(gt, CV_PI, gt, gt < -CV_PI / 2);
        cv::subtract(gt, CV_PI, gt, gt > CV_PI / 2);
    }

    if (blur > 0)
        op_blur(img);

    int num = static_cast<int>(cv::sum(mask)[0]) / 255;
    gt.setTo(0, mask == 0);

    if (noise > 0)
        op_noise(img);

    e.grad->process(img);
    cv::Mat dir = e.grad->direction();
    dir.setTo(0, mask == 0);

#ifdef WRITE_IMAGE_FILES
    cv::imwrite(path + fname + ".png", img);
    cv::imwrite(path + fname + "-mask.png", mask);
#endif
#ifdef SHOW_IMAGES
    cv::imshow("img", img);
    cv::imshow("mask", mask > 0);
    cv::imshow("A", quiverDir<DT, uchar>(img, gt, Range<double>(-CV_PI, CV_PI), mask, 4, 4, 1, 8, 1));
    cv::imshow("B", quiverDir<DT, uchar>(img, dir, Range<double>(-CV_PI, CV_PI), mask, 4, 4, 1, 8, 1));
    //cv::imshow(e.name + "-mask", mask > 0);

    //cv::Mat res = e.fixGrad(e.process(img));
    //imshow(e.name + " - grad", res);
    //imshow(e.name + " - grad", quiverDir<DT, MT>(res, e.grad->direction(), e.grad->directionRange(), e.grad->magnitude(), 1, 1, 4, 1, 0.5));
#endif
    return error(gt, dir) / num;
}

int main(int argc, char** argv)
{
    std::vector<Entry<short>> gradI;
    gradI.push_back(Entry<short>(new SusanGradient<short, int, double>, "Susan (37)", ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new SusanGradient<short, int, double>(20,true), "Susan (3x3)", ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new RCMGradient<uchar, 1, short, int, double>(3,1), "RMG (3x3)",ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new RCMGradient<uchar, 1, short, int, double>(5, 3), "RMG (5x5)", ENTRY_HALF_RANGE));

    std::vector<Entry<double, double>> gradF;
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, RobertsDerivative>, "Roberts (2x2)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, PrewittDerivative>, "Prewitt (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, ScharrDerivative>, "Scharr (3x3)"));

    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>, "Sobel (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",5)}), "Sobel (5x5)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",7) }), "Sobel (7x7)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",9) }), "Sobel (9x9)"));

    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",3), NV("grad_range",1.5) }), "Gauss (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",5), NV("grad_range",2.3) }), "Gauss (5x5)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",7), NV("grad_range",3.0) }), "Gauss (7x7)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",9), NV("grad_range",3.5) }), "Gauss (9x9)"));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size",3), NV("grad_kernel_spacing",1.24008) }), "QF StG (3x3)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 5), NV("grad_kernel_spacing", 1.008) }), "QF StG (5x5)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 7), NV("grad_kernel_spacing", 0.873226) }), "QF StG (7x7)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 9), NV("grad_kernel_spacing", 0.781854) }), "QF StG (9x9)", ENTRY_HALF_RANGE));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 3), NV("grad_kernel_spacing", 1.2) }), "SQF Po (3x3)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 5), NV("grad_kernel_spacing", 1.2) }), "SQF Po (5x5)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 7), NV("grad_kernel_spacing", 1.2) }), "SQF Po (7x7)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 9), NV("grad_kernel_spacing", 1.2) }), "SQF Po (9x9)"));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureLGF<uchar, double>>({ NV("grad_waveLength", 5), NV("grad_sigmaOnf", 0.55) }), "SQFF LG"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureSF<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_spacing", 1.2) }), "SQFF Po12"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureSF<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 3), NV("grad_kernel_spacing", 2.5) }), "SQFF Po"));

    gradF.push_back(Entry<double, double>(new GradientEnergy<PCLgf<uchar, double>>({ NV("grad_waveLength", 3), NV("grad_sigmaOnf", 0.55) }), "PCF Lg"));
    gradF.push_back(Entry<double, double>(new GradientOdd<PCSqf<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 3), NV("grad_kernel_spacing", 2.5) }), "PCF Po"));

    int cols = gradI.size() + gradF.size() + 1;
    int rows = 7;
    std::vector<std::vector<std::string>> table_box, table_disk;
    table_box.resize(cols); table_disk.resize(cols);
    for_each(table_box.begin(), table_box.end(), [&](std::vector<std::string> &row) {
        row.resize(rows);
    });

    for_each(table_disk.begin(), table_disk.end(), [&](std::vector<std::string> &row) {
        row.resize(rows);
    });

    std::cout << "none...";
    table_box[0][0] = "Method";
    table_box[0][1] = "none";

    table_disk[0][0] = "Method";
    table_disk[0][1] = "none";

    DerivativeGradient<uchar, double, double, double, SobelDerivative> sobel;
    cv::Mat box = boxImage();
    sobel.process(box);
    cv::Mat box_mask = sobel.magnitude() > sobel.magnitudeThreshold(magTh);

    cv::Mat disk = diskImage();
    sobel.process(disk);
    cv::Mat disk_mask = sobel.magnitude() > sobel.magnitudeThreshold(magTh);

    int row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        table_box[row][0] = e.name;
        table_disk[row][0] = e.name;
        fname = "box-"+ e.name;
        table_box[row][1] = boost::str(boost::format("%.3f") % testBox(e,box.clone(),box_mask.clone(),0,0));
        fname = "disk-"+ e.name;
        table_disk[row++][1] = boost::str(boost::format("%.3f") % testDisk(e,disk.clone(),disk_mask.clone(),0,0));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        table_box[row][0] = e.name;
        table_disk[row][0] = e.name;
        fname = "box-" + e.name;
        table_box[row][1] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 0));
        fname = "disk-" + e.name;
        table_disk[row++][1] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 0));
    });

    std::cout << "done" << std::endl << "blur...";
    table_box[0][2] = "blur";
    table_disk[0][2] = "blur";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-blur" + e.name;
        table_box[row][2] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 0));
        fname = "disk-blur" + e.name;
        table_disk[row++][2] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 0));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-blur-"+ e.name;
        table_box[row][2] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 0));
        fname = "disk-blur-"+ e.name;
        table_disk[row++][2] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 0));
    });

    std::cout << "done" << std::endl << "noise 10...";
    table_box[0][3] = "noise10";
    table_disk[0][3] = "noise10";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-noise10-"+ e.name;
        table_box[row][3] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 10));
        fname = "disk-noise10-"+ e.name;
        table_disk[row++][3] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 10));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-noise10-"+ e.name;
        table_box[row][3] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 10));
        fname = "disk-noise10-" + e.name;
        table_disk[row++][3] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 10));
    });

    std::cout << "done" << std::endl << "noise 40...";
    table_box[0][4] = "noise40";
    table_disk[0][4] = "noise40";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-noise40-" + e.name;
        table_box[row][4] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 40));
        fname = "disk-noise40-" + e.name;
        table_disk[row++][4] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 40));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-noise40-" + e.name;
        table_box[row][4] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 40));
        fname = "disk-noise40-" + e.name;
        table_disk[row++][4] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 40));
    });

    std::cout << "done" << std::endl << "blur + noise 10...";
    table_box[0][5] = "bnoise10";
    table_disk[0][5] = "bnoise10";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-bnoise10-" + e.name;
        table_box[row][5] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 10));
        fname = "disk-bnoise10-" + e.name;
        table_disk[row++][5] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 10));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-bnoise10-" + e.name;
        table_box[row][5] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 10));
        fname = "disk-bnoise10-" + e.name;
        table_disk[row++][5] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 10));
    });

    std::cout << "done" << std::endl << "blur + noise 40...";
    table_box[0][6] = "bnoise40";
    table_disk[0][6] = "bnoise40";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-bnoise40-" + e.name;
        table_box[row][6] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 40));
        fname = "disk-bnoise40-" + e.name;
        table_disk[row++][6] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 40));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-bnoise40-" + e.name;
        table_box[row][6] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 40));
        fname = "disk-bnoise40-" + e.name;
        table_disk[row++][6] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 40));
    });
    std::cout << "done" << std::endl;

    std::ofstream ofs;
    ofs.open("gradient_orientation_box.csv");

    for_each(table_box.begin(), table_box.end(), [&](const std::vector<std::string> &row) {
        for_each(row.begin(), row.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    ofs.open("gradient_orientation_disk.csv");

    for_each(table_disk.begin(), table_disk.end(), [&](const std::vector<std::string> &row) {
        for_each(row.begin(), row.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    cv::waitKey();

    return 0;
}


//******************************************************************************
//  Copyright(C) 2008-2013 Intel Corporation. All Rights Reserved.
//
//  The source code, information  and  material ("Material") contained herein is
//  owned  by Intel Corporation or its suppliers or licensors, and title to such
//  Material remains  with Intel Corporation  or its suppliers or licensors. The
//  Material  contains proprietary information  of  Intel or  its  suppliers and
//  licensors. The  Material is protected by worldwide copyright laws and treaty
//  provisions. No  part  of  the  Material  may  be  used,  copied, reproduced,
//  modified, published, uploaded, posted, transmitted, distributed or disclosed
//  in any way  without Intel's  prior  express written  permission. No  license
//  under  any patent, copyright  or  other intellectual property rights  in the
//  Material  is  granted  to  or  conferred  upon  you,  either  expressly,  by
//  implication, inducement,  estoppel or  otherwise.  Any  license  under  such
//  intellectual  property  rights must  be express  and  approved  by  Intel in
//  writing.
//
//  *Third Party trademarks are the property of their respective owners.
//
//  Unless otherwise  agreed  by Intel  in writing, you may not remove  or alter
//  this  notice or  any other notice embedded  in Materials by Intel or Intel's
//  suppliers or licensors in any way.
//
//******************************************************************************
// Content:
//     Intel(R) Math Kernel Library (MKL) overloaded Boost/uBLAS prod()
//******************************************************************************

#ifndef _MKL_BOOST_UBLAS_GEMM_
#define _MKL_BOOST_UBLAS_GEMM_

#include <boost/version.hpp>
#if defined (BOOST_VERSION) && (BOOST_VERSION >= 103401)

#include <boost/numeric/ublas/detail/concepts.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "mkl_cblas.h"

#ifndef MKL_BOOST_UBLAS_INLINE
#define MKL_BOOST_UBLAS_INLINE inline
#endif

namespace boost { namespace numeric { namespace ublas {

namespace mkl {

    template<class E1, class E2>
    BOOST_UBLAS_INLINE
    typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                         typename E2::value_type, E2>::result_type
    gemm(const matrix_expression<E1> &e1,
         const matrix_expression<E2> &e2)
    {
        typedef typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                                                 typename E2::value_type, E2>::storage_category storage_category;
        typedef typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                                                 typename E2::value_type, E2>::orientation_category orientation_category;
        return prod (e1, e2, storage_category (), orientation_category ());
    } // For unsupported matrix types.
    template<class T>
    MKL_BOOST_UBLAS_INLINE
    void gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
            const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
            const MKL_INT k, const T alpha, const T *a,
            const MKL_INT lda, const T *b, const MKL_INT ldb,
            const T beta, T *c, const MKL_INT ldc)
    {}// To resolve externals for unsupported matrix types. Never is called.
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const double alpha, const double *a,
         const MKL_INT lda, const double *b, const MKL_INT ldb,
         const double beta, double *c, const MKL_INT ldc)
    {
        cblas_dgemm(Order, TransA, TransB, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const float alpha, const float *a,
         const MKL_INT lda, const float *b, const MKL_INT ldb,
         const float beta, float *c, const MKL_INT ldc)
    {
        cblas_sgemm(Order, TransA, TransB, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const std::complex<double> alpha, const std::complex<double> *a,
         const MKL_INT lda, const std::complex<double> *b, const MKL_INT ldb,
         const std::complex<double> beta, std::complex<double> *c, const MKL_INT ldc)
    {
        cblas_zgemm(Order, TransA, TransB, m, n, k, static_cast<const void *>(&alpha),
            static_cast<const void *>(a), lda, static_cast<const void *>(b), ldb,
            static_cast<const void *>(&beta), static_cast<void *>(c), ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const std::complex<float> alpha, const std::complex<float> *a,
         const MKL_INT lda, const std::complex<float> *b, const MKL_INT ldb,
         const std::complex<float> beta, std::complex<float> *c, const MKL_INT ldc)
    {
        cblas_cgemm(Order, TransA, TransB, m, n, k, static_cast<const void *>(&alpha),
            static_cast<const void *>(a), lda, static_cast<const void *>(b), ldb,
            static_cast<const void *>(&beta), static_cast<void *>(c), ldc);
    }

    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // M
    pointer(const matrix<T,F,A> &m)
    {
        //return &m(0,0);
        return const_cast<T*>(&m.data().begin()[0]);
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // trans(M)
    pointer(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m)
    {
        return pointer(m.expression().expression());
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // trans(conj(M))
    pointer(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m)
    {
        return pointer(m.expression().expression().expression());
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // conj(trans(M))
    pointer(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m)
    {
        return pointer(m.expression().expression().expression());
    }

    template<class E>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix_expression<E> &e, row_major_tag)
    {
        return e().size1();
    }
    template<class E>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix_expression<E> &e, column_major_tag)
    {
        return e().size2();
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix<T,F,A> &m, row_major_tag)
    {
        return m.size2();
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix<T,F,A> &m, column_major_tag)
    {
        return m.size1();
    }

    template<class T>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(T);
    template<>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(row_major_tag) {
        return CblasRowMajor;
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(column_major_tag) {
        return CblasColMajor;
    }

    template<class T>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(T)
    { return 0; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(float)
    { return 1; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(double)
    { return 2; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(std::complex<float>)
    { return 3; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(std::complex<double>)
    { return 4; }

    template<class E1, class E2, class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_TRANSPOSE transa, const  CBLAS_TRANSPOSE transb,
         const matrix_expression<E1> &a, const matrix_expression<E2> &b,
         matrix<T,F,A> &c)
    {
        typedef T value_type;
        typedef typename F::orientation_category orientation_category;
        if(supported_type(value_type()) > 0) {
            MKL_INT m = a().size1();
            MKL_INT k = a().size2();
            MKL_INT n = b().size2();
            MKL_INT lda = leading_dimension(a(),orientation_category());
            MKL_INT ldb = leading_dimension(b(),orientation_category());
            MKL_INT ldc = leading_dimension(c,orientation_category());

	/*boost 1.57 change the function OneElement definition from 1.0 to 0.0 in boost/numeric/ublas/detail/concepts.hpp
	template<class T>
    T
    OneElement (T) {
        return T(0.0);
    }
		They explain this in boost/numeric/ublas/detail/concepts.hpp
		Replaced the ZeroElement and OneElement functions with the templated versions
		because the former where giving warnings with clang.  Anyway, It seems a bug or something else. so change
	//value_type alpha = OneElement(value_type());  // boost/numeric/ublas/detail/concepts.hpp  to
	*/
            value_type alpha = 1.0;
			value_type beta = ZeroElement(value_type());  // boost/numeric/ublas/detail/concepts.hpp
            CBLAS_ORDER layout = storage_layout(orientation_category());
            T *pa = pointer(a());
            T *pb = pointer(b());
            T *pc = pointer(c);
            gemm(layout, transa, transb, m, n, k, alpha, pa, lda, pb, ldb, beta, pc, ldc);
        } else {
            c = gemm(a,b);
        }
    }

}  // namespace mkl

}}}
#endif  // BOOST_VERSION
#endif  // _MKL_BOOST_UBLAS_GEMM_


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file simulate-offline.cpp
    \brief This file runs a simulation of the calibration problem in batch mode.
  */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

#include <sm/BoostPropertyTree.hpp>

#include <aslam/backend/Optimizer2Options.hpp>
#include <aslam/backend/GaussNewtonTrustRegionPolicy.hpp>
#include <aslam/backend/Optimizer2.hpp>

#include <aslam/calibration/core/OptimizationProblem.h>
#include <aslam/calibration/statistics/UniformDistribution.h>
#include <aslam/calibration/statistics/NormalDistribution.h>
#include <aslam/calibration/data-structures/VectorDesignVariable.h>
#include <aslam/calibration/geometry/Transformation.h>
#include <aslam/calibration/base/Timestamp.h>
#include <aslam-tsvd-solver/aslam-tsvd-solver.h>

#include "aslam/calibration/2dlrf/utils.h"
#include "aslam/calibration/2dlrf/ErrorTermMotion.h"
#include "aslam/calibration/2dlrf/ErrorTermObservation.h"

using namespace aslam::calibration;
using namespace aslam::backend;
using namespace sm::kinematics;
using namespace sm;

typedef aslam::backend::AslamTruncatedSvdSolver LinearSolver;
int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <conf_file>" << std::endl;
    return -1;
  }

  // load configuration file
  BoostPropertyTree propertyTree;
  propertyTree.loadXml(argv[1]);

  // steps to simulate
  const size_t steps = propertyTree.getInt("lrf/problem/steps");

  // timestep size
  const double T = propertyTree.getDouble("lrf/problem/timestep");

  // true state
  std::vector<Eigen::Vector3d > x_true;
  x_true.reserve(steps);

  // integrated odometry
  std::vector<Eigen::Vector3d > x_odom;
  x_odom.reserve(steps);

  // true control input
  std::vector<Eigen::Vector3d > u_true;
  const double sineWaveAmplitude = propertyTree.getDouble(
    "lrf/problem/sineWaveAmplitude");
  const double sineWaveFrequency = propertyTree.getDouble(
    "lrf/problem/sineWaveFrequency");
  genSineWavePath(u_true, steps, sineWaveAmplitude, sineWaveFrequency, T);

  // measured control input
  std::vector<Eigen::Vector3d > u_noise;
  u_noise.reserve(steps);

  // number of landmarks
  const size_t nl = propertyTree.getInt("lrf/problem/numLandmarks");

  // playground size
  const Eigen::Vector2d min(propertyTree.getDouble("lrf/problem/groundMinX"),
    propertyTree.getDouble("lrf/problem/groundMinY"));
  const Eigen::Vector2d max(propertyTree.getDouble("lrf/problem/groundMaxX"),
    propertyTree.getDouble("lrf/problem/groundMaxY"));

  // bearing measurements
  std::vector<std::vector<double> > b;
  b.reserve(steps);
  b.push_back(std::vector<double>(nl, 0));

  // range measurements
  std::vector<std::vector<double> > r;
  r.reserve(steps);
  r.push_back(std::vector<double>(nl, 0));

  // covariance matrix for motion model
  Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
  Q(0, 0) = propertyTree.getDouble("lrf/problem/motion/sigma2_x");
  Q(1, 1) = propertyTree.getDouble("lrf/problem/motion/sigma2_y");
  Q(2, 2) = propertyTree.getDouble("lrf/problem/motion/sigma2_t");

  // covariance matrix for observation model
  Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
  R(0, 0) = propertyTree.getDouble("lrf/problem/observation/sigma2_r");
  R(1, 1) = propertyTree.getDouble("lrf/problem/observation/sigma2_b");

  // landmark positions
  std::vector<Eigen::Vector2d > x_l;
  UniformDistribution<double, 2>(min, max).getSamples(x_l, nl);

  // true calibration parameters
  const Eigen::Vector3d Theta(propertyTree.getDouble("lrf/problem/thetaTrue/x"),
    propertyTree.getDouble("lrf/problem/thetaTrue/y"),
    propertyTree.getDouble("lrf/problem/thetaTrue/t"));

  // guessed calibration parameters
  const Eigen::Vector3d Theta_hat(
    propertyTree.getDouble("lrf/problem/thetaHat/x"),
    propertyTree.getDouble("lrf/problem/thetaHat/y"),
    propertyTree.getDouble("lrf/problem/thetaHat/t"));

  // initial state
  const Eigen::Vector3d x_0(propertyTree.getDouble("lrf/problem/x0/x"),
    propertyTree.getDouble("lrf/problem/x0/y"),
    propertyTree.getDouble("lrf/problem/x0/t"));
  x_true.push_back(x_0);
  x_odom.push_back(x_0);
  u_noise.push_back(Eigen::Vector3d::Zero());

  // simulate
  for (size_t i = 1; i < steps; ++i) {
    Eigen::Matrix3d B = Eigen::Matrix3d::Identity();
    B(0, 0) = cos(x_true[i - 1](2));
    B(0, 1) = -sin(x_true[i - 1](2));
    B(1, 0) = sin(x_true[i - 1](2));
    B(1, 1) = cos(x_true[i - 1](2));
    Eigen::Vector3d xk = x_true[i - 1] + T * B * u_true[i];
    xk(2) = angleMod(xk(2));
    x_true.push_back(xk);
    u_noise.push_back(u_true[i] +
      NormalDistribution<3>(Eigen::Vector3d::Zero(), Q).getSample());
    B(0, 0) = cos(x_odom[i - 1](2));
    B(0, 1) = -sin(x_odom[i - 1](2));
    B(1, 0) = sin(x_odom[i - 1](2));
    B(1, 1) = cos(x_odom[i - 1](2));
    xk = x_odom[i - 1] + T * B * u_noise[i];
    xk(2) = angleMod(xk(2));
    x_odom.push_back(xk);
    const double ct = cos(x_true[i](2));
    const double st = sin(x_true[i](2));
    std::vector<double> rk(nl, 0);
    std::vector<double> bk(nl, 0);
    for (size_t j = 0; j < nl; ++j) {
      const double aa = x_l[j](0) - x_true[i](0) - Theta(0) * ct +
        Theta(1) * st;
      const double bb = x_l[j](1) - x_true[i](1) - Theta(0) * st -
        Theta(1) * ct;
      const double range = sqrt(aa * aa + bb * bb) +
        NormalDistribution<1>(0, R(0, 0)).getSample();
      rk[j] = range;
      bk[j] = angleMod(atan2(bb, aa) - x_true[i](2) - Theta(2) +
        NormalDistribution<1>(0, R(1, 1)).getSample());
    }
    r.push_back(rk);
    b.push_back(bk);
  }

  // landmark guess
  std::vector<Eigen::Vector2d > x_l_hat;
  initLandmarks(x_l_hat, x_odom, Theta_hat, r, b);

  // create optimization problem
  auto problem = boost::make_shared<OptimizationProblem>();

  // create calibration parameters design variable
  auto dv_Theta = boost::make_shared<VectorDesignVariable<3> >(Theta_hat);
  dv_Theta->setActive(true);
  problem->addDesignVariable(dv_Theta, 2);

  // create state design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<3> > > dv_x;
  dv_x.reserve(steps);
  for (size_t i = 0; i < steps; ++i) {
    dv_x.push_back(boost::make_shared<VectorDesignVariable<3> >(x_odom[i]));
    dv_x[i]->setActive(true);
    problem->addDesignVariable(dv_x[i], 0);
  }

  // create landmarks design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<2> > > dv_x_l;
  dv_x_l.reserve(nl);
  for (size_t i = 0; i < nl; ++i) {
    dv_x_l.push_back(boost::make_shared<VectorDesignVariable<2> >(x_l_hat[i]));
    dv_x_l[i]->setActive(true);
    problem->addDesignVariable(dv_x_l[i], 1);
  }

  // set the ordering of the problem
  problem->setGroupsOrdering({0, 1, 2});

  // add motion and observation error terms
  for (size_t i = 1; i < steps; ++i) {
    auto e_mot = boost::make_shared<ErrorTermMotion>(dv_x[i - 1].get(),
      dv_x[i].get(), T, u_noise[i], Q);
    problem->addErrorTerm(e_mot);
    for (size_t j = 0; j < nl; ++j) {
      auto e_obs = boost::make_shared<ErrorTermObservation>(dv_x[i].get(),
        dv_x_l[j].get(), dv_Theta.get(), r[i][j], b[i][j], R);
      problem->addErrorTerm(e_obs);
    }
  }

  // optimization
  std::cout << "Calibration before: " << *dv_Theta << std::endl;
  Optimizer2 optimizer(PropertyTree(propertyTree, "lrf/estimator/optimizer"),
    boost::make_shared<LinearSolver>(PropertyTree(propertyTree,
    "lrf/estimator/optimizer/linearSolver")),
    boost::make_shared<GaussNewtonTrustRegionPolicy>());
  optimizer.setProblem(problem);

  size_t JCols = 0;
  for (auto it = problem->getGroupsOrdering().cbegin();
      it != problem->getGroupsOrdering().cend(); ++it)
    JCols += problem->getGroupDim(*it);
  const size_t dim = problem->getGroupDim(2);
  auto linearSolver = optimizer.getSolver<LinearSolver>();
  linearSolver->setMargStartIndex(JCols - dim);
  const double before = Timestamp::now();
  optimizer.optimize();
  const double after = Timestamp::now();
  std::cout << "Elapsed time [s]: " << after - before << std::endl;
  std::cout << "Calibration after: " << *dv_Theta << std::endl;
  std::cout << "Singular values (scaled): "
    << linearSolver->getSingularValues().transpose() << std::endl;
  std::cout << "Unobservable basis (scaled): " << std::endl
    << linearSolver->getNullSpace() << std::endl;
  std::cout << "Observable basis (scaled): " << std::endl
    << linearSolver->getRowSpace() << std::endl;
  linearSolver->analyzeMarginal();
  std::cout << "SVD rank: " << linearSolver->getSVDRank() << std::endl;
  std::cout << "SVD rank deficiency: " << linearSolver->getSVDRankDeficiency()
    << std::endl;
  std::cout << "SVD tolerance: " << linearSolver->getSVDTolerance()
    << std::endl;
  std::cout << "Singular values: " << optimizer.getSolver<LinearSolver>()
    ->getSingularValues().transpose() << std::endl;
  std::cout << "QR rank: " << optimizer.getSolver<LinearSolver>()->getQRRank()
    << std::endl;
  std::cout << "QR rank deficiency: " << optimizer.getSolver<LinearSolver>()
    ->getQRRankDeficiency() << std::endl;
  std::cout << "QR tolerance: " << linearSolver->getQRTolerance() << std::endl;
  std::cout << "Unobservable basis: " << std::endl
    << linearSolver->getNullSpace() << std::endl;
  std::cout << "Observable basis: " << std::endl << linearSolver->getRowSpace()
    << std::endl;
  std::cout << "Covariance: " << std::endl << linearSolver->getCovariance()
    << std::endl;
  std::cout << "Observable covariance: " << std::endl
    << linearSolver->getRowSpaceCovariance() << std::endl;
  std::cout << "Peak memory usage (MB): " << linearSolver->getPeakMemoryUsage()
    / 1024.0 / 1024.0 << std::endl;
  std::cout << "Memory usage (MB): " << linearSolver->getMemoryUsage() /
    1024.0 / 1024.0 << std::endl;
  std::cout << "Flop count: " << linearSolver->getNumFlops() << std::endl;
  std::cout << "Linear solver time: " << linearSolver->getLinearSolverTime()
    << std::endl;
  std::cout << "Marginal analysis time: "
    << linearSolver->getMarginalAnalysisTime() << std::endl;
  std::cout << "Symbolic factorization time: "
    << linearSolver->getSymbolicFactorizationTime() << std::endl;
  std::cout << "Numeric factorization time: "
    << linearSolver->getNumericFactorizationTime() << std::endl;
  std::cout << "Log2sum of singular values: "
    << linearSolver->getSingularValuesLog2Sum() << std::endl;

  // output results to file
  std::ofstream x_true_log("x_true.txt");
  for (size_t i = 0; i < steps; ++i)
    x_true_log << x_true[i].transpose() << std::endl;
  std::ofstream x_odom_log("x_odom.txt");
  for (size_t i = 0; i < steps; ++i)
    x_odom_log << x_odom[i].transpose() << std::endl;
  std::ofstream x_est_log("x_est.txt");
  for (size_t i = 0; i < steps; ++i)
    x_est_log << *(dv_x[i]) << std::endl;
  std::ofstream l_log("l.txt");
  for (size_t i = 0; i < nl; ++i)
    l_log << x_l[i].transpose() << std::endl;
  std::ofstream l_est_log("l_est.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_log << *(dv_x_l[i]) << std::endl;

  // align landmarks
  Eigen::MatrixXd l = Eigen::MatrixXd::Zero(3, nl);
  Eigen::MatrixXd l_est = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i) {
    l(0, i) = x_l[i](0);
    l(1, i) = x_l[i](1);
    l_est(0, i) = dv_x_l[i]->getValue()(0);
    l_est(1, i) = dv_x_l[i]->getValue()(1);
  }
  Transformation<double, 3> trans(threePointSvd(l, l_est));
  Eigen::MatrixXd l_est_trans = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i)
    l_est_trans.col(i) = trans(l_est.col(i));
  std::ofstream l_est_trans_log("l_est_trans.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_trans_log << l_est_trans.col(i).head<2>().transpose() << std::endl;

  // align poses
  std::vector<Eigen::Vector3d > x_est_trans;
  x_est_trans.reserve(steps);
  std::ofstream x_est_trans_log("x_est_trans.txt");
  for (size_t i = 0; i < steps; ++i) {
    Eigen::Vector3d pose((Eigen::Vector3d()
      << dv_x[i]->getValue().head<2>(), 0).finished());
    trans.transform(pose, pose);
    pose(2) = dv_x[i]->getValue()(2);
    x_est_trans.push_back(pose);
    x_est_trans_log << x_est_trans[i].transpose() << std::endl;
  }

  return 0;
}


//
// Created by david on 2018-11-16.
//

#include "matvec_dense.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <general/class_tic_toc.h>
#include <memory>
#define profile_matrix_product_dense 1

// Function definitions
template<typename Scalar>
using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
template<typename Scalar>
using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
template<typename Scalar>
using VectorTypeT = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;

namespace dense_lu {
    std::optional<Eigen::PartialPivLU<MatrixType<double>>>               lu_real;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>>> lu_cplx;
    void                                                                 reset() {
        lu_real.reset();
        lu_cplx.reset();
    }
    template<typename Scalar>
    void init() {
        if constexpr(std::is_same_v<Scalar, double>) { dense_lu::lu_real = Eigen::PartialPivLU<MatrixType<Scalar>>(); }
        if constexpr(std::is_same_v<Scalar, std::complex<double>>) dense_lu::lu_cplx = Eigen::PartialPivLU<MatrixType<Scalar>>();
    }

}

template<typename Scalar>
MatVecDense<Scalar>::~MatVecDense() {
    dense_lu::reset();
}

// Pointer to data constructor, copies the matrix into an internal Eigen matrix.
template<typename Scalar>
MatVecDense<Scalar>::MatVecDense(const Scalar *const A_, const long L_, const bool copy_data, const eig::Form form_, const eig::Side side_)
    : A_ptr(A_), L(L_), form(form_), side(side_) {
    if(copy_data) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    dense_lu::init<Scalar>();
    init_profiling();
}

template<typename Scalar>
void MatVecDense<Scalar>::FactorOP()

/*  Partial pivot LU decomposition
 *  Factors P(A-sigma*I) = LU
 */
{
    if(readyFactorOp) return; // happens only once
    if(not readyShift) throw std::runtime_error("Cannot FactorOP: Shift value sigma has not been set.");
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    t_factorOP->tic();
    if constexpr(std::is_same_v<Scalar, eig::real>) { dense_lu::lu_real.value().compute(A_matrix); }
    if constexpr(std::is_same_v<Scalar, eig::cplx>) { dense_lu::lu_cplx.value().compute(A_matrix); }

    readyFactorOp = true;
    t_factorOP->toc();
}

template<typename Scalar>
void MatVecDense<Scalar>::MultOPv(Scalar *x_in_ptr, Scalar *x_out_ptr) {
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    t_multOPv->tic();
    switch(side) {
        case eig::Side::R: {
            Eigen::Map<VectorType<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorType<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>) x_out.noalias() = dense_lu::lu_real.value().solve(x_in);
            if constexpr(std::is_same_v<Scalar, std::complex<double>>) x_out.noalias() = dense_lu::lu_cplx.value().solve(x_in);
            break;
        }
        case eig::Side::L: {
            Eigen::Map<VectorTypeT<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorTypeT<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>) x_out.noalias() = x_in * dense_lu::lu_real.value().inverse();
            if constexpr(std::is_same_v<Scalar, std::complex<double>>) x_out.noalias() = x_in * dense_lu::lu_cplx.value().inverse();
            break;
        }
        case eig::Side::LR: {
            throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
        }
    }
    t_multOPv->toc();
    counter++;
}

template<typename Scalar>
void MatVecDense<Scalar>::MultAx(Scalar *x_in, Scalar *x_out) {
    auto                                 token = t_multAx->tic_token();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = A_matrix * x_vec_in;
                    break;
                }
                case eig::Side::L: {
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = x_vec_in * A_matrix;
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
            Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
            x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Lower>() * x_vec_in;
            break;
        }
    }
    counter++;
}

template<typename T>
void MatVecDense<T>::MultAx(void *x, int *ldx, void *y, int *ldy, int *blockSize, [[maybe_unused]] primme_params *primme, [[maybe_unused]] int *err) {
    auto                                 token = t_multAx->tic_token();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    for(int i = 0; i < *blockSize; i++) {
                        T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                            x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                        Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                        x_vec_out.noalias() = A_matrix * x_vec_in;
                        counter++;
                    }
                    break;
                }
                case eig::Side::L: {
                    for(int i = 0; i < *blockSize; i++) {
                        T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                            x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                        Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                        x_vec_out.noalias() = x_vec_in * A_matrix;
                        counter++;
                    }
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            for(int i = 0; i < *blockSize; i++) {
                T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                T *                            x_out = static_cast<T *>(y) + *ldy * i;
                Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Lower>() * x_vec_in;
                counter++;
            }
            break;
        }
    }
    *err = 0;
}

template<typename Scalar>
void MatVecDense<Scalar>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
}

template<typename Scalar>
void MatVecDense<Scalar>::set_shift(std::complex<double> sigma_) {
    if(readyShift) { return; }
    sigma = sigma_;
    if(A_stl.empty()) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    Eigen::Map<MatrixType<Scalar>> A_matrix(A_stl.data(), L, L);
    if constexpr(std::is_same_v<Scalar, eig::real>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * std::real(sigma);
    if constexpr(std::is_same_v<Scalar, eig::cplx>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * sigma;
    readyShift = true;
}

template<typename Scalar>
void MatVecDense<Scalar>::set_mode(const eig::Form form_) {
    form = form_;
}
template<typename Scalar>
void MatVecDense<Scalar>::set_side(const eig::Side side_) {
    side = side_;
}
template<typename Scalar>
const eig::Form &MatVecDense<Scalar>::get_form() const {
    return form;
}
template<typename Scalar>
const eig::Side &MatVecDense<Scalar>::get_side() const {
    return side;
}

template<typename Scalar>
void MatVecDense<Scalar>::init_profiling() {
    t_factorOP = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time FactorOp");
    t_multOPv  = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time MultOpv");
    t_multAx   = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time MultAx");
}

// Explicit instantiations

template class MatVecDense<double>;
template class MatVecDense<std::complex<double>>;


//  Copyright (c) 2006 Xiaogang Zhang, 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the p < 0 case.
//  Updated 2015 to use Carlson's latest methods.
//

#ifndef BOOST_MATH_ELLINT_RJ_HPP
#define BOOST_MATH_ELLINT_RJ_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/ellint_rc.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/ellint_rd.hpp>

// Carlson's elliptic integral of the third kind
// R_J(x, y, z, p) = 1.5 * \int_{0}^{\infty} (t+p)^{-1} [(t+x)(t+y)(t+z)]^{-1/2} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc1p_imp(T y, const Policy& pol)
{
   using namespace boost::math;
   // Calculate RC(1, 1 + x)
   BOOST_MATH_STD_USING

  static const char* function = "boost::math::ellint_rc<%1%>(%1%,%1%)";

   if(y == -1)
   {
      return policies::raise_domain_error<T>(function,
         "Argument y must not be zero but got %1%", y, pol);
   }

   // for 1 + y < 0, the integral is singular, return Cauchy principal value
   T result;
   if(y < -1)
   {
      result = sqrt(1 / -y) * detail::ellint_rc_imp(T(-y), T(-1 - y), pol);
   }
   else if(y == 0)
   {
      result = 1;
   }
   else if(y > 0)
   {
      result = atan(sqrt(y)) / sqrt(y);
   }
   else
   {
      if(y > -0.5)
      {
         T arg = sqrt(-y);
         result = (boost::math::log1p(arg, pol) - boost::math::log1p(-arg, pol)) / (2 * sqrt(-y));
      }
      else
      {
         result = log((1 + sqrt(-y)) / sqrt(1 + y)) / sqrt(-y);
      }
   }
   return result;
}

template <typename T, typename Policy>
T ellint_rj_imp(T x, T y, T z, T p, const Policy& pol)
{
   BOOST_MATH_STD_USING

   static const char* function = "boost::math::ellint_rj<%1%>(%1%,%1%,%1%)";

   if(x < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument x must be non-negative, but got x = %1%", x, pol);
   }
   if(y < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument y must be non-negative, but got y = %1%", y, pol);
   }
   if(z < 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument z must be non-negative, but got z = %1%", z, pol);
   }
   if(p == 0)
   {
      return policies::raise_domain_error<T>(function,
         "Argument p must not be zero, but got p = %1%", p, pol);
   }
   if(x + y == 0 || y + z == 0 || z + x == 0)
   {
      return policies::raise_domain_error<T>(function,
         "At most one argument can be zero, "
         "only possible result is %1%.", std::numeric_limits<T>::quiet_NaN(), pol);
   }

   // for p < 0, the integral is singular, return Cauchy principal value
   if(p < 0)
   {
      //
      // We must ensure that x < y < z.
      // Since the integral is symmetrical in x, y and z
      // we can just permute the values:
      //
      if(x > y)
         std::swap(x, y);
      if(y > z)
         std::swap(y, z);
      if(x > y)
         std::swap(x, y);

      BOOST_MATH_ASSERT(x <= y);
      BOOST_MATH_ASSERT(y <= z);

      T q = -p;
      p = (z * (x + y + q) - x * y) / (z + q);

      BOOST_MATH_ASSERT(p >= 0);

      T value = (p - z) * ellint_rj_imp(x, y, z, p, pol);
      value -= 3 * ellint_rf_imp(x, y, z, pol);
      value += 3 * sqrt((x * y * z) / (x * y + p * q)) * ellint_rc_imp(T(x * y + p * q), T(p * q), pol);
      value /= (z + q);
      return value;
   }

   //
   // Special cases from http://dlmf.nist.gov/19.20#iii
   //
   if(x == y)
   {
      if(x == z)
      {
         if(x == p)
         {
            // All values equal:
            return 1 / (x * sqrt(x));
         }
         else
         {
            // x = y = z:
            return 3 * (ellint_rc_imp(x, p, pol) - 1 / sqrt(x)) / (x - p);
         }
      }
      else
      {
         // x = y only, permute so y = z:
         using std::swap;
         swap(x, z);
         if(y == p)
         {
            return ellint_rd_imp(x, y, y, pol);
         }
         else if((std::max)(y, p) / (std::min)(y, p) > 1.2)
         {
            return 3 * (ellint_rc_imp(x, y, pol) - ellint_rc_imp(x, p, pol)) / (p - y);
         }
         // Otherwise fall through to normal method, special case above will suffer too much cancellation...
      }
   }
   if(y == z)
   {
      if(y == p)
      {
         // y = z = p:
         return ellint_rd_imp(x, y, y, pol);
      }
      else if((std::max)(y, p) / (std::min)(y, p) > 1.2)
      {
         // y = z:
         return 3 * (ellint_rc_imp(x, y, pol) - ellint_rc_imp(x, p, pol)) / (p - y);
      }
      // Otherwise fall through to normal method, special case above will suffer too much cancellation...
   }
   if(z == p)
   {
      return ellint_rd_imp(x, y, z, pol);
   }

   T xn = x;
   T yn = y;
   T zn = z;
   T pn = p;
   T An = (x + y + z + 2 * p) / 5;
   T A0 = An;
   T delta = (p - x) * (p - y) * (p - z);
   T Q = pow(tools::epsilon<T>() / 5, -T(1) / 8) * (std::max)((std::max)(fabs(An - x), fabs(An - y)), (std::max)(fabs(An - z), fabs(An - p)));

   unsigned n;
   T lambda;
   T Dn;
   T En;
   T rx, ry, rz, rp;
   T fmn = 1; // 4^-n
   T RC_sum = 0;

   for(n = 0; n < policies::get_max_series_iterations<Policy>(); ++n)
   {
      rx = sqrt(xn);
      ry = sqrt(yn);
      rz = sqrt(zn);
      rp = sqrt(pn);
      Dn = (rp + rx) * (rp + ry) * (rp + rz);
      En = delta / Dn;
      En /= Dn;
      if((En < -0.5) && (En > -1.5))
      {
         //
         // Occasionally En ~ -1, we then have no means of calculating
         // RC(1, 1+En) without terrible cancellation error, so we
         // need to get to 1+En directly.  By substitution we have
         //
         // 1+E_0 = 1 + (p-x)*(p-y)*(p-z)/((sqrt(p) + sqrt(x))*(sqrt(p)+sqrt(y))*(sqrt(p)+sqrt(z)))^2
         //       = 2*sqrt(p)*(p+sqrt(x) * (sqrt(y)+sqrt(z)) + sqrt(y)*sqrt(z)) / ((sqrt(p) + sqrt(x))*(sqrt(p) + sqrt(y)*(sqrt(p)+sqrt(z))))
         //
         // And since this is just an application of the duplication formula for RJ, the same
         // expression works for 1+En if we use x,y,z,p_n etc.
         // This branch is taken only once or twice at the start of iteration,
         // after than En reverts to it's usual very small values.
         //
         T b = 2 * rp * (pn + rx * (ry + rz) + ry * rz) / Dn;
         RC_sum += fmn / Dn * detail::ellint_rc_imp(T(1), b, pol);
      }
      else
      {
         RC_sum += fmn / Dn * ellint_rc1p_imp(En, pol);
      }
      lambda = rx * ry + rx * rz + ry * rz;

      // From here on we move to n+1:
      An = (An + lambda) / 4;
      fmn /= 4;

      if(fmn * Q < An)
         break;

      xn = (xn + lambda) / 4;
      yn = (yn + lambda) / 4;
      zn = (zn + lambda) / 4;
      pn = (pn + lambda) / 4;
      delta /= 64;
   }

   T X = fmn * (A0 - x) / An;
   T Y = fmn * (A0 - y) / An;
   T Z = fmn * (A0 - z) / An;
   T P = (-X - Y - Z) / 2;
   T E2 = X * Y + X * Z + Y * Z - 3 * P * P;
   T E3 = X * Y * Z + 2 * E2 * P + 4 * P * P * P;
   T E4 = (2 * X * Y * Z + E2 * P + 3 * P * P * P) * P;
   T E5 = X * Y * Z * P * P;
   T result = fmn * pow(An, T(-3) / 2) *
      (1 - 3 * E2 / 14 + E3 / 6 + 9 * E2 * E2 / 88 - 3 * E4 / 22 - 9 * E2 * E3 / 52 + 3 * E5 / 26 - E2 * E2 * E2 / 16
      + 3 * E3 * E3 / 40 + 3 * E2 * E4 / 20 + 45 * E2 * E2 * E3 / 272 - 9 * (E3 * E4 + E2 * E5) / 68);

   result += 6 * RC_sum;
   return result;
}

} // namespace detail

template <class T1, class T2, class T3, class T4, class Policy>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ellint_rj(T1 x, T2 y, T3 z, T4 p, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3, T4>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rj_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z),
         static_cast<value_type>(p),
         pol), "boost::math::ellint_rj<%1%>(%1%,%1%,%1%,%1%)");
}

template <class T1, class T2, class T3, class T4>
inline typename tools::promote_args<T1, T2, T3, T4>::type
   ellint_rj(T1 x, T2 y, T3 z, T4 p)
{
   return ellint_rj(x, y, z, p, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RJ_HPP


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

/**
 * ublas binding to GSL  Singular Value Decomposition
 *
 * A = U S V^T
 *
 * @param A MxN matrix do decompose. Becomes an MxN orthogonal matrix U
 * @param V NxN orthogonal square matrix
 * @param S N vector of non-negative numbers forming a non-increasing sequence
 * @return succeeded or not
 */

bool linalg_singular_value_decomposition(ub::matrix<double> &A, ub::matrix<double> &VT, ub::vector<double> &S )
{

        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t M = A.size1();
        const size_t N = A.size2();
        // gsl does not handle conversion of a symmetric_matrix

         if (M>N){
        throw runtime_error("Matrix for svd has the wrong shape first dimension must be equal or larger than second.");
    }
	S.resize(N, false);
	VT.resize(N, N, false);

	gsl_matrix_view A_view = gsl_matrix_view_array(&A(0,0), M, N);
	gsl_vector_view S_view = gsl_vector_view_array(&S(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&VT(0,0), N, N);
	gsl_vector * work = gsl_vector_alloc(N);

        int status = gsl_linalg_SV_decomp (&A_view.matrix, &V_view.matrix, &S_view.vector, work);
	//gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_ABS_ASC);
	gsl_set_error_handler(handler);
        gsl_vector_free (work);
        VT=ub::trans(VT);
	return (status != 0);

};

}}


/* vim:set ts=3 sw=3 sts=3 et: */
/**
 * Copyright © 2008-2013 Last.fm Limited
 *
 * This file is part of libmoost.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

/**
 * @file spline_interpolation.hpp
 * @brief generate a cubic spline with natural boundary conditions
 * @author Ricky Cormier (thanks to Marcus for the suggestion)
 * @version See version.h (N/A if it doesn't exist)
 * @date 2012-04-26
 */

#ifndef MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP
#define MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP

#include <cassert>
#include <vector>
#include <algorithm>

#include <gsl/gsl_spline.h> // gnu scientific library

#include <boost/noncopyable.hpp>

#include "../utils/scope_exit.hpp"

namespace moost { namespace algorithm {

   /**
    * @brief Generate a spline interpolation of X and Y data points using
    *        the GNU Scientific Library.
    *
    * http://www.gnu.org/software/gsl/manual/html_node/Interpolation.html
    *
    */

   class spline_interpolation : private boost::noncopyable
   {
      private:
         typedef moost::utils::scope_exit::type<
            gsl_interp *>::call_free_function_with_val interp_t;

         typedef moost::utils::scope_exit::type<
            gsl_interp_accel *>::call_free_function_with_val accel_t;

      public:

         /**
          * @brief create a spline interpolation object
          *
          * @param x : A vector of X data points
          * @param y : A vector of Y data points
          *
          * @note The X vector MUST be sorted in ascending order.
          *
          * Given a vector of X/Y data points, this class will interpolate a
          * value of Y for any value of X providing the value of X is given
          * within the range of X_begin to X_end.
          */

         spline_interpolation(
               std::vector<double> const & x,
               std::vector<double> const & y)
            : interp_(gsl_interp_alloc(gsl_interp_cspline, x.size()), &gsl_interp_free)
              , accel_(gsl_interp_accel_alloc(), &gsl_interp_accel_free)
              , x_(x)
              , y_(y)

         {
            validate_construction(); // throws on fail
            gsl_interp_init(interp_->get(), &x_[0], &y_[0], x_.size());
         }

         /**
          * @brief Given a value of X return Y (or Z if X is out of range)
          *
          * @param x An X data point
          * @param y An interpolated Y data point
          * @param z Used as the default if X is out of range
          *
          * @note the value of X doesn't have to be one of the original values
          *       of the X vector, it just has to be within the range of the
          *       lowest to the highest X value (hence, Y is an interpolation).
          *
          * @return Interpolation of Y or Z if X is out of range
          */

         bool operator () (double const x, double & y, double const z = 0) const
         {
            bool ok =  0 == gsl_interp_eval_e(
                  interp_->get(),
                  &x_[0], &y_[0], x,
                  accel_->get(), &y
                  );

            if(!ok) { y = z;}

            return ok;
         }

         /**
          * @brief Given a value of X return Y (or throw if out of range)
          *
          * @param x An X data point
          *
          * @note the value of X doesn't have to be one of the original values
          *       of the X vector, it just has to be within the range of the
          *       lowest to the highest X value (hence, Y is an interpolation).
          *
          * @return An interpolation of Y data point
          */
         double operator () (double const x) const
         {
            double y = 0.0;

            if(!(*this)(x, y))
            {
               throw std::range_error("out of range");
            }

            return y;
         }

      private:

         void validate_construction() const
         {
            // x and y must be the same size
            assert(x_.size() == y_.size());
            if(x_.size() != y_.size())
            {
               throw std::runtime_error("the size of x and y must be the same");
            }

            // x must be sorted by-asc (not my rule, gsl demands it)
            bool const sorted = (std::adjacent_find(x_.begin(), x_.end(), std::greater_equal<double>()) == x_.end());
            assert(sorted);
            if(!sorted)
            {
               throw std::runtime_error("x must be sorted in assending order");
            }
         }

      private:
         interp_t interp_;
         accel_t  accel_;
         std::vector<double> const & x_;
         std::vector<double> const & y_;
   };

}}

#endif // MOOST_ALGORITHM_SPLINE_INTERPOLATION_HPP


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <sstream>
#include <string>
#include <vector>

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::vector<int>> temp;
  while (std::getline(file, line)) {
    std::vector<int> t;
    // Not sure why passing chars to stoi didn't work but what evers
    // Probably needs null termination
    for (auto i = 0; i < line.size(); ++i) {
      std::string c = {line[i]};
      if (c != "\n") {
        t.push_back(std::stoi(c));
      }
    }
    temp.push_back(std::move(t));
  }

  auto rows = temp.size();
  auto cols = temp[0].size();

  Eigen::MatrixXi out(rows, cols);
  out.setZero();

  for (auto i = 0; i < rows; ++i) {
    for (auto j = 0; j < cols; ++j) {
      out(i, j) = temp[i][j];
    }
  }

  return out;
}

auto findLow(auto const &m) {
  Eigen::MatrixXi out(m.rows(), m.cols());
  out.setConstant(-1);

  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      if (r == 0 && c == 0) {
        if (m(0, 0) < m(0, 1) && m(0, 0) < m(1, 0)) { // upper left
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1) && c == (m.cols() - 1)) { // bottom right
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == 0 && c == (m.cols() - 1)) { // upper right
        if (m(r, c) < m(r + 1, c) && m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1) && c == 0) { // lower left
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c + 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == 0) { // top row
        if (m(r, c) < m(r + 1, c) && m(r, c) < m(r, c + 1) &&
            m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (r == (m.rows() - 1)) { // bottom row
        if (m(r, c) < m(r - 1, c) && m(r, c) < m(r, c + 1) &&
            m(r, c) < m(r, c - 1)) {
          out(r, c) = m(r, c);
        }
      } else if (c == 0) { // Left col
        if (m(r, c) < m(r, c + 1) && m(r, c) < m(r + 1, c) &&
            m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      } else if (c == (m.cols() - 1)) { // right col
        if (m(r, c) < m(r, c - 1) && m(r, c) < m(r + 1, c) &&
            m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      } else { // Whew in the middle
        if (m(r, c) < m(r, c + 1) && m(r, c) < m(r, c - 1) &&
            m(r, c) < m(r + 1, c) && m(r, c) < m(r - 1, c)) {
          out(r, c) = m(r, c);
        }
      }
    }
  }

  return out;
}

int discoverBasin(Eigen::MatrixXi &m, int i, int j) {
  if (i < 0 || i >= m.rows() || j < 0 || j >= m.cols() || m(i, j) == 9) {
    return 0; // Can't be here
  }

  if (m(i, j) == -1) { // Already been here
    return 0;
  }

  m(i,j) = -1; // Been here

  // The + 1 is so that we get credit for this node
  return discoverBasin(m, i + 1, j) + discoverBasin(m, i - 1, j) +
         discoverBasin(m, i, j + 1) + discoverBasin(m, i, j - 1) + 1;
}

int main(int _, char **argv) {
  auto M = parseFile(argv[1]);
  if (M.rows() < 11 && M.cols() < 11) {
    std::cout << "M:\n" << M << "\n\n";
  }
  auto lows = findLow(M);
  if (M.rows() < 11 && M.cols() < 11) {
    std::cout << "L:\n" << lows << "\n\n";
  }
  auto sum = 0;
  for (auto i = 0; i < lows.rows(); ++i) {
    for (auto j = 0; j < lows.cols(); ++j) {
      if (lows(i, j) != -1) {
        sum += lows(i, j) + 1;
      }
    }
  }

  fmt::print("Sum of low points: {}\n", sum);

  // for part two we need to do a search
  Eigen::MatrixXi D = M;
  std::vector<int> basins;
  for(auto i = 0; i < D.rows(); ++i){
    for(auto j = 0; j < D.cols(); ++j){
      if(D(i,j) != 9 && D(i,j) != -1){
        basins.push_back(discoverBasin(D, i, j));
      }
    }
  }

  std::sort(basins.begin(), basins.end(), std::greater<>{});
  fmt::print("Basins: {}\n", fmt::join(basins, ","));
  fmt::print("Prod of top 3: {}\n", basins[0] * basins[1] * basins[2]);
}