//  (C) Copyright Nick Thompson, 2018
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_MODULE numerical_differentiation_test

#include <cmath>
#include <limits>
#include <iostream>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/tools/numerical_differentiation.hpp>

using std::abs;
using std::pow;
using boost::math::tools::finite_difference_derivative;
using boost::math::tools::complex_step_derivative;
using boost::math::cyl_bessel_j;
using boost::math::cyl_bessel_j_prime;
using boost::math::constants::half;
using boost::multiprecision::cpp_dec_float_100;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::multiprecision::cpp_bin_float_quad;

template<class Real, size_t order>
void test_order(size_t points_to_test)
{
    std::cout << "Testing order " <<  order  << " derivative error estimate on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << std::fixed << std::scientific;
    auto f = [](Real t) { return cyl_bessel_j<Real>(1, t); };
    Real min = -100000.0;
    Real max = -min;
    Real x = min;
    Real max_error = 0;
    Real max_relative_error_in_error = 0;
    size_t j = 0;
    size_t failures = 0;
    while (j < points_to_test)
    {
        x = min + (Real) 2*j*max/ (Real) points_to_test;
        Real error_estimate;
        Real computed = finite_difference_derivative<decltype(f), Real, order>(f, x, &error_estimate);
        Real expected = (Real) cyl_bessel_j_prime<Real>(1, x);
        Real error = abs(computed - expected);
        // The error estimate is provided under the assumption that the function is evaluated to 1 ULP.
        // Presumably no one will be too offended by this estimate being off by a factor of 2 or so.
        if (error > 2*error_estimate)
        {
            ++failures;
            Real relative_error_in_error = abs(error - error_estimate)/ error;
            if (relative_error_in_error > max_relative_error_in_error)
            {
                max_relative_error_in_error = relative_error_in_error;
            }
            if (relative_error_in_error > 2)
            {
                throw std::logic_error("Relative error in error is too high!");
            }
        }
        if (error > max_error)
        {
            max_error = error;
        }
        ++j;
    }
    //std::cout << "Maximum error :" << max_error << "\n";
    //std::cout <<  "Error estimate failed " << failures << " times out of " << points_to_test << "\n";
    //std::cout << "Failure rate: " << (double) failures / (double) points_to_test << "\n";
    //std::cout << "Maximum error in estimated error = " << max_relative_error_in_error << "\n";
    //Real convergence_rate = (Real) order/ (Real) (order + 1);
    //std::cout << "eps^(order/order+1) = " << pow(std::numeric_limits<Real>::epsilon(), convergence_rate) << "\n\n\n";

    bool max_error_good = max_error < 2*sqrt(std::numeric_limits<Real>::epsilon());
    BOOST_TEST(max_error_good);

    bool error_estimate_good = max_relative_error_in_error < (Real) 2;
    BOOST_TEST(error_estimate_good);

    double failure_rate = (double) failures / (double) points_to_test;
    BOOST_CHECK_SMALL(failure_rate, 0.05);
}

template<class Real>
void test_bessel()
{
    std::cout << "Testing numerical derivatives of Bessel's function on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);

    Real eps = std::numeric_limits<Real>::epsilon();
    Real x = static_cast<Real>(25.1);
    auto f = [](Real t) { return cyl_bessel_j(12, t); };

    Real computed = finite_difference_derivative<decltype(f), Real, 1>(f, x);
    Real expected = cyl_bessel_j_prime(12, x);
    Real error_estimate = 4*abs(f(x))*sqrt(eps);
    //std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << "cyl_bessel_j_prime: " << expected << std::endl;
    //std::cout << "First order fd    : " << computed << std::endl;
    //std::cout << "Error             : " << abs(computed - expected) << std::endl;
    //std::cout << "a prior error est : " << error_estimate << std::endl;

    BOOST_CHECK_CLOSE_FRACTION(expected, computed, 10*error_estimate);

    computed = finite_difference_derivative<decltype(f), Real, 2>(f, x);
    expected = cyl_bessel_j_prime(12, x);
    error_estimate = abs(f(x))*pow(eps, boost::math::constants::two_thirds<Real>());
    //std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << "cyl_bessel_j_prime: " << expected << std::endl;
    //std::cout << "Second order fd   : " << computed << std::endl;
    //std::cout << "Error             : " << abs(computed - expected) << std::endl;
    //std::cout << "a prior error est : " << error_estimate << std::endl;

    BOOST_CHECK_CLOSE_FRACTION(expected, computed, 50*error_estimate);

    computed = finite_difference_derivative<decltype(f), Real, 4>(f, x);
    expected = cyl_bessel_j_prime(12, x);
    error_estimate = abs(f(x))*pow(eps, (Real) 4 / (Real) 5);
    //std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << "cyl_bessel_j_prime: " << expected << std::endl;
    //std::cout << "Fourth order fd   : " << computed << std::endl;
    //std::cout << "Error             : " << abs(computed - expected) << std::endl;
    //std::cout << "a prior error est : " << error_estimate << std::endl;

    BOOST_CHECK_CLOSE_FRACTION(expected, computed, 25*error_estimate);

    computed = finite_difference_derivative<decltype(f), Real, 6>(f, x);
    expected = cyl_bessel_j_prime(12, x);
    error_estimate = abs(f(x))*pow(eps, (Real)  6/ (Real) 7);
    //std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << "cyl_bessel_j_prime: " << expected << std::endl;
    //std::cout << "Sixth order fd    : " << computed << std::endl;
    //std::cout << "Error             : " << abs(computed - expected) << std::endl;
    //std::cout << "a prior error est : " << error_estimate << std::endl;

    BOOST_CHECK_CLOSE_FRACTION(expected, computed, 100*error_estimate);

    computed = finite_difference_derivative<decltype(f), Real, 8>(f, x);
    expected = cyl_bessel_j_prime(12, x);
    error_estimate = abs(f(x))*pow(eps, (Real)  8/ (Real) 9);
    //std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    //std::cout << "cyl_bessel_j_prime: " << expected << std::endl;
    //std::cout << "Eighth order fd   : " << computed << std::endl;
    //std::cout << "Error             : " << abs(computed - expected) << std::endl;
    //std::cout << "a prior error est : " << error_estimate << std::endl;

    BOOST_CHECK_CLOSE_FRACTION(expected, computed, 25*error_estimate);
}

// Example of a function which is subject to catastrophic cancellation using finite-differences, but is almost perfectly stable using complex step:
template<class RealOrComplex>
RealOrComplex moler_example(RealOrComplex x)
{
    using std::sin;
    using std::cos;
    using std::exp;

    RealOrComplex cosx = cos(x);
    RealOrComplex sinx = sin(x);
    return exp(x)/(cosx*cosx*cosx + sinx*sinx*sinx);
}

template<class RealOrComplex>
RealOrComplex moler_example_derivative(RealOrComplex x)
{
    using std::sin;
    using std::cos;
    using std::exp;

    RealOrComplex expx = exp(x);
    RealOrComplex cosx = cos(x);
    RealOrComplex sinx = sin(x);
    RealOrComplex coscubed_sincubed = cosx*cosx*cosx + sinx*sinx*sinx;
    return (expx/coscubed_sincubed)*(1 - 3*(sinx*sinx*cosx - sinx*cosx*cosx)/ (coscubed_sincubed));
}

template<class Real>
void test_complex_step()
{
    using std::abs;
    using std::complex;
    using std::isfinite;
    using std::isnormal;
    std::cout << "Testing numerical derivatives of Bessel's function on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10);
    Real x = -100;
    while ( x < 100 )
    {
        if (!isfinite(moler_example(x)))
        {
            x += 1;
            continue;
        }
        Real expected = moler_example_derivative<Real>(x);
        Real computed = complex_step_derivative(moler_example<complex<Real>>, x);
        if (!isfinite(expected))
        {
            x += 1;
            continue;
        }
        if (abs(expected) <= std::numeric_limits<Real>::epsilon())
        {
            bool issmall = computed < std::numeric_limits<Real>::epsilon();
            BOOST_TEST(issmall);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(expected, computed, 200*std::numeric_limits<Real>::epsilon());
        }
        x += 1;
    }
}

BOOST_AUTO_TEST_CASE(numerical_differentiation_test)
{
    test_complex_step<float>();
    test_complex_step<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_complex_step<long double>();
#endif
    test_bessel<float>();
    test_bessel<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_bessel<long double>();
#endif
    test_bessel<cpp_bin_float_50>();

    size_t points_to_test = 1000;
    test_order<float, 1>(points_to_test);
    test_order<double, 1>(points_to_test);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_order<long double, 1>(points_to_test);
#endif
    test_order<cpp_bin_float_50, 1>(points_to_test);

    test_order<float, 2>(points_to_test);
    test_order<double, 2>(points_to_test);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_order<long double, 2>(points_to_test);
#endif
    test_order<cpp_bin_float_50, 2>(points_to_test);

    test_order<float, 4>(points_to_test);
    test_order<double, 4>(points_to_test);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_order<long double, 4>(points_to_test);
#endif

    test_order<float, 6>(points_to_test);
    test_order<double, 6>(points_to_test);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_order<long double, 6>(points_to_test);
#endif

    test_order<float, 8>(points_to_test);
    test_order<double, 8>(points_to_test);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_order<long double, 8>(points_to_test);
#endif
}


// Implementing the class that is defined in the header file: ChooserOption.hpp
//
// (c) Sudhansh Dua

#include "ChooserOption.hpp"
#include <string>
#include <boost/math/distributions.hpp>
#include <cmath>

using namespace std;
using namespace boost::math;

//	Gaussian functions using boost libraries
double ChooserOption::N(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return cdf(Standard_normal, x);
}

double ChooserOption::n(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return pdf(Standard_normal, x);
}

double ChooserOption::ChooserPrice() const
{
	return ::ChooserPrice(S, K, T, t, r, sig, b);
}

// Initialising all the default values
void ChooserOption::init()
{
	//	Default values
	r = 0.03;
	sig = 0.2;
	K = 105;
	S = 100;			//	Default stock price
	b = r;				//	Black - Scholes(1973) stock option model : b = r
	T = 1;
	t = 0.5;			//	time that has passed since t = 0

}

void ChooserOption::copy(const ChooserOption& option)
{
	r = option.r;
	t = option.t;
	T = option.T;
	sig = option.sig;
	K = option.K;
	b = option.b;
	S = option.S;
}

//	Constructors and destructor
//	Default Constructor
ChooserOption::ChooserOption() : Option()
{
	init();
}

//	Copy constructor
ChooserOption::ChooserOption(const ChooserOption& option) : Option(option)
{
	copy(option);
}

//	Constructor that accepts values
ChooserOption::ChooserOption(const double& S1, const double& K1, const double& T1, const double& t1, const double& r1,
	const double& sig1, const double& b1) : Option(), S(S1), K(K1), T(T1), t(t1), r(r1), sig(sig1), b(b1) {}

//	Destructor
ChooserOption::~ChooserOption() {}

