#include <math.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <chrono>

#include <opencv2/opencv.hpp>

#include <opencv2/core/cuda.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudafeatures2d.hpp>
#include <opencv2/xfeatures2d/cuda.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudawarping.hpp>

#include <Eigen/Dense>

#define PI 3.14285714286

using namespace Eigen;
using namespace std;
using namespace std::chrono;

struct imageData {
    std::string imageName = "";
    double latitude = 0;
    double longitude = 0;
    double altitudeFeet = 0;
    double altitudeMeter = 0;
    double roll = 0;
    double pitch = 0;
    double yaw = 0;
};

cv::Mat computeUnRotMatrix (imageData& pose) {
    double a = (pose.yaw * PI) / 180;
    double b = (pose.pitch * PI) / 180;
    double g = (pose.roll * PI) / 180;
    Matrix3d Rz;
    Rz << cos (a), -sin (a), 0,
          sin (a), cos (a), 0,
          0, 0, 1;
    Matrix3d Ry;
    Ry << cos (b), 0, sin (b),
          0, 1, 0,
          -sin (b), 0, cos (b);
    Matrix3d Rx;
    Rx << 1, 0, 0,
          0, cos (g), -sin (g),
          0, sin (g), cos (g);
    Matrix3d R = Rz * (Rx * Ry);
    R(0,2) = 0;
    R(1,2) = 0;
    R(2,2) = 1;
    Matrix3d Rtrans = R.transpose ();
    Matrix3d InvR = Rtrans.inverse ();
    cv::Mat transformation = (cv::Mat_<double>(3,3) << InvR(0,0), InvR(0,1), InvR(0,2),
                                                       InvR(1,0), InvR(1,1), InvR(1,2),
                                                       InvR(2,0), InvR(2,1), InvR(2,2));
    return transformation;
}

void printMat (cv::Mat& mat) {
    int rows = mat.rows;
    int cols = mat.cols;
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
            std::cout << mat.at<double>(i,j) << " ";
        }
        std::cout << std::endl;
    }
}

cv::Mat warpPerspectiveWithPadding (const cv::Mat& image, cv::Mat& transformation) {
    int height = image.rows;
    int width = image.cols;
    cv::Mat small_img;
    cv::resize (image, small_img, cv::Size (width/2, height/2));
    std::vector<cv::Point2f> corners = {cv::Point2f (0,0), cv::Point2f (0,height/2),
                                   cv::Point2f (width/2,height/2), cv::Point2f (width/2,0)};
    std::vector<cv::Point2f> warpedCorners;
    cv::perspectiveTransform (corners, warpedCorners, transformation);
    float xMin = 1e9, xMax = -1e9;
    float yMin = 1e9, yMax = -1e9;
    for (int i = 0; i < 4; i++) {
        xMin = (xMin > warpedCorners[i].x)? warpedCorners[i].x : xMin;
        xMax = (xMax < warpedCorners[i].x)? warpedCorners[i].x : xMax;
        yMin = (yMin > warpedCorners[i].y)? warpedCorners[i].y : yMin;
        yMax = (yMax < warpedCorners[i].y)? warpedCorners[i].y : yMax;
    }
    int xMin_ = (xMin - 0.5);
    int xMax_ = (xMax + 0.5);
    int yMin_ = (yMin - 0.5);
    int yMax_ = (yMax + 0.5);
    cv::Mat translation = (cv::Mat_<double>(3,3) << 1, 0, -xMin_, 0, 1, -yMin_, 0, 0, 1);
    cv::Mat fullTransformation = translation * transformation;
    cv::cuda::GpuMat result;
    cv::cuda::GpuMat gpu_img (small_img);
    cv::cuda::GpuMat gpu_ft (fullTransformation);
    cv::cuda::warpPerspective (gpu_img, result, fullTransformation,
                                cv::Size (xMax_-xMin_, yMax_-yMin_));
    cv::Mat result_ (result.size(), result.type());
    result.download (result_);
    return result_;
}

cv::Mat combinePair (cv::Mat& img1, cv::Mat& img2) {

    cv::cuda::GpuMat img1_gpu (img1), img2_gpu (img2);
    cv::cuda::GpuMat img1_gray_gpu, img2_gray_gpu;

    cv::cuda::cvtColor (img1_gpu, img1_gray_gpu, cv::COLOR_BGR2GRAY);
    cv::cuda::cvtColor (img2_gpu, img2_gray_gpu, cv::COLOR_BGR2GRAY);

    cv::cuda::GpuMat mask1;
    cv::cuda::GpuMat mask2;

    cv::cuda::threshold (img1_gray_gpu, mask1, 1, 255, cv::THRESH_BINARY);
    cv::cuda::threshold (img2_gray_gpu, mask2, 1, 255, cv::THRESH_BINARY);

    cv::cuda::SURF_CUDA detector;

    cv::cuda::GpuMat keypoints1_gpu, descriptors1_gpu;
    detector (img1_gray_gpu, mask1, keypoints1_gpu, descriptors1_gpu);

    std::vector<cv::KeyPoint> keypoints1;
    detector.downloadKeypoints (keypoints1_gpu, keypoints1);

    cv::cuda::GpuMat keypoints2_gpu, descriptors2_gpu;
    detector(img2_gray_gpu, mask2, keypoints2_gpu, descriptors2_gpu);

    std::vector<cv::KeyPoint> keypoints2;
    detector.downloadKeypoints (keypoints2_gpu, keypoints2);

    cv::Ptr<cv::cuda::DescriptorMatcher> matcher =
        cv::cuda::DescriptorMatcher::createBFMatcher ();

    std::vector<std::vector<cv::DMatch>> knn_matches;
    matcher->knnMatch (descriptors2_gpu, descriptors1_gpu, knn_matches, 2);

    std::vector<cv::DMatch> matches;
    std::vector<std::vector<cv::DMatch>>::const_iterator it;
    for (it = knn_matches.begin(); it != knn_matches.end(); ++it) {
        if(it->size() > 1 && (*it)[0].distance/(*it)[1].distance < 0.55) {
            matches.push_back((*it)[0]);
        }
    }

    std::vector<cv::Point2f> src_pts;
    std::vector<cv::Point2f> dst_pts;
    for (auto m : matches) {
        src_pts.push_back (keypoints2[m.queryIdx].pt);
        dst_pts.push_back (keypoints1[m.trainIdx].pt);
    }

    cv::Mat A = cv::estimateRigidTransform(src_pts, dst_pts, false);
    int height1 = img1.rows, width1 = img1.cols;
    int height2 = img2.rows, width2 = img2.cols;

    std::vector<std::vector<float>> corners1 {{0,0},{0,height1},{width1,height1},{width1,0}};
    std::vector<std::vector<float>> corners2 {{0,0},{0,height2},{width2,height2},{width2,0}};

    std::vector<std::vector<float>> warpedCorners2 (4, std::vector<float>(2));
    std::vector<std::vector<float>> allCorners = corners1;

    for (int i = 0; i < 4; i++) {
        float cornerX = corners2[i][0];
        float cornerY = corners2[i][1];
        warpedCorners2[i][0] = A.at<double> (0,0) * cornerX +
                            A.at<double> (0,1) * cornerY + A.at<double> (0,2);
        warpedCorners2[i][1] = A.at<double> (1,0) * cornerX +
                            A.at<double> (1,1) * cornerY + A.at<double> (1,2);
        allCorners.push_back (warpedCorners2[i]);
    }

    float xMin = 1e9, xMax = -1e9;
    float yMin = 1e9, yMax = -1e9;
    for (int i = 0; i < 7; i++) {
        xMin = (xMin > allCorners[i][0])? allCorners[i][0] : xMin;
        xMax = (xMax < allCorners[i][0])? allCorners[i][0] : xMax;
        yMin = (yMin > allCorners[i][1])? allCorners[i][1] : yMin;
        yMax = (yMax < allCorners[i][1])? allCorners[i][1] : yMax;
    }
    int xMin_ = (xMin - 0.5);
    int xMax_ = (xMax + 0.5);
    int yMin_ = (yMin - 0.5);
    int yMax_ = (yMax + 0.5);

    cv::Mat translation = (cv::Mat_<double>(3,3) << 1, 0, -xMin_, 0, 1, -yMin_, 0, 0, 1);

    cv::cuda::GpuMat warpedResImg;
    cv::cuda::warpPerspective (img1_gpu, warpedResImg, translation,
                               cv::Size (xMax_-xMin_, yMax_-yMin_));

    cv::cuda::GpuMat warpedImageTemp;
    cv::cuda::warpPerspective (img2_gpu, warpedImageTemp, translation,
                                cv::Size (xMax_ - xMin_, yMax_ - yMin_));
    cv::cuda::GpuMat warpedImage2;
    cv::cuda::warpAffine (warpedImageTemp, warpedImage2, A,
                          cv::Size (xMax_ - xMin_, yMax_ - yMin_));

    cv::cuda::GpuMat mask;
    cv::cuda::threshold (warpedImage2, mask, 1, 255, cv::THRESH_BINARY);
    int type = warpedResImg.type();

    warpedResImg.convertTo (warpedResImg, CV_32FC3);
    warpedImage2.convertTo (warpedImage2, CV_32FC3);
    mask.convertTo (mask, CV_32FC3, 1.0/255);
    cv::Mat mask_;
    mask.download (mask_);

    cv::cuda::GpuMat dst (warpedImage2.size(), warpedImage2.type());
    cv::cuda::multiply (mask, warpedImage2, warpedImage2);

    cv::Mat diff_ = cv::Scalar::all (1.0) - mask_;
    cv::cuda::GpuMat diff (diff_);
    cv::cuda::multiply(diff, warpedResImg, warpedResImg);
    cv::cuda::add (warpedResImg, warpedImage2, dst);
    dst.convertTo (dst, type);

    cv::Mat ret;
    dst.download (ret);
    return ret;
}

cv::Mat combine (std::vector<cv::Mat>& imageList) {
    cv::Mat result = imageList[0];
    for (int i = 1; i < imageList.size(); i++) {
        cv::Mat image = imageList[i];
        cout << i << endl;
        auto start = high_resolution_clock::now();
        result = combinePair (result, image);
        auto end = high_resolution_clock::now();
        auto duration = duration_cast<microseconds> (end-start);
        cout << "time taken by the functions: " << duration.count() << endl;
        float h = result.rows;
        float w = result.cols;
        if (h > 4000 || w > 4000) {
            if (h > 4000) {
                float hx = 4000.0/h;
                h = h * hx;
                w = w * hx;
            }
            else if (w > 4000) {
                float wx = 4000.0/w;
                w = w * wx;
                h = h * wx;
            }
        }
        cout << h << " " << w << endl;
        cv::resize (result, result, cv::Size (w, h));
    }
    return result;
}

void readData (std::string& filename,
               std::vector<imageData>& dataMatrix) {
    std::ifstream file;
    file.open (filename);
    if (file.is_open()) {
        std::string line;
        while (getline (file, line)) {
            std::stringstream ss (line);
            std::string word;
            imageData id;
            int i = 0;
            while (getline (ss, word, ',')) {
                if (i == 0)	{ id.imageName = word; }
                else if (i == 1) { id.latitude = stof(word); }
                else if (i == 2) { id.longitude = stof(word); }
                else if (i == 3) {
                    id.altitudeFeet = stof (word);
                    id.altitudeMeter = id.altitudeFeet * 0.3048;
                }
                else if (i == 4) { id.yaw = stof(word); }
                else if (i == 5) { id.pitch = stof(word); }
                else if (i == 6) { id.roll = stof(word); }
                i++;
            }
            dataMatrix.push_back (id);
        }
    }
}

void getImageList (std::vector<cv::Mat>& imageList,
                   std::vector<imageData>& dataMatrix,
                   std::string base_path) {
    for (auto data : dataMatrix) {
        std::string img_path = base_path + data.imageName;
        cv::Mat img = cv::imread (img_path, 1);
        // cout << img.empty () << endl;
        // cout << img_path << endl;
        imageList.push_back (img);
    }
}

void changePerspective (std::vector<cv::Mat>& imageList,
                        std::vector<imageData>& dataMatrix) {
    std::cout << "Warping Images Now" << std::endl;
    int n = imageList.size();
    for (int i = 0; i < n; i++) {
        cv::Mat M = computeUnRotMatrix (dataMatrix[i]);
        cv::Mat correctedImage = warpPerspectiveWithPadding (imageList[i], M);

        cv::imwrite ("/home/ksakash/misc/Drone-Image-Stitching/temp/"
                     +dataMatrix[i].imageName, correctedImage);
    }
    std::cout << "Image Warping Done" << std::endl;
}

int main () {
    std::string filename = "/home/ksakash/misc/Drone-Image-Stitching/datasets/imageData.txt";
    std::vector<imageData> dataMatrix;
    readData (filename, dataMatrix);
    std::vector<cv::Mat> imageList;
    std::string base_path = "/home/ksakash/misc/Drone-Image-Stitching/datasets/images/";
    getImageList (imageList, dataMatrix, base_path);
    changePerspective (imageList, dataMatrix);
    imageList.clear();
    base_path = "/home/ksakash/misc/Drone-Image-Stitching/temp/";
    getImageList (imageList, dataMatrix, base_path);
    cv::Mat result = combine (imageList);
    cv::imwrite ("/home/ksakash/misc/Drone-Image-Stitching/results/result.png", result);
    return 0;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "kcm.h"
#include "../structures/molecule.h"
#include "../parameters.h"

CHARGEFW2_METHOD(KCM)

std::vector<double> KCM::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();
    size_t m = molecule.bonds().size();

    Eigen::MatrixXd W = Eigen::MatrixXd::Zero(m, m);
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(m, n);
    Eigen::VectorXd chi0 = Eigen::VectorXd::Zero(n);

    /* Compute
     *  q = (B.T @ W @ B + I)^-1 @ chi0 - chi0
     */

    for (size_t i = 0; i < n; i++) {
        chi0(i) = parameters_->atom()->parameter(atom::electronegativity)(molecule.atoms()[i]);
    }

    for (size_t i = 0; i < m; i++) {
        auto &bond = molecule.bonds()[i];
        auto &first = bond.first();
        auto &second = bond.second();

        W(i, i) = 1 / (parameters_->atom()->parameter(atom::hardness)(first) +
                            parameters_->atom()->parameter(atom::hardness)(second));

        B(i, first.index()) = 1;
        B(i, second.index()) = -1;
    }

    Eigen::VectorXd q = (B.transpose() * W * B + Eigen::MatrixXd::Identity(n, n)).partialPivLu().solve(chi0) - chi0;
    return std::vector<double>(q.data(), q.data() + q.size());
}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/translation_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Translation Matrix Example
 *  @date   Mar 11, 2022
 **/

#include <iostream>
#include <Eigen/Core>

void trans3dVec(const Eigen::Matrix<float, 4, 1>& v,
                Eigen::Matrix<float, 4, 1>& r,
		const Eigen::Matrix<float, 3, 1>& q)
{

  Eigen::Matrix<float, 4, 4> translation;
  translation << 1.0, 0.0, 0.0, q[0],
                 0.0, 1.0, 0.0, q[1],
		 0.0, 0.0, 1.0, q[2],
		 0.0, 0.0, 0.0, 1.0;

  std::cout << "v values: \n" << v << std::endl;
  std::cout << "translation values: \n" << translation << std::endl;

  r = translation * v;

}

int main(int argc, char* argv[])
{

  Eigen::Matrix<float, 4, 1> v;
  Eigen::Matrix<float, 4, 1> r;
  Eigen::Matrix<float, 3, 1> Q;

  // init matrix
  v << 0.5, 0.5, 0.0, 1.0;
  Q << 1.0, 1.0, 0.0;

  trans3dVec(v, r, Q);

  std::cout << "r:\n" << r << std::endl;

  return 0;
}


#include "../GEFE_utility.h"
#include <armadillo>
#include "gtest/gtest.h"
#include <string>

// Compares the normalized eigenvector values produced by GEFE with those
// produced by Armadillo's eig_sym().
void compareGEFEwithARMA(const arma::mat& H) {
    assert(H.is_square());
    std::string column_numbers;
    int numbers[H.n_rows];
    std::iota(numbers, numbers + H.n_rows,0);
    for (auto number : numbers) {
        column_numbers += std::to_string(number);
        column_numbers += " ";
    }
    const arma::vec ii(column_numbers);

    arma::vec arma_eigvals;
    arma::mat arma_eigvecs;
    arma::eig_sym(arma_eigvals, arma_eigvecs, H);

    for (int current_column = 0; current_column < H.n_cols; ++current_column) {
        const arma::vec gefe_eigvecs_column_j = getEigenvectorFromEigenvalues(H, ii, /*j=*/current_column);
        const arma::vec arma_eigvecs_column_j = arma::square(arma_eigvecs.col(current_column)); // Normalized.
        for (const double current_row : gefe_eigvecs_column_j) {
            EXPECT_DOUBLE_EQ(arma_eigvecs_column_j[current_row], gefe_eigvecs_column_j[current_row]);
        }
    }
}

TEST(TwoByTwo, Hermitian) {
    const arma::mat H1("-0.2414 0.3160;"
                       "0.3160 -0.8649");
    compareGEFEwithARMA(H1);

    const arma::mat H2("2.7694 0.8425;"
                       "0.8425 0.7254");
    compareGEFEwithARMA(H2);
}


/**
 * \file libs/numeric/ublasx/test/pow.cpp
 *
 * \brief Test suite for the \c pow operation.
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * Distributed under 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/pow.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( test_real_matrix_positive_exponent )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Matrix - Positive Exponent" );

	typedef double value_type;
	typedef std::size_t size_type;
	typedef ublas::matrix<value_type> matrix_type;

	const size_type n = 2;
	const double exp = 3;

	matrix_type A(n,n);

	A(0,0) = 1; A(0,1) = 2;
	A(1,0) = 4; A(1,1) = 5;

	matrix_type R;
	matrix_type expect_R;

	R = ublasx::pow(A, exp);

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

	expect_R = A;
	for (size_type i = 0; i < (exp-1); ++i)
	{
		expect_R = ublas::prod(expect_R, A);
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix_negative_exponent )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Matrix - Negative Exponent" );

	typedef double value_type;
	typedef std::size_t size_type;
	typedef ublas::matrix<value_type> matrix_type;

	const size_type n = 2;
	const double exp = -3;

	matrix_type A(n,n);

	A(0,0) = 1; A(0,1) = 2;
	A(1,0) = 4; A(1,1) = 5;

	matrix_type R;
	matrix_type expect_R;

	R = ublasx::pow(A, exp);

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

	matrix_type invA(n,n);
	invA(0,0) = -5.0/3.0; invA(0,1) =  2.0/3.0;
	invA(1,0) =  4.0/3.0; invA(1,1) = -1.0/3.0;

	expect_R = invA;
	for (size_type i = 0; i < (-exp-1); ++i)
	{
		expect_R = ublas::prod(expect_R, invA);
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix_zero_exponent )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real Matrix - Zero Exponent" );

	typedef double value_type;
	typedef std::size_t size_type;
	typedef ublas::matrix<value_type> matrix_type;

	const size_type n = 2;
	const double exp = 0;

	matrix_type A(n,n);

	A(0,0) = 1; A(0,1) = 2;
	A(1,0) = 4; A(1,1) = 5;

	matrix_type R;
	matrix_type expect_R = ublas::identity_matrix<value_type>(n);

	R = ublasx::pow(A, exp);

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_matrix_positive_exponent )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex Matrix - Positive Exponent" );

	typedef std::complex<double> in_value_type;
	typedef in_value_type out_value_type;
	typedef std::size_t size_type;
	typedef ublas::matrix<in_value_type> in_matrix_type;
	typedef ublas::matrix<out_value_type> out_matrix_type;

	const size_type n = 2;
	const int exp = 3;

	in_matrix_type A(n,n);

	A(0,0) = in_value_type(1,2); A(0,1) = in_value_type(2,3);
	A(1,0) = in_value_type(4,5); A(1,1) = in_value_type(5,6);

	out_matrix_type R;
	out_matrix_type expect_R;

	R = ublasx::pow(A, exp);

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "pow(A, " << exp << ") = " << R );

	expect_R = A;
	for (size_type i = 0; i < (exp-1); ++i)
	{
		expect_R = ublas::prod(expect_R, A);
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, n, n, tol );
}

int main()
{

	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'pow' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( test_real_matrix_positive_exponent );
	BOOST_UBLASX_TEST_DO( test_real_matrix_negative_exponent );
	BOOST_UBLASX_TEST_DO( test_real_matrix_zero_exponent );
	BOOST_UBLASX_TEST_DO( test_complex_matrix_positive_exponent );

	BOOST_UBLASX_TEST_END();
}


#include <dlib/optimization.h>

#include <bfgs/bfgs.h>
#include <bfgs/dval.h>

int _fun_call = 0;

// Rosenbrock's function.
class FRosen
{
private:
	const double _p1;

	template <typename T>
	T _f(const T& x1, const T& x2) const
	{
		++_fun_call;
		T v1 = x2 - x1 * x1;
		T v2 = _p1 - x1;
		T res = 100.0 * v1 * v1 + v2 * v2 + 1.0;
		return res;
	}

public:
	FRosen(double p1 = 1.0):
		_p1(p1)
	{
	}

	double operator()(const dlib::matrix<double,0,1>& m) const
	{
		return _f(m(0), m(1));
	}

	double operator()(const double* const x, int n) const
	{
		return _f(x[0], x[1]);
	}

	DVal<0> operator()(const DVal<0>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}

	template <uint32_t N>
	DVal<N> operator()(const DVal<N>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}
};

// Simple function.
class FSimple
{
private:
	const double _p1;

	template <typename T>
	T _f(const T& x1, const T& x2) const
	{
		++_fun_call;
		return _p1 * x1 * x1 + x2 * x2 + x1 + x2;
	}

public:
	FSimple(double p1 = 1.0):
		_p1(p1)
	{
	}

	double operator()(const dlib::matrix<double,0,1>& m) const
	{
		return _f(m(0), m(1));
	}

	double operator()(const double* const x, int n) const
	{
		return _f(x[0], x[1]);
	}

	DVal<0> operator()(const DVal<0>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}

	template <uint32_t N>
	DVal<N> operator()(const DVal<N>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}
};

template <typename fun>
void test(const fun& f)
{
	const int iter = 1000;
	// const int iter = 1;

	{
		dlib::matrix<double,0,1> dlib_point = {-1.0, -1.0};
		double dlib_y;

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			dlib_point(0) = -1.0;
			dlib_point(1) = -1.0;
			dlib_y = dlib::find_min_using_approximate_derivatives(
					dlib::bfgs_search_strategy(),
					// dlib::gradient_norm_stop_strategy(1e-7, 1000),
					dlib::objective_delta_stop_strategy(1e-7, 1000),
					f,
					dlib_point,
					-1,
					1e-8);
		}

		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tdlib" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << dlib_y  << std::endl <<  dlib_point;
	}

	BFGS bfgs;
	bfgs.set_grad_eps(1e-8);
	bfgs.set_stop_grad_eps(1e-7);
	bfgs.set_stop_step_eps(1e-7);
	bfgs.set_max_iter(1000);
	bfgs.set_line_central_diff(false);

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_num(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_num(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs num" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(5);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_num(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_num(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tlbfgs num" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_auto(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_auto(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs auto dynamic" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_auto<2>(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_auto<2>(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs auto fixed" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	std::cout << std::endl;
}

int main()
{
	std::cout.precision(9);

	for (int i = 1; i <= 100; i *= 10)
		test(FRosen(i));
	for (int i = 1; i <= 100; i *= 10)
		test(FSimple(i));

	return 0;
}


/**
 * @ file avgvalboundary_test.cc
 * @ brief NPDE homework AvgValBoundary code
 * @ author Simon Meierhans, edited by Oliver Rietmann
 * @ date 11.03.2019
 * @ copyright Developed at ETH Zurich
 */