//	Assignment Operator
ChooserOption& ChooserOption::operator = (const ChooserOption& option)
{
	if (this == &option)
	{
		return *this;		//	Self-assignment check!
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

// Member function that calculate the option price
double ChooserOption::Price() const
{
	return ChooserPrice();
}

// Global functions
double ChooserPrice(const double S, const double K, const double T, const double t, const double r, const double sig, const double b)
{

	double y1 = (log(S / K) + (b * T) + (sig * sig * 0.5 * t)) / (sig * sqrt(t));
	double y2 = y1 - (sig * sqrt(t));

	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);

	double w = (S * exp((b - r) * T) * cdf(standard_normal, d1)) - (K * exp(-r * T) * cdf(standard_normal, d2)) - (S * exp((b - r) * T) * cdf(standard_normal, -y1)) + (K * exp(-r * T) * cdf(standard_normal, -y2));
	return w;

}


#ifndef RBF_HPP
#define RBF_HPP

#pragma once

#include <vector>
#include <glimac/Cube.hpp>
#include <math.h>
#include <algorithm>
#include <numeric>
#include <Eigen/Dense>

namespace glimac{

enum class FunctionType
    {
        Gaussian,         // f(r) = exp(-(epsilon * r)^2)
        ThinPlateSpline,  // f(r) = (r^2) * log(r)
        InverseQuadratic, // f(r) = (1 + (epsilon * r)^2)^(-1)
        BiharmonicSpline, // f(r) = r
        Multiquadric,     // f(r) = sqrt(1 + (epsiolon * r^2))
    };

 class ControlPoint
{
public:
    glm::vec3 m_position;
    float m_value;
};

    float getRBF(FunctionType type, const glm::vec3 vecA, const glm::vec3 vecB, const float epsilon);
    Eigen::VectorXf caclulRBF(std::vector <ControlPoint> &list_controls, FunctionType type,  const float epsilon);
    void applyRBF(std::vector <Cube> &list_cubes,  std::vector <ControlPoint> &list_controls, FunctionType type);

}

#endif


/*
 * 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 Example of 1-dimensional RBF interpolation.
 */
#include <Eigen/Core>
#include <pybind11/eigen.h>
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"
#include "num_collect/interp/kernel/kernel_interpolator.h"
#include "num_collect/interp/kernel/rbf_kernel.h"

auto main() -> int {
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::kernel_interpolator;
    using num_collect::interp::kernel::rbf_kernel;

    using kernel_type =
        rbf_kernel<euclidean_distance<double>, gaussian_rbf<double>>;

    const auto vars = std::vector<double>{0.0, 0.1, 0.5, 0.4, 1.2, 1.0};
    const auto data = Eigen::VectorXd{{0.0, 0.2, 0.5, 0.7, 1.0, 2.0}};

    auto interpolator = kernel_interpolator<kernel_type>();
    interpolator.compute(vars, data);

    constexpr num_collect::index_type num_samples = 201;
    const Eigen::VectorXd sample_vars =
        Eigen::VectorXd::LinSpaced(num_samples, -0.1, 1.3);
    Eigen::VectorXd sample_data = Eigen::VectorXd::Zero(num_samples);
    Eigen::VectorXd sample_upper = Eigen::VectorXd::Zero(num_samples);
    Eigen::VectorXd sample_lower = Eigen::VectorXd::Zero(num_samples);
    for (num_collect::index_type i = 0; i < num_samples; ++i) {
        const auto [mean, var] =
            interpolator.evaluate_mean_and_variance_on(sample_vars(i));
        const auto err = 3.0 * std::sqrt(var);
        sample_data(i) = mean;
        sample_upper(i) = mean + err;
        sample_lower(i) = mean - err;
    }

    pybind11::scoped_interpreter interpreter;
    auto go = pybind11::module::import("plotly.graph_objects");
    auto fig = go.attr("Figure")();

    // upper and lower limits
    fig.attr("add_trace")(go.attr("Scatter")(pybind11::arg("x") = sample_vars,
        pybind11::arg("y") = sample_lower, pybind11::arg("mode") = "lines",
        pybind11::arg("name") = "Lower bound (3 sigma)"));
    fig.attr("add_trace")(go.attr("Scatter")(pybind11::arg("x") = sample_vars,
        pybind11::arg("y") = sample_upper, pybind11::arg("mode") = "lines",
        pybind11::arg("name") = "Uppper bound (3 sigma)",
        pybind11::arg("fill") = "tonexty"));

    // interpolated line
    fig.attr("add_trace")(go.attr("Scatter")(pybind11::arg("x") = sample_vars,
        pybind11::arg("y") = sample_data, pybind11::arg("mode") = "lines",
        pybind11::arg("name") = "Interpolation"));

    // inputs
    fig.attr("add_trace")(go.attr("Scatter")(pybind11::arg("x") = vars,
        pybind11::arg("y") = data, pybind11::arg("mode") = "markers",
        pybind11::arg("name") = "Input data"));

    fig.attr("update_layout")(pybind11::arg("title") = "RBF Interpolation",
        pybind11::arg("xaxis_title") = "x", pybind11::arg("yaxis_title") = "y");

    fig.attr("write_html")("interp_rbf_1dim.html");
    fig.attr("write_image")("interp_rbf_1dim.png");
}


#include <iostream>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <random>
#include <limits>

#include "GRANSAC.hpp"
#include "least_squares_model.hpp"

#include <vector>
#include <Eigen/QR>

using namespace std;

int main(int argc, char *argv[])
{
    if (argc != 1 && argc != 3)
    {
        std::cout << "[ USAGE ]: " << argv[0] << " [<Image Size> = 1000] [<nPoints> = 500]" << std::endl;
        return -1;
    }

    int side = 400;
    int n_points = 50;
    if (argc == 3)
    {
        side = std::atoi(argv[1]);
        n_points = std::atoi(argv[2]);
    }

    cv::Mat img_canvas(side, side, CV_8UC3);
    img_canvas.setTo(255);

    // Randomly generate points in a 2D plane roughly aligned in a line for testing
    std::random_device seed_device;
    std::mt19937 RNG = std::mt19937(seed_device());

    std::uniform_int_distribution<int> uni_dist(0, side - 1); // [Incl, Incl]
    int perturb = 40;
    std::normal_distribution<float> perturb_dist(0, perturb);

    std::vector<std::shared_ptr<GRANSAC::AbstractParameter>> cand_points;
    while (n_points > 0){
        int diag = uni_dist(RNG);
        float x = diag + perturb_dist(RNG);
        // y = 2100-20x+0.05*x*x
        float y;
        if (n_points > 5 )
            y = 2100 - 20 * x + 0.05 * x * x + perturb_dist(RNG);
        else
            y = uni_dist(RNG);

        if (x<=side && x > 0 && y<=side && y > 0){
            cv::Point pt(floor(x), floor(y));
            cv::circle(img_canvas, pt, floor(side / 100) + 2, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);

            std::shared_ptr<GRANSAC::AbstractParameter> cand_pt = std::make_shared<GRANSAC::Point2D>(pt.x, pt.y);
            cand_points.push_back(cand_pt);

            n_points -= 1;
        }
    }

    //set grid
    std::map<string, float> additional_params = {{"img_width", float(side)}, {"img_height", float(side)}, {"grid_num_x", float(side/10)}, {"grid_num_y", float(side/10)}};

    //draw grid
    cv::Mat img_overlay;
    float alpha = 0.3;
    img_canvas.copyTo(img_overlay);
    for (int i=0; i < side/10; i++){
        cv::Point pt_1(i*10, 0);
        cv::Point pt_2(i*10, side);
        cv::line(img_overlay, pt_1, pt_2, cv::Scalar(0, 100, 0), 1, cv::LINE_AA, 0);
        cv::Point pt_3(0, i*10);
        cv::Point pt_4(side, i*10);
        cv::line(img_overlay, pt_3, pt_4, cv::Scalar(0, 100, 0), 1, cv::LINE_AA, 0);
    }
    cv::addWeighted(img_overlay, alpha, img_canvas, 1 - alpha, 0, img_canvas);

    // estimate parameters with RANSAC
    int param_num = 4; // 3rd order
    additional_params["sample_number"]=10; // sample number for over-deterministic least squares
    GRANSAC::RANSAC<GRANSAC::LinearLeastSquaresModel<4>, 4> estimator;
    estimator.initialize(1, 100, additional_params); // Threshold, iterations, the threshold is p-1 distance on grid, 1 means 1 grid cell distance

    float time_average = 0;
    for (int i=0; i<100; i++){
        int start = cv::getTickCount();
        estimator.estimate(cand_points);
        int end = cv::getTickCount();
        float time_of_trial = float(end - start) / float(cv::getTickFrequency()) * 1000.0; // [ms]
        time_average += time_of_trial;
    }
    std::cout << "RANSAC took, average time in 100 trials: " << time_average/100 << " ms." << std::endl;

    // draw definition points from best model
    auto best_inliers = estimator.getBestInliers();
    if (best_inliers.size() > 0)
    {
        for (auto &inlier : best_inliers)
        {
            auto RPt = std::dynamic_pointer_cast<GRANSAC::Point2D>(inlier);
            cv::Point pt(floor(RPt->m_point2D[0]), floor(RPt->m_point2D[1]));
            cv::circle(img_canvas, pt, floor(side / 100), cv::Scalar(0, 255, 0), -1, cv::LINE_AA);
        }
    }

    // draw curve from best model
    auto best_line = estimator.getBestModel();
    if (best_line)
    {
        cout << "coefficients are : " << endl;
        for (auto each : best_line->getModelCoefficients()){
            cout << each << ", ";
        }
        cout << endl;

        std::vector<float> x_values, y_values, coeff;
        for (int i=0; i<best_line->getModelDefParams().size(); i++){
            auto best_line_pt = std::dynamic_pointer_cast<GRANSAC::Point2D>(best_line->getModelDefParams()[i]);
            x_values.push_back(best_line_pt->m_point2D[0]); //x
            y_values.push_back(best_line_pt->m_point2D[1]); //x
            cv::Point pt(floor(best_line_pt->m_point2D[0]), floor(best_line_pt->m_point2D[1]));
            cv::circle(img_canvas, pt, floor(side / 100), cv::Scalar(0, 0, 255), -1, cv::LINE_AA);
        }

        // draw polynomial line
        float start_point_x = 0;
        float end_point_x = side;
        vector<cv::Point2f> curve_points;
        //Define the curve through equation. In this example, a simple parabola
        for (float x = start_point_x; x <= end_point_x; x+=1){
            float y = 0;
            for (int j = 0; j < best_line->getModelCoefficients().size(); j++){
                y += best_line->getModelCoefficients()[j]*std::pow(x, float(j));
            }

            cv::Point2f new_point = cv::Point2f(x, y);                  //resized to better visualize
            curve_points.push_back(new_point);                       //add point to vector/list
        }
        for (int i = 0; i < curve_points.size() - 1; i++){
            cv::line(img_canvas, curve_points[i], curve_points[i + 1], cv::Scalar(0,255,0), 2, CV_AA);
        }
    }

    while (true)
    {
        cv::imshow("RANSAC Example", img_canvas);
        char Key = cv::waitKey(1);
        if (Key == 27)
            return 0;
    }

    return 0;
}


#define BOOST_TEST_MODULE Logic test
#include <boost/test/unit_test.hpp>

#include "Variable.h"

using namespace omnn::math;

BOOST_AUTO_TEST_CASE(ifz_tests)
{
    // two different bits
    // if a=0 then b=1 else b=0
    // if a=1 then b=0 else b=1
    Variable a,b;
    auto e = a.Equals(1).Ifz(b.Equals(0), b.Equals(1));
    {
        auto ee = e;
        ee.Eval(a, 0);
        ee.Eval(b, 1);
        ee.optimize();
        BOOST_TEST(ee==0);
    }
    {
        auto ee = e;
        ee.Eval(a, 1);
        ee.Eval(b, 0);
        ee.optimize();
        BOOST_TEST(ee==0);
    }
// TODO :   {
//        auto ee = e;
//        ee.Eval(a,1); // b == 0
//        ee.optimize();
//        auto solutions = ee(b);
//        BOOST_TEST(solutions.size()==1);
//        BOOST_TEST(*solutions.begin()==0);
//    }
// TODO :
//    {
//        auto ee = e;
//        ee.Eval(a, 0); // b == 1
//        ee.optimize();
//        auto solutions = ee(b);
//        BOOST_TEST(solutions.size()==1);
//        BOOST_TEST(*solutions.begin()==1);
//    }
}


#include <iostream>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using namespace boost::numeric::odeint;

const double b = 0.1;
const double g = 0.05;

typedef boost::array< double , 3 > state_type;

void sir( const state_type &x , state_type &dxdt , double t )
{
    dxdt[0] = -b * x[0] * x[1];
    dxdt[1] = b * x[0] * x[1] - g * x[1];
    dxdt[2] = g * x[1];
}

void write_sir(const state_type &x , const double t )
{
    std::cout << t << ' ' << x[0] << ' ' << x[1] << ' ' << x[2] << std::endl;
}

int main(int argc, char **argv)
{
    state_type x = { 0.99 , 0.01 , 0.0 }; // initial conditions
    integrate( sir , x , 0.0 , 200.0 , 0.1 , write_sir );
}


// Spline fit with unknown parameter positions.

#include <iostream>

#include "main.h"
#include <unsupported/Eigen/LevenbergMarquardt>
#include <unsupported/Eigen/SparseExtra>

#include <Eigen/Eigen>
#include "unsupported/Eigen/src/SparseExtra/BlockSparseQR.h"
#include "unsupported/Eigen/src/SparseExtra/BlockDiagonalSparseQR.h"

// This disables some useless Warnings on MSVC.
// It is intended to be done for this test only.
#include <Eigen/src/Core/util/DisableStupidWarnings.h>

#define NUM_SAMPLE_POINTS 100

struct SplineParameter {
  int segment;
  double t;
};

template <typename _Scalar>
struct SplineFitting : LevenbergMarquardtFunctor<_Scalar>
{
  typedef LevenbergMarquardtFunctor<Scalar> Base;

  typedef int Index;
  typedef Matrix<Scalar, Dynamic, 1> InputType;
  typedef Matrix<Scalar, Dynamic, 1> ValueType;
  typedef Matrix<Scalar, Dynamic, 1> StepType;

  typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType;

  typedef SparseQR<SparseMatrix<Scalar>, COLAMDOrdering<int> > BlockSolver;
  typedef ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic> > DenseSolver;
  typedef BlockDiagonalSparseQR<JacobianType, DenseSolver> LeftSuperBlockSolver;
  typedef BlockSparseQR<JacobianType, LeftSuperBlockSolver, DenseSolver> QRSolver;

  const Eigen::Matrix<double, 3, Eigen::Dynamic> SplinePoints;

  static const int nParamsModel = 5;

  SplineFitting(Eigen::Matrix<double, 3, Eigen::Dynamic>& points) :
    Base(nParamsModel + points.cols(), points.cols() * 2),
    SplinePoints(points)
  {
  }

  // Functor functions
  int operator()(const InputType& uv, ValueType& fvec) {
    int npoints = SplinePoints.cols();
    auto params = uv.tail(nParamsModel);
    double a = params[0];
    double b = params[1];
    double x0 = params[2];
    double y0 = params[3];
    double r = params[4];
    for (int i = 0; i < npoints; i++) {
      double t = uv[i];
      double x = a*cos(t)*cos(r) - b*sin(t)*sin(r) + x0;
      double y = a*cos(t)*sin(r) + b*sin(t)*cos(r) + y0;
      fvec(2 * i + 0) = SplinePoints(0, i) - x;
      fvec(2 * i + 1) = SplinePoints(1, i) - y;
    }

    return 0;
  }

  int df(const InputType& uv, JacobianType& fjac) {
    // X_i - (a*cos(t_i) + x0)
    // Y_i - (b*sin(t_i) + y0)
    int npoints = SplinePoints.cols();
    auto params = uv.tail(nParamsModel);
    double a = params[0];
    double b = params[1];
    double r = params[4];
    for (int i = 0; i<npoints; i++) {
      double t = uv(i);
      fjac.coeffRef(2 * i, npoints + 0) = -cos(t)*cos(r);
      fjac.coeffRef(2 * i, npoints + 1) = +sin(t)*sin(r);
      fjac.coeffRef(2 * i, npoints + 2) = -1;
      fjac.coeffRef(2 * i, npoints + 4) = +a*cos(t)*sin(r) + b*sin(t)*cos(r);
      fjac.coeffRef(2 * i, i) = +a*cos(r)*sin(t) + b*sin(r)*cos(t);

      fjac.coeffRef(2 * i + 1, npoints + 0) = -cos(t)*sin(r);
      fjac.coeffRef(2 * i + 1, npoints + 1) = -sin(t)*cos(r);
      fjac.coeffRef(2 * i + 1, npoints + 3) = -1;
      fjac.coeffRef(2 * i + 1, npoints + 4) = -a*cos(t)*cos(r) + b*sin(t)*sin(r);
      fjac.coeffRef(2 * i + 1, i) = +a*sin(r)*sin(t) - b*cos(r)*cos(t);
    }

    fjac.makeCompressed();
    return 0;
  }

  void initQRSolver(QRSolver &qr) {
    // set block size
    qr.getLeftSolver().setSparseBlockParams(2, 1);
    qr.setBlockParams(SplinePoints.cols());
  }

  void increment_in_place(InputType* x, StepType const& p)
  {
    *x += p;
  }

  double estimateNorm(InputType const& x, StepType const& diag)
  {
    return x.cwiseProduct(diag).stableNorm();
  }
};

void testSplineFitting() {

  // Spline PARAMETERS
  double a, b, x0, y0, r;
  a = 7.5;
  b = 2;
  x0 = 17.;
  y0 = 23.;
  r = 0.23;

  std::cout << "GROUND TRUTH   " << " ";
  std::cout << "a=" << a << "\t";
  std::cout << "b=" << b << "\t";
  std::cout << "x0=" << x0 << "\t";
  std::cout << "y0=" << y0 << "\t";
  std::cout << "r=" << r*180. / EIGEN_PI << "\t";
  std::cout << std::endl;

  // CREATE DATA SAMPLES

  int nDataPoints = NUM_SAMPLE_POINTS;
  Eigen::Matrix<double, 3, Eigen::Dynamic> SplinePoints;
  SplinePoints.resize(3, nDataPoints);
  double incr = 1.3*EIGEN_PI / double(nDataPoints);
  for (int i = 0; i<nDataPoints; i++) {
    double t = double(i)*incr;
    SplinePoints(0, i) = x0 + a*cos(t)*cos(r) - b*sin(t)*sin(r);
    SplinePoints(1, i) = y0 + a*cos(t)*sin(r) + b*sin(t)*cos(r);
    SplinePoints(2, i) = 1;
  }

  // INITIAL PARAMS
  SplineFitting<double>::InputType lm_params;
  auto& params = lm_params;
  params.resize(SplineFitting<double>::nParamsModel + nDataPoints);
  double minX, minY, maxX, maxY;
  minX = maxX = SplinePoints(0, 0);
  minY = maxY = SplinePoints(1, 0);
  for (int i = 0; i<SplinePoints.cols(); i++) {
    minX = (std::min)(minX, SplinePoints(0, i));
    maxX = (std::max)(maxX, SplinePoints(0, i));
    minY = (std::min)(minY, SplinePoints(1, i));
    maxY = (std::max)(maxY, SplinePoints(1, i));
  }
  params(SplinePoints.cols()) = 0.5*(maxX - minX);
  params(SplinePoints.cols() + 1) = 0.5*(maxY - minY);
  params(SplinePoints.cols() + 2) = 0.5*(maxX + minX);
  params(SplinePoints.cols() + 3) = 0.5*(maxY + minY);
  params(SplinePoints.cols() + 4) = 0;
  for (int i = 0; i<SplinePoints.cols(); i++) {
    params(i) = double(i)*incr;
  }

  std::cout << "INITIALIZATION" << " ";
  std::cout << "a=" << params(SplinePoints.cols()) << "\t";
  std::cout << "b=" << params(SplinePoints.cols() + 1) << "\t";
  std::cout << "x0=" << params(SplinePoints.cols() + 2) << "\t";
  std::cout << "y0=" << params(SplinePoints.cols() + 3) << "\t";
  std::cout << "r=" << params(SplinePoints.cols() + 4)*180. / EIGEN_PI << "\t";
  std::cout << std::endl << std::endl;

  SplineFitting<double> functor(SplinePoints);
  Eigen::LevenbergMarquardt< SplineFitting<double> > lm(functor);
  Eigen::LevenbergMarquardtSpace::Status info;

  info = lm.minimizeInit(lm_params);
  if (info == Eigen::LevenbergMarquardtSpace::ImproperInputParameters) {
    std::cerr << "Improper Input Parameters" << std::endl;
    return;
  }

  do {
    info = lm.minimizeOneStep(lm_params);
  } while (info == Eigen::LevenbergMarquardtSpace::Running);

  std::cout << "END" << " ";
  std::cout << "a=" << params(SplinePoints.cols()) << "\t";
  std::cout << "b=" << params(SplinePoints.cols() + 1) << "\t";
  std::cout << "x0=" << params(SplinePoints.cols() + 2) << "\t";
  std::cout << "y0=" << params(SplinePoints.cols() + 3) << "\t";
  std::cout << "r=" << params(SplinePoints.cols() + 4)*180. / EIGEN_PI << "\t";
  std::cout << std::endl << std::endl;

  // check parameters ambiguity before test result
  // a should be bigger than b
  if (fabs(params(SplinePoints.cols() + 1)) > fabs(params(SplinePoints.cols()))) {
    std::swap(params(SplinePoints.cols()), params(SplinePoints.cols() + 1));
    params(SplinePoints.cols() + 4) -= 0.5*EIGEN_PI;
  }
  // a and b should be positive
  if (params(SplinePoints.cols())<0) {
    params(SplinePoints.cols()) *= -1.;
    params(SplinePoints.cols() + 1) *= -1.;
    params(SplinePoints.cols() + 4) += EIGEN_PI;
  }
  // fix rotation angle range
  while (params(SplinePoints.cols() + 4) < 0) params(SplinePoints.cols() + 4) += 2.*EIGEN_PI;
  while (params(SplinePoints.cols() + 4) > EIGEN_PI) params(SplinePoints.cols() + 4) -= EIGEN_PI;

  eigen_assert(fabs(a - params(SplinePoints.cols())) < 0.00001);
  eigen_assert(fabs(b - params(SplinePoints.cols() + 1)) < 0.00001);
  eigen_assert(fabs(x0 - params(SplinePoints.cols() + 2)) < 0.00001);
  eigen_assert(fabs(y0 - params(SplinePoints.cols() + 3)) < 0.00001);
  eigen_assert(fabs(r - params(SplinePoints.cols() + 4)) < 0.00001);

}

void test_spline_fitting()
{
  CALL_SUBTEST(testSplineFitting());
}


/**
 * @file cd_tools.cc
 * @brief Utility Functions for the Convection-Diffusion Problem
 * @author Philippe Peter
 * @date July 2021
 * @copyright Developed at SAM, ETH Zurich
 */

#include "cd_tools.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>

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

namespace ConvectionDiffusion {

double Diameter(const lf::mesh::Entity& entity) {
  const lf::geometry::Geometry* geo_p = entity.Geometry();
  Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

  switch (entity.RefEl()) {
    case lf::base::RefEl::kTria(): {
      // Diameter of a triangle corresponds to the longest edge
      Eigen::Vector2d e0 = corners.col(1) - corners.col(0);
      Eigen::Vector2d e1 = corners.col(2) - corners.col(1);
      Eigen::Vector2d e2 = corners.col(0) - corners.col(1);
      return std::max(e0.norm(), std::max(e1.norm(), e2.norm()));
    }
    case lf::base::RefEl::kQuad(): {
      // Diameter of a (convex) quadrilateral corresponds to the longer diagonal
      Eigen::Vector2d d0 = corners.col(2) - corners.col(0);
      Eigen::Vector2d d1 = corners.col(3) - corners.col(1);
      return std::max(d0.norm(), d1.norm());
    }
    default: {
      LF_ASSERT_MSG(false,
                    "Diameter not available for " << entity.RefEl().ToString());
    }
  }
}  // namespace ConvectionDiffusion

double MeshWidth(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  double h = 0.0;
  for (const lf::mesh::Entity* entity_p : mesh_p->Entities(0)) {
    h = std::max(h, Diameter(*entity_p));
  }
  return h;
}

}  // namespace ConvectionDiffusion


/**
 * @file    Types.hpp
 *
 * @author  btran
 *
 */

#pragma once

#include <memory>
#include <string>

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

namespace perception
{
class CameraInfo
{
 public:
    using Ptr = std::shared_ptr<CameraInfo>;

    explicit CameraInfo(const std::string cameraInfoPath);
    ~CameraInfo();

    const Eigen::Matrix3d& K() const;

 private:
    Eigen::Matrix3d m_K;
};

using TransformInfo = Eigen::Matrix<double, 6, 1>;  // x, y, z, r, p, y
}  // namespace perception


#ifndef CANNON_ML_RLS_H
#define CANNON_ML_RLS_H

/*!
 * \file cannon/ml/rls.hpp
 * \brief File containing RLSFilter class definition.
 */

#include <utility>

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Class representing a Recursive Least Squares Filter supporting
     * intercept estimation and forgetting. See
     * http://cannontwo.com/assets/rls_notes.pdf.
     */
    class RLSFilter {
      public:

        RLSFilter() = delete;

        /*!
         * \brief Constructor taking input dimension, output dimension, inverse
         * covariance matrix initialization alpha, and forgetting factor.
         */
        RLSFilter(unsigned int in_dim, unsigned int out_dim,
                  double alpha = 1.0e6, double forgetting = 1.0)
            : in_dim_(in_dim), out_dim_(out_dim), param_dim_(in_dim),
              alpha_(alpha), forgetting_(forgetting),
              intercept_(VectorXd::Zero(out_dim_)),
              theta_(MatrixXd::Zero(param_dim_, out_dim_)),
              corrected_theta_(MatrixXd::Zero(param_dim_, out_dim_)),
              feat_mean_(RowVectorXd::Zero(param_dim_)),
              output_mean_(RowVectorXd::Zero(out_dim_)),
              covar_(MatrixXd::Identity(param_dim_, param_dim_) * alpha_),
              pred_error_covar_(MatrixXd::Zero(out_dim_, out_dim_)) {}

        /*!
         * \brief Get number of data points contributing to this filter.
         *
         * \returns Number of data points used to fit this filter.
         */
        unsigned int get_num_data() const;

        /*!
         * \brief Process a single datum and update this filter.
         *
         * \param in_vec Input features of the datum to process.
         * \param output Output features of the datum to process.
         */
        void process_datum(const VectorXd& in_vec, const VectorXd& output);

        /*!
         * \brief Get the identified linear parameters and offset vector fit by
         * this filter.
         *
         * \returns A pair containing the linear parameters and offset vector.
         */
        std::pair<MatrixXd, VectorXd> get_identified_mats() const;

        /*!
         * \brief Get prediction error covariance matrix for this filter.
         *
         * \returns Prediction error covariance matrix.
         */
        MatrixXd get_pred_error_covar() const;

        /*!
         * \brief Predict the value of the linear approximation fit by this
         * filter for the input point.
         *
         * \param in_vec Input features of point to predict for.
         *
         * \returns Prediction fit by this filter.
         */
        VectorXd predict(const VectorXd& in_vec) const;

        /*!
         * \brief Set the parameters of this RLS filter. Input and output mean
         * are recovered from the intercept.
         *
         * \param theta Parameter matrix to set.
         * \param intercept Intercept to set.
         * \param in_mean Input mean.
         */
        void set_params(const Ref<const MatrixXd> &theta,
                        const Ref<const VectorXd> &intercept,
                        const Ref<const VectorXd> &in_mean);

        /*!
         * \brief Reset this filter.
         */
        void reset();

      private:

        /*!
         * \brief Make internal feature vector for the input features.
         *
         * \param in_vec Input features to transform into internal feature space.
         *
         * \returns Internal features.
         */
        RowVectorXd make_feature_vec_(const VectorXd& in_vec) const;

        /*!
         * \brief Make U matrix for inverse covariance matrix update.
         *
         * \param feat Internal feature vector for update.
         *
         * \returns U matrix.
         */
        MatrixX2d make_u_(const RowVectorXd& feat) const;

        /*!
         * \brief Make V matrix for inverse covariance matrix update.
         *
         * \param feat Internal feature vector for update.
         *
         * \returns V matrix.
         */
        Matrix2Xd make_v_(const RowVectorXd& feat) const;

        /*!
         * \brief Make C matrix for inverse covariance matrix update.
         *
         * \returns C matrix.
         */
        Matrix2d make_c_() const;

        /*!
         * \brief Update inverse covariance matrix for this filter.
         *
         * \param u U matrix
         * \param c C matrix
         * \param v V matrix
         */
        void update_covar_(const MatrixX2d& u, const Matrix2d& c,
            const Matrix2Xd& v);

        /*!
         * \brief Update linear approximation stored by this filter.
         *
         * \param c_t C matrix transpose.
         * \param feat Internal feature vector for new datum.
         * \param output Output for new datum.
         */
        void update_theta_(const MatrixXd& c_t, const RowVectorXd& feat,
            const VectorXd& output);

        /*!
         * \brief Update output mean for this filter.
         *
         * \param output Observed output.
         */
        void update_output_mean_(const VectorXd& output);

        /*!
         * \brief Update internal feature mean for this filter.
         *
         * \param feat Observed internal features.
         */
        void update_feat_mean_(const RowVectorXd& feat);

        /*!
         * \brief Update prediction error covariance matrix in light of an
         * observed datum.
         *
         * \param in_vec Input feature vector.
         * \param output Observed output.
         */
        void update_pred_error_covar_(const VectorXd& in_vec, const VectorXd& output);

        // Parameters
        unsigned int in_dim_; //!< Dimension of input to this filter.
        unsigned int out_dim_; //!< Dimension of output for this filter.
        unsigned int param_dim_; //!< Number of linear parameters of this filter.
        double alpha_; //!< Inverse covariance matrix initialization parameter.
        double forgetting_; //!< Forgetting factor.
        double t_ = 0.0; //!< Number of observed data points.

        // Matrices
        VectorXd intercept_; //!< Intercept vector of filter
        MatrixXd theta_; //!< Linear parameters of filter
        MatrixXd corrected_theta_; //!< Linear parameters corrected for intercept
        RowVectorXd feat_mean_; //!< Mean of internal features for this filter
        RowVectorXd output_mean_; //!< Mean of observed outputs for this filter
        MatrixXd covar_; //!< Internal inverse covariance matrix.
        MatrixXd pred_error_covar_; //!< Prediction error covariance matrix for this filter.

    };

  } // namespace ml
} // namespace cannon