#include "../avgvalboundary.h"

#include <gtest/gtest.h>
#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <memory>

namespace AvgValBoundary::test {

constexpr char mesh_file[] = CURRENT_SOURCE_DIR "/../../meshes/square.msh";

constexpr auto const_one = [](Eigen::Vector2d x) -> double { return 1.0; };

TEST(AvgValBoundary, TestH1SemiNorm) {
  // obtain dofh for lagrangian finite element space
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory), mesh_file);
  auto mesh = reader.mesh();
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // get solution of test problem
  Eigen::VectorXd mu = solveTestProblem(dofh);
  // compute H1 seminorm
  double h1s_norm = compH1seminorm(dofh, mu);

  ASSERT_NEAR(h1s_norm, 0.151178, 0.00001);
}

TEST(AvgValBoundary, TestBoundaryFunctional) {
  // obtain dofh for lagrangian finite element space
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory), mesh_file);
  auto mesh = reader.mesh();
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh);
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};

  // get solution of test problem
  Eigen::VectorXd mu = solveTestProblem(dofh);
  // compute boundary functional
  double boundary_functional = compBoundaryFunctional(dofh, mu, const_one);
  ASSERT_NEAR(boundary_functional, 0.77546, 0.00001);
}
}  // namespace AvgValBoundary::test


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix3f m = Matrix3f::Random();
Matrix3f y = Matrix3f::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the matrix y:" << endl << y << endl;
Matrix3f x;
x = m.fullPivHouseholderQr().solve(y);
assert(y.isApprox(m*x));
cout << "Here is a solution x to the equation mx=y:" << endl << x << endl;

}
  return 0;
}


/**
 * @file gradientflow_main.cc
 * @brief NPDE homework GradientFlow code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>
#include <vector>

#include "gradientflow.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  // Parameters and initial condition of the Gradient Flow ODE
  double T = 0.1;
  double lambda = 10.0;
  Eigen::Vector2d d(1.0, 0.0);
  Eigen::Vector2d y0(1.0, 0.0);

  // Approximate exact solution using small timesteps
  int M_ref = 10000;
  std::cout << "T = " << T << ", lambda = " << lambda << std::endl;
  Eigen::Vector2d y_ref =
      GradientFlow::SolveGradientFlow(d, lambda, y0, T, M_ref).back();

  std::cout << "Final value (exact): " << y_ref.transpose().format(CSVFormat)
            << std::endl;

  // Compute error table
  Eigen::VectorXi M(6);
  M << 10, 20, 40, 80, 160, 320;
  std::cout << "Error table:\n";
  std::cout << "M\t error norm" << std::endl;
  for (int i = 0; i < M.size(); ++i) {
    Eigen::Vector2d y_approx =
        GradientFlow::SolveGradientFlow(d, lambda, y0, T, M(i)).back();
    std::cout << M(i) << "\t" << (y_approx - y_ref).norm() << "\t" << std::endl;
  }

  return 0;
}


#include <chrono>
#include <random>
#include <mpi.h>
#include <boost/program_options.hpp>
#include "io.hpp"
#include "myDist.hpp"
#include "matGen_scalapack.hpp"
#include "io_mpi.hpp"

namespace po = boost::program_options;

int main(int argc, char* argv[]){

  MPI_Init(&argc, &argv);

  po::options_description descp("Artificial Matrices with ScaLAPACK: Options");

  descp.add_options()
       ("help,h","show the help"
		 "Attention, for the current implementation of parallel IO, please make sure N/mbsize/dim0 == 0 and N/bbsize/dim1 == 0")
       ("N", po::value<std::size_t>()->default_value(10), "number of row and column of matrices to be generated.")
       ("dim0", po::value<int>()->default_value(1), "first dimension of 2D MPI cartesian grid.")
       ("dim1", po::value<int>()->default_value(1), "second dimension of 2D MPI cartesian grid.")
       ("mbsize", po::value<std::size_t>()->default_value(5), "ScaLAPACK block size in the first dimension of 2D MPI cartesian grid.")
       ("nbsize", po::value<std::size_t>()->default_value(5), "ScaLAPACK block size in the second dimension of 2D MPI cartesian grid.")
       ("dmax", po::value<double>()->default_value(1), "A scalar which scales the generated eigenvalues, this makes"
							 " the maximum absolute eigenvalue is abs(dmax)." )
       ("epsilon", po::value<double>()->default_value(0.1), "This value is epsilon." )
       ("myDist", po::value<std::size_t>()->default_value(0), "Specifies my externel setup distribution for generating eigenvalues:\n "
							      "0: Uniform eigenspectrum lambda_k = dmax * (epsilon + k * (1 - epsilon) / n for k = 0, ..., n-1)\n "
                                                              "1: Geometric eigenspectrum lambda_k =lambda_k = epsilon^[(n - k) / n] for k = 0, ..., n-1) \n")
       ("mean", po::value<double>()->default_value(0.5), "Mean value of Normal distribution for the randomness." )
       ("stddev", po::value<double>()->default_value(1.0), "Standard deviation value of Normal distribution for the randomness." )
  ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, descp), vm);

  if (vm.count("help")) {
    std::cout << descp << std::endl;
    return 1;
  }

  //will be parsered by boost
  //2D dimmension of MPI grid
  int dim0 = vm["dim0"].as<int>();
  int dim1 = vm["dim1"].as<int>();
  //size of matrix to be generated
  std::size_t N = vm["N"].as<std::size_t>();
  //block size of scalapack
  std::size_t mbsize = vm["mbsize"].as<std::size_t>();
  std::size_t nbsize = vm["nbsize"].as<std::size_t>();
  //for the randomness with normal distribution
  double mean = vm["mean"].as<double>();
  double stddev = vm["stddev"].as<double>();
  double dmax = vm["dmax"].as<double>();
  double eps = vm["epsilon"].as<double>();
  std::size_t dist = vm["myDist"].as<std::size_t>();

  std::string mode;

  if(dist == 0){
    mode= "Uniform";
  }else if(dist == 1){
    mode = "Geometric";
  }

  int rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  int myproc, nprocs;
  blacs_pinfo( &myproc, &nprocs );

  int ictxt;
  int val;
  blacs_get( &ictxt, &i_zero, &val );
  blacs_gridinit( &ictxt, 'C', &dim0, &dim1 );

  std::chrono::high_resolution_clock::time_point start, end;
  std::chrono::duration<double> elapsed;

  start = std::chrono::high_resolution_clock::now();

  double *A;

  if(dist == 0){
      A = matGen_scalapack<double>(ictxt, N, mbsize, nbsize,
                        	       mean, stddev, myUniformDist<double>, N, eps, dmax);
  }else if(dist == 1){
      A = matGen_scalapack<double>(ictxt, N, mbsize, nbsize,
                                       mean, stddev, myGeometricDist<double>, N, eps, dmax);
  }else{
      A = matGen_scalapack<double>(ictxt, N, mbsize, nbsize, mean, stddev,
		      		[](std::size_t n, int x){
                		    double *eigenv = new double[n];
                		    for(auto k = 0; k < n; k++){
                    			eigenv[k] = k * (x + 1);
                		    }
                		    return eigenv;
            		        }, N, 1);

  }

  end = std::chrono::high_resolution_clock::now();

  elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(end - start);

  if(rank == 0) std::cout << "]> matrix generated in " << elapsed.count() << " seconds" << std::endl;

  std::ostringstream out_str;

  out_str << std::scientific << "matgen_m_" << N << "_" << mode << "_eps_"<< eps << "_dmax_" << dmax
            << ".bin";

  start = std::chrono::high_resolution_clock::now();

  wrtMatIntoBinaryMPI<double>(ictxt, A, out_str.str(), N, mbsize, nbsize);

  end = std::chrono::high_resolution_clock::now();

  elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(end - start);

  double gb = (double)N * (double)N * sizeof(double)/1024.0/1024.0/1024.0;

  if(rank == 0)
    std::cout << "]> matrix wrote in binary " << out_str.str() << " of " << gb << "GB in "
	<< elapsed.count() << " seconds" << std::endl;

  if(N <= 10){
      double matdiff = matDiff(ictxt, A, N, mbsize, nbsize, out_str.str());
  }

  delete[] A;

  MPI_Finalize();

}


#ifndef DATASETS_HPP
#define DATASETS_HPP

#include <Eigen/Core>
#include <vector>

#include "misc.hpp"

std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> house_value_dataset() {
    auto input_v = parse_csv<double>("house_data.csv", ',');
    Eigen::MatrixXd input(input_v.size(), 3);
    Eigen::VectorXd target(input_v.size(), 1);

    auto i = 0;
    for (const auto& row : input_v) {
        input(i, 0) = 1;
        for (auto j = 0; j < 2; j++) {
            input(i, j + 1) = row[j];
        }
        target(i) = row[2];
        i++;
    }

    return std::make_tuple(input, target);
}

inline Eigen::VectorXd correlatedData(double x) {
    return (Eigen::VectorXd(4) << 1, x, 2 * x, 0.5 * x*x).finished();
}

inline Eigen::VectorXd correlatedTarget(double x) {
    return (Eigen::VectorXd(2) << 5 * x + 3, x).finished();
}

std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> correlated_data_dataset(int n) {
    Eigen::VectorXd points = Eigen::VectorXd::Random(n, 1) * 100;
    Eigen::MatrixXd input(points.rows(), correlatedData(1).rows());
    Eigen::MatrixXd target(points.rows(), correlatedTarget(1).rows());

    for (auto i = 0; i < points.rows(); i++) {
        input.row(i) = correlatedData(points(i)).topRows(input.cols());
        target.row(i) = correlatedTarget(points(i));
    }

    return std::make_tuple(input, target);
}
#endif


#include <simpleuv/triangulate.h>
#include <Eigen/Dense>
#include <cmath>

namespace simpleuv
{

static Eigen::Vector3d norm(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, const Eigen::Vector3d &p3)
{
    auto side1 = p2 - p1;
    auto side2 = p3 - p1;
    auto perp = side1.cross(side2);
    return perp.normalized();
}

static float angle360(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &direct)
{
    auto angle = atan2((a.cross(b)).norm(), a.dot(b)) * 180.0 / 3.1415926;
    auto c = a.cross(b);
    if (c.dot(direct) < 0) {
        angle += 180;
    }
    return angle;
}

static Eigen::Vector3d vertexToEigenVector3d(const Vertex &vertex)
{
    return Eigen::Vector3d(vertex.xyz[0], vertex.xyz[1], vertex.xyz[2]);
}

static bool pointInTriangle(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &c, const Eigen::Vector3d &p)
{
    auto u = b - a;
    auto v = c - a;
    auto w = p - a;
    auto vXw = v.cross(w);
    auto vXu = v.cross(u);
    if (vXw.dot(vXu) < 0.0) {
        return false;
    }
    auto uXw = u.cross(w);
    auto uXv = u.cross(v);
    if (uXw.dot(uXv) < 0.0) {
        return false;
    }
    auto denom = uXv.norm();
    auto r = vXw.norm() / denom;
    auto t = uXw.norm() / denom;
    return r + t <= 1.0;
}

static Eigen::Vector3d ringNorm(const std::vector<Vertex> &vertices, const std::vector<size_t> &ring)
{
    Eigen::Vector3d normal;
    for (size_t i = 0; i < ring.size(); ++i) {
        auto j = (i + 1) % ring.size();
        auto k = (i + 2) % ring.size();
        const auto &enter = vertexToEigenVector3d(vertices[ring[i]]);
        const auto &cone = vertexToEigenVector3d(vertices[ring[j]]);
        const auto &leave = vertexToEigenVector3d(vertices[ring[k]]);
        normal += norm(enter, cone, leave);
    }
    return normal.normalized();
}

void triangulate(const std::vector<Vertex> &vertices, std::vector<Face> &faces, const std::vector<size_t> &ring)
{
    if (ring.size() < 3)
        return;
    std::vector<size_t> fillRing = ring;
    Eigen::Vector3d direct = ringNorm(vertices, fillRing);
    while (fillRing.size() > 3) {
        bool newFaceGenerated = false;
        for (decltype(fillRing.size()) i = 0; i < fillRing.size(); ++i) {
            auto j = (i + 1) % fillRing.size();
            auto k = (i + 2) % fillRing.size();
            const auto &enter = vertexToEigenVector3d(vertices[fillRing[i]]);
            const auto &cone = vertexToEigenVector3d(vertices[fillRing[j]]);
            const auto &leave = vertexToEigenVector3d(vertices[fillRing[k]]);
            auto angle = angle360(cone - enter, leave - cone, direct);
            if (angle >= 1.0 && angle <= 179.0) {
                bool isEar = true;
                for (size_t x = 0; x < fillRing.size() - 3; ++x) {
                    auto fourth = vertexToEigenVector3d(vertices[(i + 3 + k) % fillRing.size()]);
                    if (pointInTriangle(enter, cone, leave, fourth)) {
                        isEar = false;
                        break;
                    }
                }
                if (isEar) {
                    Face newFace;
                    newFace.indices[0] = fillRing[i];
                    newFace.indices[1] = fillRing[j];
                    newFace.indices[2] = fillRing[k];
                    faces.push_back(newFace);
                    fillRing.erase(fillRing.begin() + j);
                    newFaceGenerated = true;
                    break;
                }
            }
        }
        if (!newFaceGenerated)
            break;
    }
    if (fillRing.size() == 3) {
        Face newFace;
        newFace.indices[0] = fillRing[0];
        newFace.indices[1] = fillRing[1];
        newFace.indices[2] = fillRing[2];
        faces.push_back(newFace);
    }
}

}


/** @file 33.cpp Problem 33: Digit canceling fractions
 *
 * The fraction 49/98 is a curious fraction, as an inexperienced mathematician
 * in attempting to simplify it may incorrectly believe that 49/98 = 4/8, which
 * is correct, is obtained by cancelling the 9s.

 * We shall consider fractions like, 30/50 = 3/5, to be trivial examples.
 *
 * There are exactly four non-trivial examples of this type of fraction, less
 * than one in value, and containing two digits in the numerator and
 * denominator.
 *
 * If the product of these four fractions is given in its lowest common terms,
 * find the value of the denominator.
 */