#endif /* ifndef CANNON_ML_RLS_H */


/* Basic example showing the usage of QuantLibAdjoint
*/

#include <ql/indexes/ibor/euribor.hpp>
#include <ql/instruments/makevanillaswap.hpp>
#include <ql/termstructures/yield/flatforward.hpp>

#include <boost/make_shared.hpp>

#include <iostream>

using namespace QuantLib;
using std::vector;
using std::cout;
using std::endl;
using std::ios;

int main(int, char* []) {

	Date referenceDate(3, Aug, 2016);
	Settings::instance().evaluationDate() = referenceDate;
	Actual365Fixed dayCounter;

	// Example 1

	// These will be the X (independent) and Y (dependent) vectors
	vector<Rate> zeroRate(1, 0.02);
	vector<Rate> swapNpv(1, 0.0);

	// Start taping with zeroRate as independent variable and set up flat zero curve
	cl::Independent(zeroRate);
	RelinkableHandle<YieldTermStructure> flatCurve(boost::make_shared<FlatForward>(referenceDate, zeroRate[0], dayCounter));
	flatCurve->enableExtrapolation();

	// Create and price swap
	Period swapTenor(5, Years);
	boost::shared_ptr<IborIndex> iborIndex = boost::make_shared<Euribor6M>(flatCurve);
	Rate fixedRate = 0.03;
	Period forwardStart(0, Days);
	boost::shared_ptr<VanillaSwap> swap = MakeVanillaSwap(swapTenor, iborIndex, fixedRate, forwardStart) .withNominal(100);
	swapNpv[0] = swap->NPV();

	// Stop taping and transfer operation sequence to function f (ultimately an AD function object)
	cl::tape_function<double> f(zeroRate, swapNpv);

	// Calculate d(swapNpv) / d(zero) with forward and reverse mode
	vector<double> dZ(1, 1.0);
	double forwardDeriv = f.Forward(1, dZ)[0];
	double reverseDeriv = f.Reverse(1, dZ)[0];

	// Calculate analytically the derivative
	Real derivative = 0.0;
	const Leg& fixedLeg = swap->fixedLeg();
	for (const auto& cf : fixedLeg) {
		Real amount = cf->amount();
		Time time = dayCounter.yearFraction(referenceDate, cf->date());
		DiscountFactor discount = flatCurve->discount(time);
		derivative += amount * time * discount;
	}
	Time timeToStart = dayCounter.yearFraction(referenceDate, swap->startDate());
	Time timeToEnd = dayCounter.yearFraction(referenceDate, swap->maturityDate());
	derivative += 100*(timeToEnd * flatCurve->discount(timeToEnd) - timeToStart * flatCurve->discount(timeToStart));

	//Compare bumped value to Taylor approximation
	Real basis_point = 0.01;
	Real approx_sensitivity = derivative * basis_point;
	//new bumped yield curve
	//when yield curve changes, swap recalculates
	flatCurve.linkTo(boost::make_shared<FlatForward>(referenceDate, zeroRate[0] + basis_point, dayCounter));
	Real bumped_npv = swap->NPV();

	// Output the results
	cout.precision(9);
	cout.setf(ios::fixed, ios::floatfield);
	cout << "Forward derivative:  " << forwardDeriv << endl;
	cout << "Reverse derivative:  " << reverseDeriv << endl;
	cout << "Analytic derivative: " << derivative << endl;
	//approx should be close to bumped
	cout << "Approx_Sensitivity: " << approx_sensitivity << endl;
	cout << "Bumped Sensitivity: " << bumped_npv - swapNpv[0] << endl;

	return 0;
}