/// @cond
#include <boost/multiprecision/cpp_int.hpp> // cpp_rational, denominator

#include <iostream> // cout
/// @endcond

using boost::multiprecision::cpp_rational;

bool equal(int n1, int d1, int n2, int d2)
{
    return d1 && d2 && cpp_rational(n1, d1) == cpp_rational(n2, d2);
}

int main()
{
    cpp_rational r = 1;
    for (int i = 10; i < 100; ++i) {
        for (int j = i + 1; j < 100; ++j) {
            if (i % 10 == 0 && j % 10 == 0)
                continue;   // Skip trivial examples.
            int a = i / 10, b = i % 10,
                c = j / 10, d = j % 10;
            if (       (b == d && equal(i, j, a, c))
                    || (b == c && equal(i, j, a, d))
                    || (a == d && equal(i, j, b, c))
                    || (a == c && equal(i, j, b, d)))
                r *= cpp_rational(i, j);
        }
    }
    std::cout << denominator(r) << std::endl;
}


// Copyright 2022 Eugen Hartmann. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <chrono>
#include <iostream>
#include <format>

#include <boost/exception/diagnostic_information.hpp>
#include <boost/exception_ptr.hpp>

#include <openssl/bn.h>

#include "base-helpers.h"
#include "diffie-hellman-boost.h"

using cpp_int = boost::multiprecision::cpp_int;

static std::string
ConverCPPIntToHex(const boost::multiprecision::cpp_int& num) {
  std::stringstream ss;
  ss << std::uppercase << std::hex << num;
  std::string hex = ss.str();
  // Add the top 0 if necessary
  if (hex.size() % 2) {
    hex = "0" + hex;
  }
  return hex;
}

static boost::multiprecision::cpp_int
ConveryHexToCPPInt(std::string_view hex) {
  return boost::multiprecision::cpp_int{std::string("0x") + std::string(hex)};
}

DiffieHellmanBoost::DiffieHellmanBoost() {
}

DiffieHellmanBoost::~DiffieHellmanBoost() {
}

std::string_view DiffieHellmanBoost::GetImplementationName() const {
  return "boost cpp_int DH";
}