/**
 * @ file pointEvaluation.cc
 * @ brief NPDE homework PointEvaluationRhs code
 * @ author Christian Mitsch, Liaowang Huang (refactoring)
 * @ date 22/03/2019, 06/01/2020 (refactoring)
 * @ copyright Developed at ETH Zurich
 */

#include "pointevaluationrhs.h"

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SparseLU>
#include <cmath>
#include <utility>

#include "pointevaluationrhs_norms.h"

namespace PointEvaluationRhs {

/* SAM_LISTING_BEGIN_5 */
Eigen::Vector2d GlobalInverseTria(Eigen::Matrix<double, 2, 3> mycorners,
                                  Eigen::Vector2d x) {
  Eigen::Vector2d x_hat;

#if SOLUTION
  // The unique affine mapping of the unit triangle to a general triangle
  // is given by the formula $\cob{\Bx = \VA_K*\wh{\Bx} + \Bs}$,
  // Use the vertex coordinates to calculate matrix A and translation vector s
  Eigen::Matrix2d A(2, 2);
  A.col(0) = mycorners.col(1) - mycorners.col(0);
  A.col(1) = mycorners.col(2) - mycorners.col(0);
  // The inverse mapping: $\cob{\wh{\Bx} = \VA_K^{-1} * (\Bx - \Ba_0)}$
  x_hat = A.partialPivLu().solve(x - mycorners.col(0));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return x_hat;
}
/* SAM_LISTING_END_5 */

/** @brief Numerically stable solution of a quadratic equation in R
 * @param a,b,c coefficients of quadratic polynomias ax^2+bx+c
 * @return both zeros, NaN if complex
 */
/* SAM_LISTING_BEGIN_4 */
std::pair<double, double> solveQuadraticEquation(double a, double b, double c) {
  // Implement the cases which are solvable and return their solutions
#if SOLUTION
  if (a != 0.) {
    b /= a;
    c /= a;
    const double D = b * b - 4 * c;  // discriminant
    if (D >= 0) {                    // real solutions
      const double rtD = std::sqrt(D);
      // Real solutions, cancellation-free formulas !
      if (b < 0) {
        const double root = 0.5 * (-b + rtD);
        return {root, c / root};
      } else {  // b >= 0
        const double root = 0.5 * (-b - rtD);
        return {root, c / root};
      }
    }
  } else {
    if (b != 0.0) {
      return {-c / b, -c / b};
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  // Return NAN if there are no (real) roots
  return {NAN, NAN};
}

/* SAM_LISTING_END_4 */

/** @brief Computes the area of a triangle
 * @param a,b,c vertex coordinate vectors
 */
inline double triaArea(const Eigen::Vector2d a, const Eigen::Vector2d b,
                       const Eigen::Vector2d c) {
  double result = 0;
#if SOLUTION
  result = 0.5 * std::abs((b[0] - a[0]) * (c[1] - a[1]) -
                          (b[1] - a[1]) * (c[0] - a[0]));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}

Eigen::Vector2d GlobalInverseQuad(Eigen::Matrix<double, 2, 4> vert,
                                  Eigen::Vector2d x) {
  constexpr double kEPS = 1.0E-8;
  Eigen::Vector2d x_hat;

  // Implement and use the functions triaArea and solveQuadraticEquation
#if SOLUTION
  /* SAM_LISTING_BEGIN_1 */
  // I: Find sub-triangle with largest area and use its
  // middle vertex as new "vertex 0".
  unsigned int vt_zero_idx{0};
  double max_area{0.0};
  for (int l = 0; l < 4; ++l) {
    const double sub_tria_area =
        triaArea(vert.col(l), vert.col((l + 1) % 4), vert.col((l + 2) % 4));
    if (sub_tria_area > max_area) {
      vt_zero_idx = (l + 1) % 4;
      max_area = sub_tria_area;
    }
  }
  // vt_zero_idx stores the index of the vertex that will now
  // be regarded as "pivotal vertex 0"

  /* SAM_LISTING_END_1 */

  const Eigen::Vector2d corner0 = vert.col(vt_zero_idx);
  const Eigen::Vector2d corner1 = vert.col((vt_zero_idx + 1) % 4);
  const Eigen::Vector2d corner2 = vert.col((vt_zero_idx + 2) % 4);
  const Eigen::Vector2d corner3 = vert.col((vt_zero_idx + 3) % 4);

  /* SAM_LISTING_BEGIN_2 */
  // Use corners to calculate matrix A, the translation t
  // and the coefficient vector d
  Eigen::Matrix2d A(2, 2);
  A.col(0) = corner1 - corner0;
  A.col(1) = corner3 - corner0;
  Eigen::Vector2d t = corner0;
  Eigen::Vector2d d = corner2 + corner0 - corner1 - corner3;

  // Calculate the coefficients in the non-linear system of equations
  Eigen::Vector2d y = A.partialPivLu().solve(x - t);
  Eigen::Vector2d q = A.partialPivLu().solve(d);
  Eigen::Vector2d q_orth(q(1), -q(0));

  // Treat exceptional cases
  // If q vanishes
  const double ref_size = y.norm();
  if (q.norm() < kEPS * ref_size) {
    x_hat = y;
  } else if (std::abs(q(0)) < kEPS * ref_size) {
    x_hat(0) = y(0);
    x_hat(1) = y(1) / (1 + q(1) * x_hat(0));
  } else if (std::abs(q(1)) < kEPS * ref_size) {
    x_hat(1) = y(1);
    x_hat(0) = y(0) / (1 + q(0) * x_hat(1));
  } else {
    // Generic case; solve quadratic equation
    double a = q(0);
    double b = 1 + y.dot(q_orth);
    double c = -y(1);

    auto [x_op1, x_op2] = solveQuadraticEquation(a, b, c);
    // No solution
    if (std::isnan(x_op1) || std::isnan(x_op2)) {
      x_hat(0) = NAN;
      x_hat(1) = NAN;
    } else {
      // There is a solution
      // Find root in the unit interval -> x_op1
      if ((x_op1 < 0.) || (x_op1 > 1.)) {
        if ((x_op2 >= 0.) && (x_op2 <= 1.)) {
          x_op1 = x_op2;
        }
      }
      x_hat(1) = x_op1;
      x_hat(0) = (y.dot(q_orth) + q(0) * x_hat(1)) / q(1);
    }
  }
  /* SAM_LISTING_END_2 */
  /* SAM_LISTING_BEGIN_3 */
  // Reverse initial permutation
  switch (vt_zero_idx) {
    case 1: {
      x_hat = ((Eigen::Matrix2d() << 0., -1., 1., 0.)).finished() * x_hat +
              Eigen::Vector2d(1.0, 0.0);
      break;
    }
    case 2: {
      x_hat = -x_hat + Eigen::Vector2d(1.0, 1.0);
      break;
    }
    case 3: {
      x_hat = ((Eigen::Matrix2d() << 0., 1., -1., 0.)).finished() * x_hat +
              Eigen::Vector2d(0.0, 1.0);
      break;
    }
  }
    /* SAM_LISTING_END_3 */
#else
  //====================
  // Your code goes here
  //====================
#endif
  return x_hat;
}

std::pair<double, double> normsSolutionPointLoadDirichletBVP(
    const lf::assemble::DofHandler &dofh, Eigen::Vector2d source_point,
    Eigen::VectorXd &sol_vec) {
  std::pair<double, double> result(0, 0);
  const unsigned int N_dofs = dofh.NumDofs();
  sol_vec.resize(N_dofs);
  sol_vec.setZero();
#if SOLUTION
  /* SAM_LISTING_BEGIN_7 */
  // Assemble matrix A
  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
  lf::uscalfe::LinearFELaplaceElementMatrix loc_mat_laplace{};
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, loc_mat_laplace, A);
  // Build rhs vector using dedicated ENTITY_VECTOR_PROVIDER
  Eigen::VectorXd rhs(N_dofs);
  rhs.setZero();
  PointEvaluationRhs::DeltaLocalVectorAssembler myvec_pro(source_point);
  lf::assemble::AssembleVectorLocally(0, dofh, myvec_pro, rhs);

  // Enforce Dirichlet boundary conditions
  const double boundary_val = 0;  // zero Dirichlet boundary conditions
  auto bd_flags{lf::mesh::utils::flagEntitiesOnBoundary(dofh.Mesh(), 2)};
  auto my_selector = [&dofh, &bd_flags, &boundary_val](unsigned int dof_idx) {
    if (bd_flags(dofh.Entity(dof_idx))) {
      return (std::pair<bool, double>(true, boundary_val));
    } else {
      // interior node: the value we return here does not matter
      return (std::pair<bool, double>(false, 42.0));
    }
  };
  lf::assemble::FixFlaggedSolutionComponents<double>(my_selector, A, rhs);

  // Solve linear system of equations A*x = rhs
  const Eigen::SparseMatrix<double> A_crs(A.makeSparse());
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  if (solver.info() == Eigen::Success) {
    sol_vec = solver.solve(rhs);
  } else {
    LF_ASSERT_MSG(false, "Eigen Factorization failed");
  }
  /* SAM_LISTING_END_7 */
  // return the norms of the solution vector
  result = std::pair<double, double>(
      PointEvaluationRhs::computeL2normLinearFE(dofh, sol_vec),
      PointEvaluationRhs::computeH1seminormLinearFE(dofh, sol_vec));
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}

/* SAM_LISTING_BEGIN_6 */
Eigen::VectorXd DeltaLocalVectorAssembler::Eval(const lf::mesh::Entity &cell) {
  Eigen::VectorXd result;
  // get the coordinates of the corners of this cell
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();
  auto vertices = lf::geometry::Corners(*geo_ptr);
#if SOLUTION
  Eigen::Vector2d x_hat;
  // the margin we allow when we determine wether a point is inside an
  // element
  const double margin = 1e-10;
  if (lf::base::RefEl::kTria() == cell.RefEl()) {
    x_hat = PointEvaluationRhs::GlobalInverseTria(vertices, x_0);
    result.resize(3);
    result.setZero();
    if (x_hat(0) <= 1 + margin && x_hat(0) >= 0 - margin &&
        x_hat(1) <= 1 + margin && x_hat(1) >= 0 - margin &&
        x_hat(1) + x_hat(0) <= 1 + margin) {
      already_found = true;
      // Barycentric coordinates on reference triangle
      result[0] = 1.0 - x_hat(0) - x_hat(1);
      result[1] = x_hat(0);
      result[2] = x_hat(1);
    }
  } else if (lf::base::RefEl::kQuad() == cell.RefEl()) {
    x_hat = PointEvaluationRhs::GlobalInverseQuad(vertices, x_0);
    result.resize(4);
    result.setZero();
    if (x_hat(0) <= 1 + margin && x_hat(0) >= 0 - margin &&
        x_hat(1) <= 1 + margin && x_hat(1) >= 0 - margin) {
      already_found = true;
      // Local shape functions on unit square
      result[0] = (1.0 - x_hat(0)) * (1.0 - x_hat(1));
      result[1] = x_hat(0) * (1.0 - x_hat(1));
      result[2] = x_hat(0) * x_hat(1);
      result[3] = (1.0 - x_hat(0)) * x_hat(1);
    }
  } else {
    LF_ASSERT_MSG(
        false, "Function only defined for triangular or quadrilateral cells");
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return result;
}
/* SAM_LISTING_END_6 */

}  // namespace PointEvaluationRhs


// HardCoded.cpp
//
// C++ code to price an option, essential algorithms.
//
// We take CEV model with a choice of the elaticity parameter
// and the Euler method. We give option price and number of times
// S hits the origin.
//
// (C) Datasim Education BC 2008-2011
//

#ifndef _SCL_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#endif

#ifndef _CRT_SECURE_NO_WARNINGS
#define _CRT_SECURE_NO_WARNINGS
#endif

#include "OptionData.hpp"
#include "StandardDeviation_StandardError.hpp"
#include "UtilitiesDJD/RNG/NormalGenerator.hpp"
#include "UtilitiesDJD/Geometry/Range.cpp"
#include <boost/tuple/tuple.hpp> // include tuple class
#include <boost/tuple/tuple_io.hpp> // include I/O for tuple
#include <vector>
#include <cmath>
#include <iostream>

template <class T> void print(const std::vector<T>& myList)
{  // A generic print function for vectors

	std::cout << std::endl << "Size of vector is " << l.size() << "\n[";

	// We must use a const iterator here, otherwise we get a compiler error.
	std::vector<T>::const_iterator i;
	for (i = myList.begin(); i != myList.end(); ++i)
	{
			std::cout << *i << ",";

	}

	std::cout << "]\n";
}

namespace SDEDefinition
{ // Defines drift + diffusion + data

	OptionData* data;				// The data for the option MC

	double drift(double t, double X)
	{ // Drift term

		return (data->r)*X; // r - D
	}

	double diffusion(double t, double X)
	{ // Diffusion term

		double betaCEV = 1.0;
		return data->sig * pow(X, betaCEV);

	}

	double diffusionDerivative(double t, double X)
	{ // Diffusion term, needed for the Milstein method

		double betaCEV = 1.0;
		return 0.5 * (data->sig) * (betaCEV) * pow(X, 2.0 * betaCEV - 1.0);
	}
} // End of namespace

int main()
{
	std::cout <<  "1 factor MC with explicit Euler\n";

	/*==========================MODIFIED================================*/
	vector<double> price_vec; // create a vector to store prices for each simulation
	/*================================================================*/

	// Batch 1 data
	OptionData myOption;
	myOption.K = 65.0;
	myOption.T = 0.25;
	myOption.r = 0.08;
	myOption.sig = 0.3;
	myOption.type = -1;	// Put -1, Call +1
	double S_0 = 60.0;

	//// Batch 2 data
	//OptionData myOption;
	//myOption.K = 100.0;
	//myOption.T = 1.0;
	//myOption.r = 0.0;
	//myOption.sig = 0.2;
	//myOption.type = -1;	// Put -1, Call +1
	//double S_0 = 100.0;

	//// Batch 4 data
	//OptionData myOption;
	//myOption.K = 100.0;
	//myOption.T = 30.0;
	//myOption.r = 0.08;
	//myOption.sig = 0.30;
	//myOption.type = 1;	// Put -1, Call +1
	//double S_0 = 100.0;

	long N = 100;
	std::cout << "Number of subintervals in time: ";
	std::cin >> N;

	// Create the basic SDE (Context class)
	Range<double> range (0.0, myOption.T);
	double VOld = S_0;
	double VNew;

	std::vector<double> x = range.mesh(N);

	// V2 mediator stuff
	long NSim = 50000;
	std::cout << "Number of simulations: ";
	std::cin >> NSim;

	double k = myOption.T / double (N);
	double sqrk = sqrt(k);

	// Normal random number
	double dW;
	double price = 0.0;	// Option price

	// NormalGenerator is a base class
	NormalGenerator* myNormal = new BoostNormal();

	using namespace SDEDefinition;
	SDEDefinition::data = &myOption;

	std::vector<double> res;
	int coun = 0; // Number of times S hits origin

	// A.
	for (long i = 1; i <= NSim; ++i)
	{ // Calculate a path at each iteration

		if ((i/10000) * 10000 == i)
		{// Give status after each 10000th iteration

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

		VOld = S_0;
		for (unsigned long index = 1; index < x.size(); ++index)
		{

			// Create a random number
			dW = myNormal->getNormal();

			// The FDM (in this case explicit Euler)
			VNew = VOld  + (k * drift(x[index-1], VOld))
						+ (sqrk * diffusion(x[index-1], VOld) * dW);

			VOld = VNew;

			// Spurious values
			if (VNew <= 0.0) coun++;
		}

		double tmp = myOption.myPayOffFunction(VNew);
		/*==========================MODIFIED================================*/
		price_vec.push_back(tmp); // add each simulation price to the price vector
	    /*================================================================*/
		price += (tmp)/double(NSim);
	}

	// D. Finally, discounting the average price
	price *= exp(-myOption.r * myOption.T);

	// Cleanup; V2 use scoped pointer
	delete myNormal;

	std::cout << "Price, after discounting: " << price << ", " << std::endl;
	std::cout << "Number of times origin is hit: " << coun << endl;

	/*==========================MODIFIED================================*/
	// show the standard deviation and standard error
	boost::tuple<double, double> SD_SE = get_SD_SE(price_vec, myOption.r, myOption.T);

	cout << "SD = " << get<0>(SD_SE) << ", SE = " << get<1>(SD_SE) << endl;
	/*================================================================*/
	system("Pause");

	return 0;
}

Part D Monte Carlo Pricing Methods/Exericse D Monte Carlo Pricing Methods/TestMC.cpp


#include <geometrycentral/surface/direction_fields.h>

#include "geometrycentral/numerical/linear_solvers.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>

using std::cout;
using std::endl;

namespace geometrycentral {
namespace surface {

namespace {

SparseMatrix<std::complex<double>> computeVertexConnectionLaplacian(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireVertexIndices();
  geometry.requireEdgeCotanWeights();
  geometry.requireTransportVectorsAlongHalfedge();

  int numInvalid = 0;

  std::vector<Eigen::Triplet<std::complex<double>>> triplets;
  for (Halfedge he : mesh.halfedges()) {

    size_t iTail = geometry.vertexIndices[he.vertex()];
    size_t iTip = geometry.vertexIndices[he.next().vertex()];

    // Levi-Civita connection between vertices
    Vector2 rot = geometry.transportVectorsAlongHalfedge[he.twin()].pow(nSym);
    double weight = geometry.edgeCotanWeights[he.edge()];
    if (!rot.isFinite() || !isfinite(weight)) {
#ifndef GC_NLINALG_DEBUG
      //std::cout << "computeVertexConnectionLaplacian: Oopsie at " << iTip << " x " << iTail << std::endl;
#endif
      weight = 0;
      rot = Vector2::zero();
      numInvalid++;
    }

    triplets.emplace_back(iTail, iTail, weight);
    triplets.emplace_back(iTail, iTip, -weight * rot);
  }
  if (numInvalid > 0) {
    std::cout << "computeVertexConnectionLaplacian: " << numInvalid << " invalid entried." << std::endl;
  }

  // assemble matrix from triplets
  Eigen::SparseMatrix<std::complex<double>> vertexConnectionLaplacian(mesh.nVertices(), mesh.nVertices());
  vertexConnectionLaplacian.setFromTriplets(triplets.begin(), triplets.end());

  // Shift to avoid singularity
  Eigen::SparseMatrix<std::complex<double>> eye(mesh.nVertices(), mesh.nVertices());
  eye.setIdentity();
  vertexConnectionLaplacian += 1e-9 * eye;

  return vertexConnectionLaplacian;
}

SparseMatrix<std::complex<double>> computeFaceConnectionLaplacian(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireFaceIndices();
  geometry.requireTransportVectorsAcrossHalfedge();

  std::vector<Eigen::Triplet<std::complex<double>>> triplets;
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];

    std::complex<double> weightDiagSum = 0;
    for (Halfedge he : f.adjacentHalfedges()) {
      if (he.twin().isInterior()) {

        Face neighFace = he.twin().face();
        size_t j = geometry.faceIndices[neighFace];

        // Levi-Civita connection between the faces
        Vector2 rot = geometry.transportVectorsAcrossHalfedge[he.twin()].pow(nSym);

        double weight = 1;
        triplets.emplace_back(i, j, -weight * rot);

        weightDiagSum += weight;
      }
    }

    triplets.emplace_back(i, i, weightDiagSum + 1e-9); // Shift to avoid singularity
  }
  // assemble matrix from triplets
  Eigen::SparseMatrix<std::complex<double>> faceConnectionLaplacian(mesh.nFaces(), mesh.nFaces());
  faceConnectionLaplacian.setFromTriplets(triplets.begin(), triplets.end());

  return faceConnectionLaplacian;
}

} // namespace

VertexData<Vector2> computeSmoothestVertexDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireVertexIndices();
  geometry.requireVertexGalerkinMassMatrix();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  // Find the smallest eigenvector
  Vector<std::complex<double>> solution = smallestEigenvectorSquare(energyMatrix, massMatrix);

  // Copy the result to a VertexData vector
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
    toReturn[v] = unit(toReturn[v]);
  }

  return toReturn;
}

FaceData<Vector2> computeSmoothestFaceDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireFaceIndices();
  geometry.requireFaceGalerkinMassMatrix();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  // Find the smallest eigenvector
  Vector<std::complex<double>> solution = smallestEigenvectorSquare(energyMatrix, massMatrix);

  // Copy the result to a FaceData vector
  FaceData<Vector2> toReturn(mesh);
  for (Face f : mesh.faces()) {
    toReturn[f] = Vector2::fromComplex(solution(geometry.faceIndices[f]));
    toReturn[f] = unit(toReturn[f]);
  }

  return toReturn;
}

VertexData<Vector2> computeSmoothestBoundaryAlignedVertexDirectionField(IntrinsicGeometryInterface& geometry,
                                                                        int nSym) {
  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireHalfedgeVectorsInVertex();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {

      // Find incoming and outgoing boundary vectors as tangent
      Halfedge heBoundaryA = v.halfedge();
      Halfedge heBoundaryB = heBoundaryA.twin().next();

      Vector2 vecA = geometry.halfedgeVectorsInVertex[heBoundaryA];
      Vector2 vecB = geometry.halfedgeVectorsInVertex[heBoundaryB];

      Vector2 tangentV = unit(-vecA + vecB);
      Vector2 normalV = tangentV.rotate90();

      boundaryValues[v] = normalV.pow(nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }

  // Assemble right-hand side from these boundary values
  Vector<std::complex<double>> b(mesh.nVertices());
  b.setZero();

  for (Vertex v : mesh.vertices()) {
    if (!v.isBoundary()) {

      for (Halfedge he : v.incomingHalfedges()) {
        Face neighFace = he.twin().face();
        if (he.vertex().isBoundary()) {

          size_t i = geometry.vertexIndices[v];
          size_t j = geometry.vertexIndices[he.vertex()];

          // move boundary terms to the right-hand side and remove the corresponding matrix entries
          std::complex<double> Aij = energyMatrix.coeff(i, j);
          energyMatrix.coeffRef(i, j) = 0;
          std::complex<double> bVal = boundaryValues[he.vertex()];
          b(i) += -Aij * bVal;
        }
      }
    }
  }

  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
  Eigen::VectorXcd RHS = massMatrix * b;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a VertexData vector for both the boundary and interior
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = Vector2::fromComplex(boundaryValues[v]);
    } else {
      toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
      toReturn[v] = unit(toReturn[v]);
    }
  }

  return toReturn;
}