Result DiffieHellmanBoost::GenerateParameters(int primeLengthInBits, int generator) {
  if (generator <= 1) {
    return Result{Result::Fail, "Bad generator value."};
  }

  try {
    unsigned int minMillerRabinChecks = GetMillerRabinMinChecks(primeLengthInBits);

    // We must use different generators for the tests and prime generation,
    // otherwise we get false positives.
    // https://www.boost.org/doc/libs/1_60_0/libs/multiprecision/doc/html/boost_multiprecision/tut/primetest.html
    boost::random::mt11213b primeGenerator(clock());
    boost::random::mt19937 testGenerator(clock());

    std::array<std::uint16_t, Primes.size()> mods{};

    while (true) {
      cpp_int prime = ProbableSafePrime(primeLengthInBits, generator, primeGenerator);
      if (miller_rabin_test(prime, minMillerRabinChecks, testGenerator)) {
        // The value is probably prime, see if (prime - 1) / 2 is also prime.
        if (miller_rabin_test((prime - 1) / 2, minMillerRabinChecks, testGenerator)) {
          prime_ = std::move(prime);
          generator_ = cpp_int{generator};
          break;
        }
      }
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::SetParameters(std::string_view hexPrime, std::string_view hexGenerator) {
  try {
    prime_ = ConveryHexToCPPInt(hexPrime);
    generator_ = ConveryHexToCPPInt(hexGenerator);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}
Result DiffieHellmanBoost::GetParameters(std::string* hexPrime, std::string* hexGenerator) const {
  try {
    *hexPrime = ConverCPPIntToHex(prime_);
    *hexGenerator = ConverCPPIntToHex(generator_);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

Result DiffieHellmanBoost::GenerateKeys() {
  try {
    // It will return prime len - 1 anyway because we always set the top bit.
    // For example, 255 for a 256 bit prime.
    unsigned int len = boost::multiprecision::msb(prime_);

    // Configure the generator.
    cpp_int max = cpp_int{1} << len;
    boost::random::uniform_int_distribution<cpp_int> generatePrivateKey{0, max};
    boost::random::mt11213b privateKeyGenerator(clock());

    // Generate the private key.
    cpp_int privateKey = generatePrivateKey(privateKeyGenerator);

    // Derive the public key.
    cpp_int publicKey = boost::multiprecision::powm(generator_, privateKey, prime_);

    privateKey_ = std::move(privateKey);
    publicKey_ = std::move(publicKey);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::GetPrivateKey(std::string* hexPrivateKey) const {
  try {
    hexPrivateKey->clear();
    hexPrivateKey->append(ConverCPPIntToHex(privateKey_));
    if (hexPrivateKey->empty()) {
      return {Result::Fail, "The private key is empty."};
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

Result DiffieHellmanBoost::GetPublicKey(std::string* hexPublicKey) const {
  try {
    hexPublicKey->clear();
    hexPublicKey->append(ConverCPPIntToHex(publicKey_));
    if (hexPublicKey->empty()) {
      return {Result::Fail, "The public key is empty."};
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

static Result CheckPublicKey(const boost::multiprecision::cpp_int& prime,
    const boost::multiprecision::cpp_int& publicKey) {
  try {
    if (publicKey <= 1) {
      return {Result::Fail, "The public key is too small."};
    }
    if (publicKey >= prime) {
      return {Result::Fail, "The public key is too large."};
    }

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::DeriveSharedSecret(std::string_view hexPeerPublicKey,
    std::string* hexSharedSecret) const {
  try {
    cpp_int peerPublicKey = ConveryHexToCPPInt(hexPeerPublicKey);

    if (Result result = CheckPublicKey(prime_, peerPublicKey); !result) {
      return result;
    }

    cpp_int sharedSecret = boost::multiprecision::powm(peerPublicKey, privateKey_, prime_);
    *hexSharedSecret = ConverCPPIntToHex(sharedSecret);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }
  return {Result::Success};
}

std::tuple<int, int> DiffieHellmanBoost::GetPrimeLength() const {
  try {
    // bytes = (bits - 1) / 8 + 1
    return {boost::multiprecision::msb(prime_) + 1,
      boost::multiprecision::msb(prime_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

std::tuple<int, int> DiffieHellmanBoost::GetPrivateKeyLength() const {
  try {
    return {boost::multiprecision::msb(privateKey_) + 1,
      boost::multiprecision::msb(privateKey_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

std::tuple<int, int> DiffieHellmanBoost::GetPublicKeyLength() const {
  try {
    return {boost::multiprecision::msb(publicKey_) + 1,
      boost::multiprecision::msb(publicKey_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

unsigned int DiffieHellmanBoost::GetMillerRabinMinChecks(int primeLengthInBits) {
  if (primeLengthInBits > 2048) {
    return 128;
  }
  return 64;
}

int DiffieHellmanBoost::GetTrivialDivisionNum(int primeLengthInBits) {
  if (primeLengthInBits <= 512) {
    return 64;
  } else if (primeLengthInBits <= 1024) {
    return 128;
  } else if (primeLengthInBits <= 2048) {
    return 384;
  } else if (primeLengthInBits <= 4096) {
    return 1024;
  }
  return Primes.size();
}

std::tuple<cpp_int, cpp_int> DiffieHellmanBoost::GetAddRem(int generator) {
  // See dh_builtin_genparams
  // libressl: crypto\dh\dh_gen.c
  boost::multiprecision::cpp_int add, rem;
  switch (generator) {
  case 2:
    add = 24;
    rem = 11;
    break;
  case 5:
    add = 10;
    rem = 3;
    break;
  default:
    add = 2;
    rem = 1;
  }
  return {add, rem};
}

cpp_int DiffieHellmanBoost::ProbableSafePrime(int primeLengthInBits,
    int generator, boost::random::mt11213b& primeGenerator) {
  int trivialDivisionNum = GetTrivialDivisionNum(primeLengthInBits);
  std::uint64_t maxDelta = 0xFFFFFFFFFFFFFFFFULL - Primes[trivialDivisionNum - 1];

  // Prepare cpp_int add/rem for the generator.
  auto [add, rem] = GetAddRem(generator);

  // Prepare the top bit.
  cpp_int topBit = cpp_int{0x80} << (primeLengthInBits - 8);

  // Prepare the bottom bit.
  cpp_int bottomBit{1};

  // Generation limits.
  cpp_int max = cpp_int{1} << primeLengthInBits;
  boost::random::uniform_int_distribution<cpp_int> generatePrime{0, max};

  cpp_int rnd;
  while (true) {
    rnd = generatePrime(primeGenerator);

    // Add top/bottom.
    rnd = rnd | topBit | bottomBit;

    // The prime should fulfill the condition prime % add == rem
    // in order to suit a given generator.
    cpp_int mod = rnd % add;
    rnd = rnd - mod + rem;

    // Probably the second condition is overhead?
    if (boost::multiprecision::msb(rnd) + 1 < primeLengthInBits || rnd < 5) {
      rnd += add;
    }

    auto [success, delta] = IncreaseProbabilityOfBeingPrime(primeLengthInBits, rnd, add);
    if (success) {
      rnd += delta;
      break;
    }
  }

  return rnd;
}

std::tuple<bool, std::uint64_t>
DiffieHellmanBoost::IncreaseProbabilityOfBeingPrime(int primeLengthInBits,
    cpp_int rnd, cpp_int add) {
  int trivialDivisionNum = GetTrivialDivisionNum(primeLengthInBits);

  std::uint64_t maxDelta = 0xFFFFFFFFFFFFFFFFULL - Primes[trivialDivisionNum - 1];
  std::uint64_t rnd64bit = rnd.convert_to<std::uint64_t>();
  std::uint64_t add64bit = add.convert_to<std::uint64_t>();
  std::uint64_t delta = 0;

  auto mods = std::make_unique<std::uint16_t[]>(Primes.size());

  for (int i = 1; i < trivialDivisionNum; ++i) {
    cpp_int mod = rnd % Primes[i];
    mods[i] = mod.convert_to<std::uint16_t>();
  }

  bool continueTesting = true;
  while (continueTesting) {
    continueTesting = false;
    for (int i = 1; i < trivialDivisionNum; ++i) {
      // Check if it is a prime.
      if (primeLengthInBits <= 31 && delta <= 0x7FFFFFFF) {
        std::uint64_t sq = static_cast<std::uint64_t>(Primes[i]) *
          static_cast<std::uint64_t>(Primes[i]);
        if (sq > rnd64bit + delta) {
          return {true, delta};
        }
      }
      // prime mod p == 1 implies q = (prime - 1) / 2 is divisible by p.
      if ((mods[i] + delta) % Primes[i] <= 1) {
        delta += add64bit;
        if (delta > maxDelta) {
          return {false, 0};
        }
        continueTesting = true;
        break;
      }
    }
  }

  return {true, delta};
}


//
// Created by kerail on 10.07.16.
//

#ifndef DAISU_SO3_HPP
#define DAISU_SO3_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <limits>

template<class T>
class SO3 {
 public:
  typedef Eigen::Matrix<T, 3, 1> Vector3;
  typedef Eigen::Matrix<T, 3, 3> Matrix33;
  typedef Eigen::Quaternion<T> Quaternion;

  SO3(const T &x, const T &y, const T &z) {
    m_coeffs = Eigen::Vector3d(x, y, z);
  }

  SO3(const Vector3 &v) {
    m_coeffs = v;
  }

  SO3(T angle, const Vector3 &v) {
    m_coeffs = v * angle;
  }

  SO3(const Quaternion &q_) {
    fromQuaternion(q_);
  }

  SO3(const Matrix33 &m) {
    assert(isRotationMatrix(m));
    //TODO: implement
  }

  Matrix33 getMatrix() const {
    Matrix33 m;
    const Vector3 &cfs = m_coeffs;

    T l2 = cfs.squaredNorm();
    T l = sqrt(l2);

    T sn_l, cs1_ll, cs;
    if (l == T(0)) {
      // if l is 0 sin(x)/x = 1
      sn_l = T(1);
    } else {
      sn_l = sin(l) / l;
    }

    cs = cos(l);
    static const T c_pi4096 = cos(M_PI / T(4096));
    if (cs > c_pi4096) {//fabs(l) < M_PI/T(4096)
      // when l is near nezo, we need to switch to more precise formula
      if (l2 == T(0)) {
        // when l2 is zero, we can precisely calculate limit
        cs1_ll = 1 / T(2);
      } else {
        // 1 - cos(x) = 2 * sin(x/2)^2
        T sn = sin(l / T(2));
        cs1_ll = T(2) * sn * sn / l2;
      }
    } else {
      // here l2 > 0 because abs(l) > pi/4096
      cs1_ll = (T(1) - cs) / l2;
    }

    Vector3 sn_ax = sn_l * m_coeffs;
    Vector3 cs1_l_ax = cs1_ll * m_coeffs;

    T tmp;
    tmp = cs1_l_ax.x() * m_coeffs.y();
    m.coeffRef(0, 1) = tmp - sn_ax.z();
    m.coeffRef(1, 0) = tmp + sn_ax.z();

    tmp = cs1_l_ax.x() * m_coeffs.z();
    m.coeffRef(0, 2) = tmp + sn_ax.y();
    m.coeffRef(2, 0) = tmp - sn_ax.y();

    tmp = cs1_l_ax.y() * m_coeffs.z();
    m.coeffRef(1, 2) = tmp - sn_ax.x();
    m.coeffRef(2, 1) = tmp + sn_ax.x();

    m.diagonal() = (cs1_l_ax.cwiseProduct(m_coeffs)).array() + cs;

//    Vector3 sq = cs1_l_ax.cwiseProduct(m_coeffs);
//    m.coeffRef(0, 0) = 1 + (-sq.y() - sq.z());
//    m.coeffRef(1, 1) = 1 + (-sq.x() - sq.z());
//    m.coeffRef(2, 2) = 1 + (-sq.x() - sq.y());

    // Rotation matrix checks
    assert(isRotationMatrix(m));
    return m;
  }

  Quaternion getQuaternion() const {
    const Vector3 cf2 = m_coeffs / T(2);
    T a = cf2.norm();
    if (a > T(0)) {
      T sn = sin(a) / a;
      return Quaternion(cos(a), cf2.x() * sn, cf2.y() * sn, cf2.z() * sn);
    } else {
      return Quaternion(T(1), cf2.x(), cf2.y(), cf2.z());
    }
  }

  Vector3 rotateVector(const Vector3 &v) const {
    T l2 = m_coeffs.squaredNorm();
    T l = sqrt(l2);

    T sa_l, ca, ca1_ll;
    if (l == T(0)) {
      sa_l = 1;
    } else {
      sa_l = sin(l) / l;
    }

    ca = cos(l);

    static const T c_pi4096 = cos(M_PI / T(4096));
    if (ca > c_pi4096) {//fabs(l) < M_PI/T(4096)
      // when l is near nezo, we need to switch to more precise formula
      if (l2 == T(0)) {
        // when l2 is zero, we can precisely calculate limit
        ca1_ll = 1 / T(2);
      } else {
        // 1 - cos(x) = 2 * sin(x/2)^2
        T sn = sin(l / T(2));
        ca1_ll = T(2) * sn * sn / l2;
      }
    } else {
      // here l2 > 0 because abs(l) > pi/4096
      ca1_ll = (T(1) - ca) / l2;
    }

    return v * ca + (m_coeffs * sa_l).cross(v) + m_coeffs * ((m_coeffs.dot(v)) * ca1_ll);
  }

  Vector3 coeffs() const {
    return m_coeffs;
  }

  T getAngle() const {
    return m_coeffs.norm();
  }

  Vector3 getAxis() const {
    T a = getAngle();
    return a > 0 ? (Vector3) (m_coeffs / a) : m_coeffs;
  }

  SO3<T> inverted() const {
    return SO3<T>(-m_coeffs);
  }

  SO3<T> operator *(const SO3<T> &r) {
    SO3<T> l(*this);
    return l *= r;
  }

  SO3<T> &operator *=(const SO3<T> &l) {
    fromQuaternion(getQuaternion() * l.getQuaternion());
    return *this;
  }

  SO3<T> operator ~() {
    return inverted();
  }

 private:
  Vector3 m_coeffs;

  void fromQuaternion(const Quaternion &q_) {
    const Quaternion &q = q_.w() >= T(0) ? q_ : Quaternion(-q_.coeffs());

    const Vector3 &qv = q.vec();
    T sinha = qv.norm();
    if (sinha > T(0)) {
      T angle = T(2) * atan2(sinha, q.w()); //NOTE: signed
      m_coeffs = qv * (angle / sinha);
    } else {
      // if l is too small, its norm can be equal 0 but norm_inf greater 0
      // probably w is much bigger that vec, use it as length
      m_coeffs = qv * (T(2) / q.w()); ////NOTE: signed
    }
  }
  bool isRotationMatrix(const Matrix33 &m) const {
    return ((m * m.transpose() - Matrix33::Identity()).norm() < 1e-10)
           && (fabs(fabs(m.determinant()) - 1) < 1e-10);
  }
};

typedef SO3<double> SO3d;
typedef SO3<float> SO3f;

#endif //DAISU_SO3_HPP


/********************************************************************************************/
/*                                                                                */
/*                                HSO3.hpp header file                                      */
/*                                                                                */
/* This file is not currently part of the Boost library. It is simply an example of the use */
/* quaternions can be put to. Hopefully it will be useful too.                              */
/*                                                                                */
/* This file provides tools to convert between quaternions and R^3 rotation matrices.       */
/*                                                                                */
/********************************************************************************************/

//  (C) Copyright Hubert Holin 2001.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#ifndef TEST_HSO3_HPP
#define TEST_HSO3_HPP

#include <algorithm>

#if    defined(__GNUC__) && (__GNUC__ < 3)
#include <boost/limits.hpp>
#else
#include <limits>
#endif

#include <stdexcept>
#include <string>

#include <boost/math/quaternion.hpp>

#if    defined(__GNUC__) && (__GNUC__ < 3)
// gcc 2.x ignores function scope using declarations, put them here instead:
using    namespace ::std;
using    namespace ::boost::math;
#endif

template<typename TYPE_FLOAT>
struct  R3_matrix
{
    TYPE_FLOAT a11, a12, a13;
    TYPE_FLOAT a21, a22, a23;
    TYPE_FLOAT a31, a32, a33;
};

// Note:    the input quaternion need not be of norm 1 for the following function

template<typename TYPE_FLOAT>
R3_matrix<TYPE_FLOAT>    quaternion_to_R3_rotation(::boost::math::quaternion<TYPE_FLOAT> const & q)
{
    using    ::std::numeric_limits;

    TYPE_FLOAT    a = q.R_component_1();
    TYPE_FLOAT    b = q.R_component_2();
    TYPE_FLOAT    c = q.R_component_3();
    TYPE_FLOAT    d = q.R_component_4();

    TYPE_FLOAT    aa = a*a;
    TYPE_FLOAT    ab = a*b;
    TYPE_FLOAT    ac = a*c;
    TYPE_FLOAT    ad = a*d;
    TYPE_FLOAT    bb = b*b;
    TYPE_FLOAT    bc = b*c;
    TYPE_FLOAT    bd = b*d;
    TYPE_FLOAT    cc = c*c;
    TYPE_FLOAT    cd = c*d;
    TYPE_FLOAT    dd = d*d;

    TYPE_FLOAT    norme_carre = aa+bb+cc+dd;

    if    (norme_carre <= numeric_limits<TYPE_FLOAT>::epsilon())
    {
        ::std::string            error_reporting("Argument to quaternion_to_R3_rotation is too small!");
        ::std::underflow_error   bad_argument(error_reporting);

        throw(bad_argument);
    }

    R3_matrix<TYPE_FLOAT>    out_matrix;

    out_matrix.a11 = (aa+bb-cc-dd)/norme_carre;
    out_matrix.a12 = 2*(-ad+bc)/norme_carre;
    out_matrix.a13 = 2*(ac+bd)/norme_carre;
    out_matrix.a21 = 2*(ad+bc)/norme_carre;
    out_matrix.a22 = (aa-bb+cc-dd)/norme_carre;
    out_matrix.a23 = 2*(-ab+cd)/norme_carre;
    out_matrix.a31 = 2*(-ac+bd)/norme_carre;
    out_matrix.a32 = 2*(ab+cd)/norme_carre;
    out_matrix.a33 = (aa-bb-cc+dd)/norme_carre;

    return(out_matrix);
}

    template<typename TYPE_FLOAT>
    void    find_invariant_vector(  R3_matrix<TYPE_FLOAT> const & rot,
                                    TYPE_FLOAT & x,
                                    TYPE_FLOAT & y,
                                    TYPE_FLOAT & z)
    {
        using    ::std::sqrt;

        using    ::std::numeric_limits;

        TYPE_FLOAT    b11 = rot.a11 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b12 = rot.a12;
        TYPE_FLOAT    b13 = rot.a13;
        TYPE_FLOAT    b21 = rot.a21;
        TYPE_FLOAT    b22 = rot.a22 - static_cast<TYPE_FLOAT>(1);
        TYPE_FLOAT    b23 = rot.a23;
        TYPE_FLOAT    b31 = rot.a31;
        TYPE_FLOAT    b32 = rot.a32;
        TYPE_FLOAT    b33 = rot.a33 - static_cast<TYPE_FLOAT>(1);

        TYPE_FLOAT    minors[9] =
        {
            b11*b22-b12*b21,
            b11*b23-b13*b21,
            b12*b23-b13*b22,
            b11*b32-b12*b31,
            b11*b33-b13*b31,
            b12*b33-b13*b32,
            b21*b32-b22*b31,
            b21*b33-b23*b31,
            b22*b33-b23*b32
        };

        TYPE_FLOAT *        where = ::std::max_element(minors, minors+9);

        TYPE_FLOAT          det = *where;

        if    (det <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_invariant_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        switch    (where-minors)
        {
            case 0:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b13*b22+b12*b23)/det;
                y = (-b11*b23+b13*b21)/det;

                break;

            case 1:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b12*b23+b13*b22)/det;
                z = (-b11*b22+b12*b21)/det;

                break;

            case 2:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b11*b23+b13*b21)/det;
                z = (-b12*b21+b11*b22)/det;

                break;

            case 3:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b13*b32+b12*b33)/det;
                y = (-b11*b33+b13*b31)/det;

                break;

            case 4:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b12*b33+b13*b32)/det;
                z = (-b11*b32+b12*b31)/det;

                break;

            case 5:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b11*b33+b13*b31)/det;
                z = (-b12*b31+b11*b32)/det;

                break;

            case 6:

                z = static_cast<TYPE_FLOAT>(1);

                x = (-b23*b32+b22*b33)/det;
                y = (-b21*b33+b23*b31)/det;

                break;

            case 7:

                y = static_cast<TYPE_FLOAT>(1);

                x = (-b22*b33+b23*b32)/det;
                z = (-b21*b32+b22*b31)/det;

                break;

            case 8:

                x = static_cast<TYPE_FLOAT>(1);

                y = (-b21*b33+b23*b31)/det;
                z = (-b22*b31+b21*b32)/det;

                break;

            default:

                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);

                throw(processing_error);

                break;
        }

        TYPE_FLOAT    vecnorm = sqrt(x*x+y*y+z*z);

        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Overflow error in find_invariant_vector!");
            ::std::overflow_error    processing_error(error_reporting);

            throw(processing_error);
        }

        x /= vecnorm;
        y /= vecnorm;
        z /= vecnorm;
    }

    template<typename TYPE_FLOAT>
    void    find_orthogonal_vector( TYPE_FLOAT x,
                                    TYPE_FLOAT y,
                                    TYPE_FLOAT z,
                                    TYPE_FLOAT & u,
                                    TYPE_FLOAT & v,
                                    TYPE_FLOAT & w)
    {
        using    ::std::abs;
        using    ::std::sqrt;

        using    ::std::numeric_limits;

        TYPE_FLOAT    vecnormsqr = x*x+y*y+z*z;

        if    (vecnormsqr <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        TYPE_FLOAT        lambda;

        TYPE_FLOAT        components[3] =
        {
            abs(x),
            abs(y),
            abs(z)
        };

        TYPE_FLOAT *    where = ::std::min_element(components, components+3);

        switch    (where-components)
        {
            case 0:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    v =
                    w = static_cast<TYPE_FLOAT>(0);
                    u = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -x/vecnormsqr;

                    u = static_cast<TYPE_FLOAT>(1) + lambda*x;
                    v = lambda*y;
                    w = lambda*z;
                }

                break;

            case 1:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    w = static_cast<TYPE_FLOAT>(0);
                    v = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -y/vecnormsqr;

                    u = lambda*x;
                    v = static_cast<TYPE_FLOAT>(1) + lambda*y;
                    w = lambda*z;
                }

                break;

            case 2:

                if    (*where <= numeric_limits<TYPE_FLOAT>::epsilon())
                {
                    u =
                    v = static_cast<TYPE_FLOAT>(0);
                    w = static_cast<TYPE_FLOAT>(1);
                }
                else
                {
                    lambda = -z/vecnormsqr;

                    u = lambda*x;
                    v = lambda*y;
                    w = static_cast<TYPE_FLOAT>(1) + lambda*z;
                }

                break;

            default:

                ::std::string        error_reporting("Impossible condition in find_invariant_vector");
                ::std::logic_error   processing_error(error_reporting);

                throw(processing_error);

                break;
        }

        TYPE_FLOAT    vecnorm = sqrt(u*u+v*v+w*w);

        if    (vecnorm <= numeric_limits<TYPE_FLOAT>::epsilon())
        {
            ::std::string            error_reporting("Underflow error in find_orthogonal_vector!");
            ::std::underflow_error   processing_error(error_reporting);

            throw(processing_error);
        }

        u /= vecnorm;
        v /= vecnorm;
        w /= vecnorm;
    }

    // Note:    we want [[v, v, w], [r, s, t], [x, y, z]] to be a direct orthogonal basis
    //            of R^3. It might not be orthonormal, however, and we do not check if the
    //            two input vectors are colinear or not.

    template<typename TYPE_FLOAT>
    void    find_vector_for_BOD(TYPE_FLOAT x,
                                TYPE_FLOAT y,
                                TYPE_FLOAT z,
                                TYPE_FLOAT u,
                                TYPE_FLOAT v,
                                TYPE_FLOAT w,
                                TYPE_FLOAT & r,
                                TYPE_FLOAT & s,
                                TYPE_FLOAT & t)
    {
        r = +y*w-z*v;
        s = -x*w+z*u;
        t = +x*v-y*u;
    }

template<typename TYPE_FLOAT>
inline bool                                is_R3_rotation_matrix(R3_matrix<TYPE_FLOAT> const & mat)
{
    using    ::std::abs;

    using    ::std::numeric_limits;

    return    (
                !(
                    (abs(mat.a11*mat.a11+mat.a21*mat.a21+mat.a31*mat.a31 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a11*mat.a12+mat.a21*mat.a22+mat.a31*mat.a32 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a12*mat.a12+mat.a22*mat.a22+mat.a32*mat.a32 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a11*mat.a13+mat.a21*mat.a23+mat.a31*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    //(abs(mat.a12*mat.a13+mat.a22*mat.a23+mat.a32*mat.a33 - static_cast<TYPE_FLOAT>(0)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())||
                    (abs(mat.a13*mat.a13+mat.a23*mat.a23+mat.a33*mat.a33 - static_cast<TYPE_FLOAT>(1)) > static_cast<TYPE_FLOAT>(10)*numeric_limits<TYPE_FLOAT>::epsilon())
                )
            );
}

template<typename TYPE_FLOAT>
::boost::math::quaternion<TYPE_FLOAT>    R3_rotation_to_quaternion(    R3_matrix<TYPE_FLOAT> const & rot,
                                                                    ::boost::math::quaternion<TYPE_FLOAT> const * hint = 0)
{
    using    ::boost::math::abs;

    using    ::std::abs;
    using    ::std::sqrt;

    using    ::std::numeric_limits;

    if    (!is_R3_rotation_matrix(rot))
    {
        ::std::string        error_reporting("Argument to R3_rotation_to_quaternion is not an R^3 rotation matrix!");
        ::std::range_error   bad_argument(error_reporting);

        throw(bad_argument);
    }

    ::boost::math::quaternion<TYPE_FLOAT>    q;

    if    (
            (abs(rot.a11 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a22 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())&&
            (abs(rot.a33 - static_cast<TYPE_FLOAT>(1)) <= numeric_limits<TYPE_FLOAT>::epsilon())
        )
    {
        q = ::boost::math::quaternion<TYPE_FLOAT>(1);
    }
    else
    {
        TYPE_FLOAT    cos_theta = (rot.a11+rot.a22+rot.a33-static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    stuff = (cos_theta+static_cast<TYPE_FLOAT>(1))/static_cast<TYPE_FLOAT>(2);
        TYPE_FLOAT    cos_theta_sur_2 = sqrt(stuff);
        TYPE_FLOAT    sin_theta_sur_2 = sqrt(1-stuff);

        TYPE_FLOAT    x;
        TYPE_FLOAT    y;
        TYPE_FLOAT    z;

        find_invariant_vector(rot, x, y, z);

        TYPE_FLOAT    u;
        TYPE_FLOAT    v;
        TYPE_FLOAT    w;

        find_orthogonal_vector(x, y, z, u, v, w);

        TYPE_FLOAT    r;
        TYPE_FLOAT    s;
        TYPE_FLOAT    t;

        find_vector_for_BOD(x, y, z, u, v, w, r, s, t);

        TYPE_FLOAT    ru = rot.a11*u+rot.a12*v+rot.a13*w;
        TYPE_FLOAT    rv = rot.a21*u+rot.a22*v+rot.a23*w;
        TYPE_FLOAT    rw = rot.a31*u+rot.a32*v+rot.a33*w;

        TYPE_FLOAT    angle_sign_determinator = r*ru+s*rv+t*rw;

        if        (angle_sign_determinator > +numeric_limits<TYPE_FLOAT>::epsilon())
        {
            q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, +x*sin_theta_sur_2, +y*sin_theta_sur_2, +z*sin_theta_sur_2);
        }
        else if    (angle_sign_determinator < -numeric_limits<TYPE_FLOAT>::epsilon())
        {
            q = ::boost::math::quaternion<TYPE_FLOAT>(cos_theta_sur_2, -x*sin_theta_sur_2, -y*sin_theta_sur_2, -z*sin_theta_sur_2);
        }
        else
        {
            TYPE_FLOAT    desambiguator = u*ru+v*rv+w*rw;

            if    (desambiguator >= static_cast<TYPE_FLOAT>(1))
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, +x, +y, +z);
            }
            else
            {
                q = ::boost::math::quaternion<TYPE_FLOAT>(0, -x, -y, -z);
            }
        }
    }

    if    ((hint != 0) && (abs(*hint+q) < abs(*hint-q)))
    {
        return(-q);
    }

    return(q);
}

#endif /* TEST_HSO3_HPP */


//

#include <string>
#include <vector>
//#include <tuple>
#include <cstdio>

#include <Eigen/Sparse>

#include "SparseMatrix.hpp"
#include "SparseVector.hpp"
#include "DenseVector.hpp"
#include "KernelComposer.hpp"
#include "KernelComposerHelpers.hpp"

#include "gtest/gtest.h"

TEST(KCEndToEnd,Thermal){
    swSim::KernelComposer ThermalKernel;
    swSim::SparseMatrix DiffusionKernel;
    swSim::SparseVector SurfaceFlux;
    swSim::DenseVector TikTemp;
    swSim::DenseVector TocTemp;

    std::vector<double> HeatPulse;

    std::vector<double> TikTempStdVec;
    std::vector<double> TocTempStdVec;
    Eigen::SparseVector<double> SurfaceFluxEigVec;
    Eigen::SparseMatrix<double,1> DiffusionKernelEigMat;

    double Fo=0.125;
    int Nx,Ny,Nz,Nt,N,idx;
    Nx=100;Ny=100;Nz=100;Nt=10000;N=Nx*Ny*Nz;

    DiffusionKernelEigMat.resize(N,N);
    SurfaceFluxEigVec.resize(N);
    for(int kz=0;kz<Nz;kz++){
        for(int ky=0;ky<Ny;ky++){
            for(int kx=0;kx<Nx;kx++){
                idx=kx+Nx*(ky+Ny*kz);
                DiffusionKernelEigMat.coeffRef(idx,idx)=1.0;
                if(kx>0 && kx<(Nx-1) &&
                   ky>0 && ky<(Ny-1) &&
                   kz>0 && kz<(Nz-1)){
                    DiffusionKernelEigMat.coeffRef(idx,idx)-=6.0*Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx-1)=Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx+1)=Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx-Nx)=Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx+Nx)=Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx-(Nx*Ny))=Fo;
                    DiffusionKernelEigMat.coeffRef(idx,idx+(Nx*Ny))=Fo;
                };
                TikTempStdVec.push_back(0.0);
                TocTempStdVec.push_back(0.0);
            };
        };
    };
    DiffusionKernelEigMat.makeCompressed();
    for(int ky=(Ny*7/16);ky<(Ny*9/16);ky++){
        for(int kx=(Nx*7/16);kx<(Nx*9/16);kx++){
            SurfaceFluxEigVec.coeffRef(kx+Nx*ky)=1.0;
        };
    };
    for(int kt=0;kt<Nt/2;kt++){
        HeatPulse.push_back(1.0);
    };

    DiffusionKernel.mat=DiffusionKernelEigMat;
    TikTemp.setVector(TikTempStdVec);
    TocTemp.setVector(TocTempStdVec);
    SurfaceFlux.vec=SurfaceFluxEigVec;

    ThermalKernel.setSparseMatrix(&DiffusionKernel);
    ThermalKernel.setDenseVector(&TikTemp);
    ThermalKernel.setDenseVector(&TocTemp);
    ThermalKernel.setSparseVector(&SurfaceFlux);

    //Configure test, ensure configuration is correct
    ASSERT_FALSE(ThermalKernel.isCommitted());
    ASSERT_EQ(ThermalKernel.setSPMV(DiffusionKernel.getName(),
                TikTemp.getName(),TocTemp.getName(),1.0,0.0),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.setSPaXPY(SurfaceFlux.getName(),
                TocTemp.getName(),&HeatPulse),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.setSPMV(DiffusionKernel.getName(),
                TocTemp.getName(),TikTemp.getName(),1.0,0.0),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.setSPaXPY(SurfaceFlux.getName(),
                TikTemp.getName(),&HeatPulse),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.kernelCommit(),swSim::KC_OK);
    ASSERT_TRUE(ThermalKernel.isCommitted());
    ASSERT_EQ(ThermalKernel.run(Nt),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.getDeviceData(TikTemp.getName()),swSim::KC_OK);
    ASSERT_EQ(ThermalKernel.kernelDecommit(),swSim::KC_OK);
    ASSERT_FALSE(ThermalKernel.isCommitted());

    FILE* pfile;
    pfile=fopen("ThermalSim.dat","wb");
    ASSERT_NE(pfile,nullptr);
    fwrite(TikTemp.getValueArray(),sizeof(double),N,pfile);
    fclose(pfile);
};


/**
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <tclap/CmdLine.h>

#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <memory>
#include <numeric>

#include "Applications/FileIO/AsciiRasterInterface.h"
#include "GeoLib/AABB.h"
#include "GeoLib/Point.h"
#include "GeoLib/Raster.h"

double compute2DGaussBellCurveValues(GeoLib::Point const& point,
                                     GeoLib::AABB const& aabb)
{
    auto const sigma_x = (aabb.getMaxPoint() - aabb.getMinPoint())[0] / 3;
    auto const sigma_y = (aabb.getMaxPoint() - aabb.getMinPoint())[1] / 3;

    auto const mid_point = (aabb.getMaxPoint() + aabb.getMinPoint()) / 2;

    return std::exp(
        -0.5 * std::pow((point[0] - mid_point[0]), 2) / std::pow(sigma_x, 2) -
        0.5 * std::pow((point[1] - mid_point[1]), 2) / std::pow(sigma_y, 2));
}

double computeSinXSinY(GeoLib::Point const& point, GeoLib::AABB const& aabb)
{
    auto const aabb_size = aabb.getMaxPoint() - aabb.getMinPoint();
    auto const offset = aabb.getMinPoint();

    return std::sin((point[0] - offset[0]) / aabb_size[0] *
                    boost::math::double_constants::pi) *
           std::sin((point[1] - offset[1]) / aabb_size[1] *
                    boost::math::double_constants::pi);
}

int main(int argc, char* argv[])
{
    TCLAP::CmdLine cmd("Add values to raster.", ' ', "0.1");

    TCLAP::ValueArg<std::string> out_raster_arg(
        "o",
        "output_raster",
        "the output raster is stored to a file of this name",
        true,
        "",
        "filename for raster output");
    cmd.add(out_raster_arg);

    TCLAP::ValueArg<double> scaling_arg(
        "",
        "scaling_value",
        "value the function sin(x pi) sin(y pi) will be scaled with",
        false,
        1,
        "double value");
    cmd.add(scaling_arg);

    TCLAP::ValueArg<double> offset_arg(
        "",
        "offset_value",
        "constant added to the function 'scaling * sin(x pi) * sin(y pi)'",
        false,
        0,
        "double value");
    cmd.add(offset_arg);

    TCLAP::ValueArg<double> ll_x_arg(
        "",
        "ll_x",
        "x coordinate of lower left point of axis aligned rectangular region",
        false,
        0,
        "double value");
    cmd.add(ll_x_arg);
    TCLAP::ValueArg<double> ll_y_arg(
        "",
        "ll_y",
        "y coordinate of lower left point of axis aligned rectangular region",
        false,
        0,
        "double value");
    cmd.add(ll_y_arg);
    TCLAP::ValueArg<double> ur_x_arg("",
                                     "ur_x",
                                     "x coordinate of the upper right point of "
                                     "axis aligned rectangular region",
                                     false,
                                     0,
                                     "double value");
    cmd.add(ur_x_arg);
    TCLAP::ValueArg<double> ur_y_arg("",
                                     "ur_y",
                                     "y coordinate of the upper right point of "
                                     "axis aligned rectangular region",
                                     false,
                                     0,
                                     "double value");

    cmd.add(ur_y_arg);
    std::vector<std::string> allowed_functions_vector{"sinxsiny", "exp"};
    TCLAP::ValuesConstraint<std::string> allowed_functions(
        allowed_functions_vector);
    TCLAP::ValueArg<std::string> function_arg(
        "f", "function", "Name of the function used to modify the raster", true,
        "", &allowed_functions);
    cmd.add(function_arg);
    TCLAP::ValueArg<std::string> input_arg("i", "input",
                                           "Name of the input raster (*.asc)",
                                           true, "", "input file name");
    cmd.add(input_arg);

    cmd.parse(argc, argv);

    std::array input_points = {
        GeoLib::Point{{ll_x_arg.getValue(), ll_y_arg.getValue(), 0}},
        GeoLib::Point{{ur_x_arg.getValue(), ur_y_arg.getValue(), 0}}};
    GeoLib::AABB const aabb{std::begin(input_points), std::end(input_points)};

    auto const s = scaling_arg.getValue();
    auto const offset = offset_arg.getValue();

    std::unique_ptr<GeoLib::Raster> const raster(
        FileIO::AsciiRasterInterface::getRasterFromASCFile(
            input_arg.getValue()));
    auto const& header = raster->getHeader();
    auto const& origin = header.origin;

    std::function<double(GeoLib::Point const& p, GeoLib::AABB const& aabb)>
        computeFunctionValue = function_arg.getValue() == "sinxsiny"
                                   ? computeSinXSinY
                                   : compute2DGaussBellCurveValues;

    for (std::size_t r = 0; r < header.n_rows; r++)
    {
        for (std::size_t c = 0; c < header.n_cols; c++)
        {
            GeoLib::Point const p{{origin[0] + header.cell_size * c,
                                   origin[1] + header.cell_size * r, 0.0}};
            if (!aabb.containsPoint(p, std::numeric_limits<double>::epsilon()))
            {
                continue;
            }

            (*raster)(r, c) += offset + s * computeFunctionValue(p, aabb);
        }
    }

    FileIO::AsciiRasterInterface::writeRasterAsASC(*raster,
                                                   out_raster_arg.getValue());
    return EXIT_SUCCESS;
}


/**
 * @file solvecauchyproblem.cc
 * @brief NPDE exam problem summer 2019 "CLEmpiricFlux" code
 * @author Oliver Rietmann
 * @date 19.07.2019
 * @copyright Developed at ETH Zurich
 */

#include "solvecauchyproblem.h"
#include "uniformcubicspline.h"

#include <cmath>

#include <Eigen/Core>

namespace CLEmpiricFlux {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector2d findSupport(const UniformCubicSpline &f,
                            Eigen::Vector2d initsupp, double t) {
  Eigen::Vector2d result;
#if SOLUTION
  Eigen::Vector2d speed = {f.derivative(-1.0), f.derivative(1.0)};
  result = initsupp + t * speed;
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
template <typename FUNCTOR>
Eigen::VectorXd semiDiscreteRhs(const Eigen::VectorXd &mu0, double h,
                                FUNCTOR &&numFlux) {
  int m = mu0.size();
  Eigen::VectorXd mu1(m);
#if SOLUTION
  mu1(0) = -1.0 / h * (numFlux(mu0(0), mu0(1)) - numFlux(mu0(0), mu0(0)));
  for (int j = 1; j < m - 1; ++j) {
    mu1(j) =
        -1.0 / h * (numFlux(mu0(j), mu0(j + 1)) - numFlux(mu0(j - 1), mu0(j)));
  }
  mu1(m - 1) =
      -1.0 / h *
      (numFlux(mu0(m - 1), mu0(m - 1)) - numFlux(mu0(m - 2), mu0(m - 1)));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return mu1;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
template <typename FUNCTOR>
Eigen::VectorXd RalstonODESolver(FUNCTOR &&rhs, Eigen::VectorXd mu0, double tau,
                                 int n) {
#if SOLUTION
  for (int i = 0; i < n; ++i) {
    Eigen::VectorXd k1 = rhs(mu0);
    Eigen::VectorXd k2 = rhs(mu0 + tau * 2.0 / 3.0 * k1);
    mu0 = mu0 + 0.25 * tau * (k1 + 3.0 * k2);
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return mu0;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::VectorXd solveCauchyProblem(const UniformCubicSpline &f,
                                   const Eigen::VectorXd &mu0, double h,
                                   double T) {
  Eigen::VectorXd muT(mu0.size());
#if SOLUTION
  double tau = std::min(h / std::abs(f.derivative(-1.0)),
                        h / std::abs(f.derivative(1.0)));
  double n = (int)std::floor(T / tau);
  GodunovFlux godunovFlux(f);
  auto rhs = [h, &godunovFlux](const Eigen::VectorXd &mu) {
    return semiDiscreteRhs(mu, h, godunovFlux);
  };
  muT = RalstonODESolver(rhs, mu0, tau, n);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return muT;
}
/* SAM_LISTING_END_4 */

}  // namespace CLEmpiricFlux


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * 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.
 */
/*!
 * \file
 * \brief Test of variable class.
 */
#include "num_collect/auto_diff/forward/variable.h"

#include <Eigen/Core>
#include <catch2/catch_template_test_macros.hpp>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "num_collect/auto_diff/forward/create_diff_variable.h"

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::forward::variable<Scalar>", "", float, double) {
    using variable_type = num_collect::auto_diff::forward::variable<TestType>;

    SECTION("construct with all arguments") {
        constexpr auto value = static_cast<TestType>(1.234);
        constexpr auto diff = static_cast<TestType>(2.345);
        const auto var = variable_type(value, diff);

        REQUIRE_THAT(var.value(), Catch::Matchers::WithinRel(value));
        REQUIRE(var.has_diff());
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(diff));
    }

    SECTION("construct with one argument") {
        constexpr auto value = static_cast<TestType>(1.234);
        const auto var = variable_type(value);

        REQUIRE_THAT(var.value(), Catch::Matchers::WithinRel(value));
        REQUIRE_FALSE(var.has_diff());
        REQUIRE_THROWS(var.diff());
    }

    SECTION("construct without arguments") {
        constexpr auto value = static_cast<TestType>(0.0);
        const auto var = variable_type();

        REQUIRE_THAT(var.value(), Catch::Matchers::WithinRel(value));
        REQUIRE_FALSE(var.has_diff());
        REQUIRE_THROWS(var.diff());
    }

    SECTION("add a variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        variable_type var = var1;
        var += var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() + var2.value()));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() + var2.diff()));
    }

    SECTION("add a value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        variable_type var = var1;
        var += var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() + var2));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(var1.diff()));
    }

    SECTION("subtract a variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        variable_type var = var1;
        var -= var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() - var2.value()));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() - var2.diff()));
    }

    SECTION("subtract a value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        variable_type var = var1;
        var -= var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() - var2));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(var1.diff()));
    }

    SECTION("multiply a variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        variable_type var = var1;
        var *= var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() * var2.value()));
        REQUIRE_THAT(var.diff(),
            Catch::Matchers::WithinRel(
                var2.value() * var1.diff() + var1.value() * var2.diff()));
    }

    SECTION("multiply a value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        variable_type var = var1;
        var *= var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() * var2));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var2 * var1.diff()));
    }

    SECTION("divide by a variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        variable_type var = var1;
        var /= var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() / var2.value()));
        REQUIRE_THAT(var.diff(),
            Catch::Matchers::WithinRel(
                (var2.value() * var1.diff() - var1.value() * var2.diff()) /
                (var2.value() * var2.value())));
    }

    SECTION("divide by a value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        variable_type var = var1;
        var /= var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() / var2));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() / var2));
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::forward::variable<Scalar> operators", "", float,
    double) {
    using variable_type = num_collect::auto_diff::forward::variable<TestType>;

    SECTION("variable + variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 + var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() + var2.value()));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() + var2.diff()));
    }

    SECTION("value + variable") {
        const auto var1 = static_cast<TestType>(1.234);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 + var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1 + var2.value()));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(var2.diff()));
    }

    SECTION("variable + value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        const auto var = var1 + var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() + var2));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(var1.diff()));
    }

    SECTION("variable - variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 - var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() - var2.value()));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() - var2.diff()));
    }

    SECTION("value - variable") {
        const auto var1 = static_cast<TestType>(1.234);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 - var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1 - var2.value()));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(-var2.diff()));
    }

    SECTION("variable - value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        const auto var = var1 - var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() - var2));
        REQUIRE_THAT(var.diff(), Catch::Matchers::WithinRel(var1.diff()));
    }

    SECTION("variable * variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 * var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() * var2.value()));
        REQUIRE_THAT(var.diff(),
            Catch::Matchers::WithinRel(
                var2.value() * var1.diff() + var1.value() * var2.diff()));
    }

    SECTION("value * variable") {
        const auto var1 = static_cast<TestType>(1.234);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 * var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1 * var2.value()));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1 * var2.diff()));
    }

    SECTION("variable * value") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        const auto var = var1 * var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() * var2));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var2 * var1.diff()));
    }

    SECTION("variable / variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 / var2;
        REQUIRE_THAT(var.value(),
            Catch::Matchers::WithinRel(var1.value() / var2.value()));
        REQUIRE_THAT(var.diff(),
            Catch::Matchers::WithinRel(
                (var2.value() * var1.diff() - var1.value() * var2.diff()) /
                (var2.value() * var2.value())));
    }

    SECTION("value / variable") {
        const auto var1 = static_cast<TestType>(1.234);
        const auto var2 = variable_type(3.456, -4.567);
        const auto var = var1 / var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1 / var2.value()));
        REQUIRE_THAT(var.diff(),
            Catch::Matchers::WithinRel(
                (-var1 * var2.diff()) / (var2.value() * var2.value())));
    }

    SECTION("variable / variable") {
        const auto var1 = variable_type(1.234, 2.345);
        const auto var2 = static_cast<TestType>(3.456);
        const auto var = var1 / var2;
        REQUIRE_THAT(
            var.value(), Catch::Matchers::WithinRel(var1.value() / var2));
        REQUIRE_THAT(
            var.diff(), Catch::Matchers::WithinRel(var1.diff() / var2));
    }
}