FaceData<Vector2> computeSmoothestBoundaryAlignedFaceDirectionField(IntrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireFaceGalerkinMassMatrix();
  geometry.requireHalfedgeVectorsInFace();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  // Compute boundary values
  FaceData<bool> isInterior(mesh);
  FaceData<std::complex<double>> boundaryValues(mesh);
  for (Face f : mesh.faces()) {
    bool isBoundary = false;
    for (Edge e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;

    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector2 bC = Vector2::zero();
      for (Halfedge he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bC -= geometry.halfedgeVectorsInFace[he].rotate90(); // negate the vector to point outwards
        }
      }
      bC = unit(bC);
      boundaryValues[f] = bC.pow(nSym);
    }
  }

  // Assemble right-hand side from these boundary values
  Vector<std::complex<double>> b(mesh.nFaces());
  b.setZero();

  for (Face f : mesh.faces()) {
    if (isInterior[f]) {

      for (Halfedge he : f.adjacentHalfedges()) {
        Face neighFace = he.twin().face();
        if (!isInterior[neighFace]) {

          size_t i = geometry.faceIndices[f];
          size_t j = geometry.faceIndices[neighFace];

          // move boundary terms to the right-hand side and remove the corresponding matrix entries
          std::complex<double> Aij = energyMatrix.coeff(i, j);
          energyMatrix.coeffRef(i, j) = 0;
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += -Aij * bVal;
        }
      }
    }
  }

  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
  Eigen::VectorXcd RHS = massMatrix * b;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a FaceData object
  FaceData<Vector2> field(mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      field[f] = Vector2::fromComplex(solution(geometry.faceIndices[f]));
      field[f] = unit(field[f]);
    } else {
      field[f] = Vector2::fromComplex(boundaryValues[f]);
    }
  }

  return field;
}