// NOLINTNEXTLINE
TEMPLATE_TEST_CASE(
    "num_collect::auto_diff::forward::variable<Scalar, Vector> operations", "",
    float, double) {
    using diff_type = Eigen::Matrix<TestType, 2, 1>;
    using variable_type =
        num_collect::auto_diff::forward::variable<TestType, diff_type>;

    const variable_type left =
        num_collect::auto_diff::forward::create_diff_variable<TestType,
            diff_type>(1.234, 2, 0);
    const variable_type right =
        num_collect::auto_diff::forward::create_diff_variable<TestType,
            diff_type>(2.345, 2, 1);

    SECTION("addition") {
        const variable_type res = left + right;
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() + right.value()));
        REQUIRE_THAT(res.diff(), eigen_approx(left.diff() + right.diff()));
    }

    SECTION("subtraction") {
        const variable_type res = left - right;
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() - right.value()));
        REQUIRE_THAT(res.diff(), eigen_approx(left.diff() - right.diff()));
    }

    SECTION("multiplication") {
        const variable_type res = left * right;
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() * right.value()));
        REQUIRE_THAT(res.diff(),
            eigen_approx(
                right.value() * left.diff() + left.value() * right.diff()));
    }

    SECTION("division") {
        const variable_type res = left / right;
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(left.value() / right.value()));
        REQUIRE_THAT(res.diff(),
            eigen_approx(
                (right.value() * left.diff() - left.value() * right.diff()) /
                (right.value() * right.value())));
    }
}

TEST_CASE("Eigen::Matrix<num_collect::auto_diff::forward::variable>") {
    using diff_type = Eigen::Vector2d;
    using variable_type =
        num_collect::auto_diff::forward::variable<double, diff_type>;
    using vector_type = Eigen::Matrix<variable_type, 2, 1>;
    using num_collect::auto_diff::forward::create_diff_variable;

    const auto vec =
        vector_type(create_diff_variable<double, diff_type>(1.234, 2, 0),
            create_diff_variable<double, diff_type>(2.345, 2, 1));

    SECTION("prod") {
        const variable_type res = vec.prod();
        REQUIRE_THAT(res.value(),
            Catch::Matchers::WithinRel(vec(0).value() * vec(1).value()));
        REQUIRE_THAT(res.diff(),
            eigen_approx(diff_type(vec(1).value(), vec(0).value())));
    }
}