VertexData<Vector2> computeCurvatureAlignedVertexDirectionField(ExtrinsicGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;
  size_t N = mesh.nVertices();

  geometry.requireVertexIndices();
  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireVertexPrincipalCurvatureDirections();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeVertexConnectionLaplacian(geometry, nSym);

  Vector<std::complex<double>> dirVec(N);
  if (nSym == 2) {
    for (Vertex v : mesh.vertices()) {
      dirVec[geometry.vertexIndices[v]] = geometry.vertexPrincipalCurvatureDirections[v];
    }
  } else if (nSym == 4) {
    for (Vertex v : mesh.vertices()) {
      dirVec[geometry.vertexIndices[v]] =
          std::pow(std::complex<double>(geometry.vertexPrincipalCurvatureDirections[v]), 2);
    }
  } else {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or 4");
  }

  // Normalize the alignment field
  double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
  dirVec /= scale;

  // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16
  double lambdaT = 0;

  Eigen::VectorXcd RHS = massMatrix * dirVec;
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a VertexData vector
  VertexData<Vector2> toReturn(mesh);
  for (Vertex v : mesh.vertices()) {
    toReturn[v] = Vector2::fromComplex(solution(geometry.vertexIndices[v]));
    toReturn[v] = unit(toReturn[v]);
  }

  return toReturn;
}

FaceData<Vector2> computeCurvatureAlignedFaceDirectionField(EmbeddedGeometryInterface& geometry, int nSym) {

  SurfaceMesh& mesh = geometry.mesh;
  const unsigned int N = mesh.nFaces();

  geometry.requireFaceIndices();
  geometry.requireFaceGalerkinMassMatrix();
  geometry.requireFacePrincipalCurvatureDirections();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.faceGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = computeFaceConnectionLaplacian(geometry, nSym);

  Eigen::VectorXcd dirVec(N);
  if (nSym == 2) {
    for (Face f : mesh.faces()) {
      dirVec[geometry.faceIndices[f]] = geometry.facePrincipalCurvatureDirections[f];
    }
  } else if (nSym == 4) {
    for (Face f : mesh.faces()) {
      dirVec[geometry.faceIndices[f]] = std::pow(std::complex<double>(geometry.facePrincipalCurvatureDirections[f]), 2);
    }
  } else {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or 4");
  }

  // Normalize the alignment field
  double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
  dirVec /= scale;

  double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

  Eigen::VectorXcd RHS = massMatrix * dirVec;
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
  Eigen::VectorXcd solution = solveSquare(LHS, RHS);

  // Copy the result to a FaceData object
  FaceData<Vector2> field(mesh);
  for (Face f : mesh.faces()) {
    field[f] = Vector2::fromComplex(solution[geometry.faceIndices[f]]);
    field[f] = unit(field[f]);
  }

  return field;
}

FaceData<int> computeFaceIndex(IntrinsicGeometryInterface& geometry, const VertexData<Vector2>& directionField,
                               int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireTransportVectorsAlongHalfedge();
  geometry.requireFaceGaussianCurvatures();

  // Store the result here
  FaceData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Face f : mesh.faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = geometry.faceGaussianCurvatures[f] * nSym;

    for (Halfedge he : f.adjacentHalfedges()) {
      if (he.twin().isInterior()) {
        // Compute the rotation along the halfedge implied by the field
        Vector2 x0 = directionField[he.vertex()];
        Vector2 x1 = directionField[he.twin().vertex()];
        Vector2 rot = geometry.transportVectorsAlongHalfedge[he].pow(nSym);

        // Find the difference in angle
        double theta0 = (rot * x0).arg();
        double theta1 = x1.arg();
        double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

        totalRot += deltaTheta; // accumulate
      }
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(IntrinsicGeometryInterface& geometry, const FaceData<Vector2>& directionField,
                                   int nSym) {

  SurfaceMesh& mesh = geometry.mesh;

  geometry.requireTransportVectorsAcrossHalfedge();
  geometry.requireVertexGaussianCurvatures();

  // Store the result here
  VertexData<int> indices(mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Vertex v : mesh.vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = geometry.vertexGaussianCurvatures[v] * nSym;

    if (!v.isBoundary()) {
      for (Halfedge he : v.incomingHalfedges()) {
        // Compute the rotation along the halfedge implied by the field
        Vector2 x0 = directionField[he.face()];
        Vector2 x1 = directionField[he.twin().face()];
        Vector2 rot = geometry.transportVectorsAcrossHalfedge[he].pow(nSym);

        double deltaTheta = regularizeAngle(x1.arg() - (rot * x0).arg() + PI) - PI; // regularize to [-PI,PI]

        totalRot += deltaTheta;
      }
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}
/*
VertexData<Vector2> computeSmoothestBoundaryAlignedVertexDirectionField(IntrinsicGeometryInterface& geometry,
                                                                        int nSym) {
  SurfaceMesh& mesh = geometry.mesh;

  if (!mesh.hasBoundary()) {
    throw std::logic_error("tried to compute smoothest boundary aligned direction field on a mesh without boundary");
  }

  geometry.requireVertexGalerkinMassMatrix();
  geometry.requireVertexConnectionLaplacian();
  geometry.requireHalfedgeVectorsInVertex();

  // Mass matrix
  SparseMatrix<std::complex<double>> massMatrix = geometry.vertexGalerkinMassMatrix.cast<std::complex<double>>();

  // Energy matrix
  SparseMatrix<std::complex<double>> energyMatrix = geometry.vertexConnectionLaplacian;

  // Compute the boundary values
  VertexData<std::complex<double>> boundaryValues(mesh);
  VertexData<char> isBoundary(mesh, false);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      isBoundary[v] = true;

      // Find incoming and outgoing boundary vectors as tangent
      Halfedge heBoundaryA = v.halfedge();
      Halfedge heBoundaryB = heBoundaryA.twin().next();

      Vector2 vecA = geometry.halfedgeVectorsInVertex[heBoundaryA];
      Vector2 vecB = geometry.halfedgeVectorsInVertex[heBoundaryB];

      Vector2 tangentV = unit(-vecA + vecB);
      Vector2 normalV = tangentV.rotate90();

      boundaryValues[v] = normalV.pow(nSym);
    } else {
      boundaryValues[v] = 0;
    }
  }
  Vector<std::complex<double>> b = boundaryValues.toVector();
  Vector<char> isBoundaryVec = isBoundary.toVector();

  // Block decompose problem

  // Compute the actual solution
  std::cout << "Solving linear problem..." << std::endl;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    geometry.requirePrincipalDirections();

    Eigen::VectorXcd dirVec(nInterior);
    for (Vertex v : mesh.vertices()) {
      if (v.isBoundary()) {
        continue;
      }

      Vector2 directionVal = geometry.principalDirections[v];
      if (nSym == 4) {
        directionVal = std::pow(directionVal, 2);
      }

      // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
      // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals and
      // the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction field
      // optimization.
      dirVec[geometry.interiorVertexIndices[v]] = directionVal / std::abs(directionVal);
    }

    double t = 0.01; // this is something of a magical constant, see "Globally
                     // Optimal Direction Fields", eqn 9

    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);
  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a VertexData vector for both the boudary and interior
  VertexData<Vector2> toReturn(*mesh);
  for (Vertex v : mesh.vertices()) {
    if (v.isBoundary()) {
      toReturn[v] = boundaryValues[v];
    } else {
      toReturn[v] = unit(solution[geometry.interiorVertexIndices[v]]);
    }
  }

  return toReturn;
}

// Helpers for computing face-based direction fields
namespace {

FaceData<Vector2> computeSmoothestFaceDirectionField_noBoundary(IntrinsicGeometryInterface& geometry, int nSym = 1,
                                                                bool alignCurvature = false) {

  SurfaceMesh* mesh = geometry->getMesh();
  unsigned int N = mesh.nFaces();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();
  geometry.requireFaceNormals();
  geometry.requireFaceAreas();
  geometry.requireDihedralAngles();
  geometry.requireFaceIndices();

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(N, N);
  energyMatrix.reserve(Eigen::VectorXi::Constant(N, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(N, N);
  massMatrix.reserve(Eigen::VectorXi::Constant(N, 1));

  // === Build matrices

  // Build the mass matrix
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];
    massMatrix.insert(i, i) = geometry.faceAreas[f];
  }

  // Build the energy matrix
  for (Face f : mesh.faces()) {
    size_t i = geometry.faceIndices[f];

    std::complex<double> weightISum = 0;
    for (Halfedge he : f.adjacentHalfedges()) {

      if (!he.twin().isInterior()) {
        continue;
      }

      Face neighFace = he.twin().face();
      unsigned int j = geometry.faceIndices[neighFace];

      // LC connection between the faces
      Vector2 rBar = std::pow(geometry.faceTransportCoefs[he.twin()], nSym);

      double weight = 1; // FIXME TODO figure out weights
      energyMatrix.insert(i, j) = -weight * rBar;
      weightISum += weight;
    }

    energyMatrix.insert(i, i) = weightISum;
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Vector2> eye(N, N);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(N);
    for (Face f : mesh.faces()) {

      // Compute something like the principal directions
      double weightSum = 0;
      Vector2 sum = 0;

      for (Halfedge he : f.adjacentHalfedges()) {

        double dihedralAngle = std::abs(geometry.dihedralAngles[he.edge()]);
        double weight = norm(geometry->vector(he));
        weightSum += weight;
        double angleCoord = angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), geometry.faceNormals[f]);
        Vector2 coord = std::exp(angleCoord * IM_I *
                                 (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

        sum += coord * weight * dihedralAngle;
      }

      sum /= weightSum;

      dirVec[geometry.faceIndices[f]] = sum;
    }

    // Normalize the alignment field
    double scale = std::sqrt(std::abs((dirVec.adjoint() * massMatrix * dirVec)[0]));
    dirVec /= scale;

    double lambdaT = 0.0; // this is something of a magical constant, see "Globally Optimal Direction Fields", eqn 16

    // Eigen::VectorXcd RHS = massMatrix * dirVec;
    Eigen::VectorXcd RHS = dirVec;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix - lambdaT * massMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the smallest eigenvector
  else {
    std::cout << "Solving smoothest field eigenvalue problem..." << std::endl;
    solution = smallestEigenvectorPositiveDefinite(energyMatrix, massMatrix);
  }

  // Copy the result to a FaceData object
  FaceData<Vector2> field(*mesh);
  for (Face f : mesh.faces()) {
    field[f] = solution[geometry.faceIndices[f]] / std::abs(solution[geometry.faceIndices[f]]);
  }

  return field;
}

FaceData<Vector2> computeSmoothestFaceDirectionField_boundary(IntrinsicGeometryInterface& geometry, int nSym = 1,
                                                              bool alignCurvature = false) {

  SurfaceMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();
  geometry.requireFaceNormals();
  geometry.requireFaceAreas();
  geometry.requireDihedralAngles();

  // Index interior faces
  size_t nInteriorFace = 0;
  FaceData<size_t> interiorFaceInd(*mesh, -77);
  FaceData<char> isInterior(*mesh);
  for (Face f : mesh.faces()) {
    bool isBoundary = false;
    for (Edge e : f.adjacentEdges()) {
      isBoundary |= e.isBoundary();
    }
    isInterior[f] = !isBoundary;
    if (!isBoundary) {
      interiorFaceInd[f] = nInteriorFace++;
    }
  }

  // Compute boundary values
  FaceData<Vector2> boundaryValues(*mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      boundaryValues[f] = 0;
    } else {
      Vector3 bVec = Vector3::zero();
      for (Halfedge he : f.adjacentHalfedges()) {
        if (he.edge().isBoundary()) {
          bVec += geometry->vector(he).rotate_around(geometry.faceNormals[f], -PI / 2.0);
        }
      }
      Vector2 bC(dot(geometry.faceBases[f][0], bVec), dot(geometry.faceBases[f][1], bVec));
      bC = unit(bC);
      boundaryValues[f] = std::pow(bC, nSym);
    }
  }

  // === Allocate matrices
  // Energy matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> energyMatrix(nInteriorFace, nInteriorFace);
  energyMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 4));

  // Mass matrix
  Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> massMatrix(nInteriorFace, nInteriorFace);
  massMatrix.reserve(Eigen::VectorXi::Constant(nInteriorFace, 1));

  // RHS
  Eigen::VectorXcd b(nInteriorFace);

  // === Build matrices

  // Build the mass matrix
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];
      massMatrix.insert(i, i) = geometry.faceAreas[f];
    }
  }

  // Build the energy matrix
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      size_t i = interiorFaceInd[f];

      std::complex<double> weightISum = 0;
      for (Halfedge he : f.adjacentHalfedges()) {

        Face neighFace = he.twin().face();
        double weight = 1; // FIXME TODO figure out weights
        Vector2 rBar = std::pow(geometry.faceTransportCoefs[he.twin()], nSym);

        if (isInterior[neighFace]) {
          size_t j = interiorFaceInd[neighFace];
          energyMatrix.insert(i, j) = -weight * rBar;
        } else {
          std::complex<double> bVal = boundaryValues[neighFace];
          b(i) += weight * rBar * bVal;
        }

        weightISum += weight;
      }

      energyMatrix.insert(i, i) = weightISum;
    }
  }

  // Shift to avoid singularity
  Eigen::SparseMatrix<Vector2> eye(nInteriorFace, nInteriorFace);
  eye.setIdentity();
  energyMatrix += 1e-4 * eye;

  // Store the solution here
  Eigen::VectorXcd solution;

  // If requested, align to principal curvatures
  if (alignCurvature) {

    Eigen::VectorXcd dirVec(nInteriorFace);
    for (Face f : mesh.faces()) {
      if (isInterior[f]) {

        // Compute something like the principal directions
        double weightSum = 0;
        Vector2 sum = 0;

        for (Halfedge he : f.adjacentHalfedges()) {

          double dihedralAngle = std::abs(geometry.dihedralAngles[he.edge()]);
          double weight = norm(geometry->vector(he));
          weightSum += weight;
          double angleCoord =
              angleInPlane(geometry->vector(f.halfedge()), geometry->vector(he), geometry.faceNormals[f]);
          Vector2 coord = std::exp(angleCoord * IM_I *
                                   (double)nSym); // nsym should be 2 or 4, checked in the funciton which calls this

          sum += coord * weight * dihedralAngle;
        }

        sum /= weightSum;

        // Normalize the curvature vectors. By doing so, we lose the property of adjusting the strength of the alignment
        // based on the strength of the curvature, but resolve any scaling issues between the magnitude of the normals
        // and the magnitude of the desired field.  Be careful when interpreting this as opposed to the usual direction
        // field optimization.
        dirVec[interiorFaceInd[f]] = unit(sum);
      }
    }

    double t = 0.1; // this is something of a magical constant, see "Globally
                    // Optimal Direction Fields", eqn 9
                    // NOTE: This value is different from the one used for vertex fields; seems to work better?

    std::cout << "Solving smoothest field dirichlet problem with curvature term..." << std::endl;
    Eigen::VectorXcd RHS = massMatrix * (t * dirVec + b);
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    solution = solveSquare(LHS, RHS);

  }
  // Otherwise find the general closest solution
  else {
    std::cout << "Solving smoothest field dirichlet problem..." << std::endl;
    Eigen::SparseMatrix<std::complex<double>, Eigen::ColMajor> LHS = energyMatrix;
    Eigen::VectorXcd RHS = massMatrix * b;
    solution = solveSquare(LHS, RHS);
  }

  // Copy the result to a FaceData object
  FaceData<Vector2> field(*mesh);
  for (Face f : mesh.faces()) {
    if (isInterior[f]) {
      field[f] = unit(solution[interiorFaceInd[f]]);
    } else {
      field[f] = unit(boundaryValues[f]);
    }
  }

  return field;
}

} // namespace

FaceData<Vector2> computeSmoothestFaceDirectionField(Geometry<Euclidean>* geometry, int nSym, bool alignCurvature) {

  std::cout << "Computing globally optimal direction field in faces" << std::endl;

  if (alignCurvature && !(nSym == 2 || nSym == 4)) {
    throw std::logic_error("ERROR: It only makes sense to align with curvature when nSym = 2 or "
                           "4");
  }

  // Dispatch to either the boundary of no boundary variant depending on the mesh type
  bool hasBoundary = false;
  for (Vertex v : geometry->getMesh()->vertices()) {
    hasBoundary |= v.isBoundary();
  }

  if (hasBoundary) {
    std::cout << "Mesh has boundary, computing dirichlet boundary condition solution" << std::endl;
    return computeSmoothestFaceDirectionField_boundary(geometry, nSym, alignCurvature);
  } else {
    std::cout << "Mesh has no boundary, computing unit-norm solution" << std::endl;
    return computeSmoothestFaceDirectionField_noBoundary(geometry, nSym, alignCurvature);
  }
}

FaceData<int> computeFaceIndex(IntrinsicGeometryInterface& geometry, const VertexData<Vector2>& directionField,
                               int nSym) {

  SurfaceMesh* mesh = geometry->getMesh();

  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();

  // Store the result here
  FaceData<int> indices(*mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Face f : mesh.faces()) {
    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (Halfedge he : f.adjacentHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Vector2 x0 = directionField[he.vertex()];
      Vector2 x1 = directionField[he.twin().vertex()];
      Vector2 transport = std::pow(geometry.vertexTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = regularizeAngle(theta1 - theta0 + PI) - PI; // regularize to [-PI,PI]

      totalRot += deltaTheta; // accumulate
    }

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[f] = index;
  }

  return indices;
}

VertexData<int> computeVertexIndex(IntrinsicGeometryInterface& geometry, const FaceData<Vector2>& directionField,
                                   int nSym) {

  SurfaceMesh* mesh = geometry->getMesh();
  GeometryCache<Euclidean>& geometry = geometry->cache;
  geometry.requireFaceTransportCoefs();

  // Store the result here
  VertexData<int> indices(*mesh);

  // TODO haven't tested that this correctly reports the index when it is larger
  // than +-1

  for (Vertex v : mesh.vertices()) {

    // Trace the direction field around the face and see how many times it
    // spins!
    double totalRot = 0;

    for (Halfedge he : v.incomingHalfedges()) {
      // Compute the rotation along the halfedge implied by the field
      Vector2 x0 = directionField[he.face()];
      Vector2 x1 = directionField[he.twin().face()];
      Vector2 transport = std::pow(geometry.faceTransportCoefs[he], nSym);

      // Find the difference in angle
      double theta0 = std::arg(transport * x0);
      double theta1 = std::arg(x1);
      double deltaTheta = std::arg(x1 / (transport * x0));

      totalRot += deltaTheta;
    }

    double angleDefect = geometry->angleDefect(v);
    totalRot += angleDefect * nSym;

    // Compute the net rotation and corresponding index
    int index = static_cast<int>(std::round(totalRot / (2 * PI))); // should be very close to a multiple of 2PI
    indices[v] = index;
  }

  return indices;
}
*/

} // namespace surface
} // namespace geometrycentral