//
//  Scheduler_Tests.cpp
//
//
//  Created by Ben Lengerich on 1/27/16.
//
//

#include "gtest/gtest.h"
#include <Eigen/Dense>
#include <memory>
#include <stdio.h>
#include <unordered_map>

#include "Scheduler/Scheduler.hpp"
#include "Scheduler/Job.hpp"
#include "Algorithms/AlgorithmOptions.hpp"
#include "Models/ModelOptions.hpp"

using namespace std;
using namespace Eigen;

class SchedulerTest : public testing::Test {
protected:
    virtual void SetUp() {
        alg_opts = AlgorithmOptions_t(
            proximal_gradient_descent,
            {{"tolerance", "0.01"}, {"learning_rate", "0.01"}});

        model_opts = ModelOptions_t(
            linear_regression,
            {{"lambda", "0.01"}, {"L2_lambda", "0.01"}});

        X = MatrixXf(10, 5);
        X << 0.8147,    0.1576,    0.6557,    0.7060,    0.4387,
        0.9058,    0.9706,    0.0357,    0.0318,    0.3816,
        0.1270,    0.9572,    0.8491,    0.2769,    0.7655,
        0.9134,    0.4854,    0.9340,    0.0462,    0.7952,
        0.6324,    0.8003,    0.6787,    0.0971,    0.1869,
        0.0975,    0.1419,    0.7577,    0.8235,    0.4898,
        0.2785,    0.4218,    0.7431,    0.6948,    0.4456,
        0.5469,    0.9157,    0.3922,    0.3171,    0.6463,
        0.9575,    0.7922,    0.6555,    0.9502,    0.7094,
        0.9649,    0.9595,    0.1712,    0.0344,    0.7547;
    	y = MatrixXf(10, 1);
        y << 0.4173,
        0.0497,
        0.9027,
        0.9448,
        0.4909,
        0.4893,
        0.3377,
        0.9001,
        0.3692,
        0.1112;

        LargeX = MatrixXf(n_patients, n_markers);
        for (int i = 0; i < n_patients; i++) {
            for (int j = 0; j < n_markers; j++) {
                LargeX(i,j) = rand();
            }
        }
    	LargeY = MatrixXf(n_patients, n_traits);
        for (int i = 0; i < n_patients; i++) {
            for (int j = 0; j < n_traits; j++) {
                LargeY(i,j) = rand();
            }
        }
    }

    virtual void TearDown() {}

    const int n_patients = 1000;
    const int n_markers = 1000;
    const int n_traits = 1;
    AlgorithmOptions_t alg_opts;
    MatrixXf X;
    MatrixXf y;
    MatrixXf LargeX;
    MatrixXf LargeY;
    ModelOptions_t model_opts;
};

TEST_F(SchedulerTest, Singleton) {
	ASSERT_EQ(&(Scheduler::Instance()), &(Scheduler::Instance()));
}

TEST_F(SchedulerTest, getNewAlgorithmId) {
    int alg_num1 = Scheduler::Instance().getNewAlgorithmId();
    EXPECT_GE(alg_num1, 0);
    int alg_num2 = Scheduler::Instance().getNewAlgorithmId();
    EXPECT_GT(alg_num2, alg_num1);

    // Since we aren't actually making any algorithms, we shouldn't run out of IDs.
    for (int i = 0; i < 1000; i++) {
        EXPECT_GE(Scheduler::Instance().getNewAlgorithmId(), 0);
    }
}

TEST_F(SchedulerTest, newAlgorithm) {
    int alg_num1 = Scheduler::Instance().newAlgorithm(alg_opts);
    EXPECT_GE(alg_num1, 0);
    EXPECT_TRUE(Scheduler::Instance().deleteAlgorithm(alg_num1));

    AlgorithmOptions_t alg_opts2 = AlgorithmOptions_t(brent_search,
        {{"tolerance", "0.01"}, {"learning_rate", "0.01"}});
    int alg_num2 = Scheduler::Instance().newAlgorithm(alg_opts2);
    EXPECT_GE(alg_num2, 0);
    EXPECT_TRUE(Scheduler::Instance().deleteAlgorithm(alg_num2));

    AlgorithmOptions_t alg_opts3 = AlgorithmOptions_t( grid_search,
        {{"tolerance", "0.01"}, {"learning_rate", "0.01"}});
    int alg_num3 = Scheduler::Instance().newAlgorithm(alg_opts3);
    EXPECT_GE(alg_num3, 0);
    EXPECT_TRUE(Scheduler::Instance().deleteAlgorithm(alg_num3));

    AlgorithmOptions_t alg_opts4 = AlgorithmOptions_t(iterative_update,
        {{"tolerance", "0.01"}, {"learning_rate", "0.01"}});
    int alg_num4 = Scheduler::Instance().newAlgorithm(alg_opts4);
    EXPECT_GE(alg_num4, 0);
    EXPECT_TRUE(Scheduler::Instance().deleteAlgorithm(alg_num4));

    AlgorithmOptions_t alg_opts5 = AlgorithmOptions_t(hypo_test,
        {{"tolerance", "0.01"}, {"learning_rate", "0.01"}});
    int alg_num5 = Scheduler::Instance().newAlgorithm(alg_opts5);
    EXPECT_GE(alg_num5, 0);
    EXPECT_TRUE(Scheduler::Instance().deleteAlgorithm(alg_num5));
}

TEST_F(SchedulerTest, getNewModelId) {
    int model_num1 = Scheduler::Instance().getNewModelId();
    EXPECT_GE(model_num1, 0);
    int model_num2 = Scheduler::Instance().getNewModelId();
    EXPECT_GT(model_num2, model_num1);

    // Since we aren't actually making any models, we shouldn't run out of IDs.
    for (int i = 0; i < 1000; i++) {
        EXPECT_GE(Scheduler::Instance().getNewModelId(), 0);
    }
}

TEST_F(SchedulerTest, newModel) {
    int model_num1 = Scheduler::Instance().newModel(model_opts);
    EXPECT_GE(model_num1, 0);
}

TEST_F(SchedulerTest, SetX) {
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    Eigen::MatrixXf m(2,3);
    m << 1, 2,
         3, 4,
         5, 6;
    EXPECT_EQ(true, Scheduler::Instance().setX(job_id, m));
}

TEST_F(SchedulerTest, SetY) {
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    Eigen::MatrixXf m(2,3);
    m << 1, 2,
         3, 4,
         5, 6;
    EXPECT_EQ(true, Scheduler::Instance().setY(job_id, m));
}

TEST_F(SchedulerTest, getNewJobId) {
    job_id_t job_id1 = Scheduler::Instance().getNewJobId();
    EXPECT_GT(job_id1, 0);
    job_id_t job_id2 = Scheduler::Instance().getNewJobId();
    EXPECT_GT(job_id2, job_id1);

    for (int i = 0; i < 1000; i++) {
        EXPECT_GT(Scheduler::Instance().getNewJobId(), 0);
    }
}

TEST_F(SchedulerTest, newJob) {
    job_id_t job_id1 = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_GT(job_id1, 0);
}

TEST_F(SchedulerTest, ValidAlgorithmId) {
    ASSERT_FALSE(Scheduler::Instance().ValidAlgorithmId(-1));
    EXPECT_TRUE(Scheduler::Instance().ValidAlgorithmId(Scheduler::Instance().getNewAlgorithmId()));
    const algorithm_id_t alg_num = Scheduler::Instance().newAlgorithm(alg_opts);
    ASSERT_TRUE(Scheduler::Instance().ValidAlgorithmId(alg_num));
}

TEST_F(SchedulerTest, ValidModelId) {
    ASSERT_FALSE(Scheduler::Instance().ValidModelId(-1));
    EXPECT_TRUE(Scheduler::Instance().ValidModelId(Scheduler::Instance().getNewModelId()));
    const model_id_t model_num1 = Scheduler::Instance().newModel(model_opts);
    ASSERT_TRUE(Scheduler::Instance().ValidModelId(model_num1));
}

TEST_F(SchedulerTest, ValidJobId) {
    ASSERT_FALSE(Scheduler::Instance().ValidJobId(-1));
    EXPECT_TRUE(Scheduler::Instance().ValidJobId(Scheduler::Instance().getNewJobId()));
    const job_id_t job_id1 = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().ValidJobId(job_id1));
    Scheduler::Instance().deleteJob(job_id1);
    ASSERT_TRUE(Scheduler::Instance().ValidJobId(job_id1));
}

TEST_F(SchedulerTest, AlgorithmIdUsed) {
    ASSERT_FALSE(Scheduler::Instance().AlgorithmIdUsed(-1));
    EXPECT_FALSE(Scheduler::Instance().AlgorithmIdUsed(Scheduler::Instance().getNewAlgorithmId()));
    const algorithm_id_t alg_num = Scheduler::Instance().newAlgorithm(alg_opts);
    ASSERT_TRUE(Scheduler::Instance().AlgorithmIdUsed(alg_num));
}

TEST_F(SchedulerTest, ModelIdUsed) {
    ASSERT_FALSE(Scheduler::Instance().ModelIdUsed(-1));
    EXPECT_FALSE(Scheduler::Instance().ModelIdUsed(Scheduler::Instance().getNewModelId()));
    const model_id_t model_num1 = Scheduler::Instance().newModel(model_opts);
    ASSERT_TRUE(Scheduler::Instance().ModelIdUsed(model_num1));
}

TEST_F(SchedulerTest, JobIdUsed) {
    ASSERT_FALSE(Scheduler::Instance().JobIdUsed(-1));
    EXPECT_FALSE(Scheduler::Instance().JobIdUsed(Scheduler::Instance().getNewJobId()));
    const job_id_t job_id1 = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().JobIdUsed(job_id1));
    Scheduler::Instance().deleteJob(job_id1);
    ASSERT_FALSE(Scheduler::Instance().JobIdUsed(job_id1));
}

void NullFunc(uv_work_t* req, int status) {};

TEST_F(SchedulerTest, Train_Not_Found) {
    try {
        Scheduler::Instance().startJob(-1, NullFunc);
    } catch (const exception& e) {
        EXPECT_STREQ("Job id must correspond to a job that has been created.", e.what());
    }
}

TEST_F(SchedulerTest, Train) {
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().setX(job_id, X));
    ASSERT_TRUE(Scheduler::Instance().setY(job_id, y));
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));
}

TEST_F(SchedulerTest, CheckJobProgress) {
    EXPECT_EQ(-1, Scheduler::Instance().checkJobProgress(-1));	// job progress == -1 for bad ID

    // Large Job
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().setX(job_id, LargeX));
    ASSERT_TRUE(Scheduler::Instance().setY(job_id, LargeY));
    ASSERT_EQ(0, Scheduler::Instance().checkJobProgress(job_id));	// job progress == 0 before being run
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));
    while(Scheduler::Instance().checkJobProgress(job_id) == 0) {
    	usleep(1);
    }
    float progress = Scheduler::Instance().checkJobProgress(job_id);	// 0 < job progress < 1 before end of run
    ASSERT_GE(progress, 0);
    ASSERT_LT(progress, 1);
    float progress_2 = Scheduler::Instance().checkJobProgress(job_id);	// job progress monotonically increasing
    ASSERT_GE(progress_2, progress);
    while(Scheduler::Instance().checkJobProgress(job_id) < 1.0) {
        usleep(1);
    }
    ASSERT_EQ(1.0, Scheduler::Instance().checkJobProgress(job_id));	// job progress == 1 after run

    // Everything should be the same for a second run (this small job only takes 1 iteration, though).
    job_id_t job_id2 = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    Scheduler::Instance().setX(job_id2, X);
    Scheduler::Instance().setY(job_id2, y);
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id2, NullFunc));

    progress = Scheduler::Instance().checkJobProgress(job_id2);
    ASSERT_GE(progress, 0);
    progress_2 = Scheduler::Instance().checkJobProgress(job_id2);
    ASSERT_GE(progress_2, progress);

    while(Scheduler::Instance().checkJobProgress(job_id2) < 1.0) {
        usleep(1);
    }
    ASSERT_EQ(1.0, Scheduler::Instance().checkJobProgress(job_id2));

    // Run large job again
    job_id_t job_id3 = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_NE(job_id, job_id3);
    Scheduler::Instance().setX(job_id3, LargeX);
    Scheduler::Instance().setY(job_id3, LargeY);
    ASSERT_EQ(0, Scheduler::Instance().checkJobProgress(job_id3));
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id3, NullFunc));
    while(Scheduler::Instance().checkJobProgress(job_id3) == 0) {
    	usleep(1);
    }
    progress = Scheduler::Instance().checkJobProgress(job_id3);
    ASSERT_GE(progress, 0);
    ASSERT_LT(progress, 1);
    progress_2 = Scheduler::Instance().checkJobProgress(job_id3);
    ASSERT_GE(progress_2, progress);
    while(Scheduler::Instance().checkJobProgress(job_id3) < 1.0) {
        usleep(1);
    }
    ASSERT_EQ(1.0, Scheduler::Instance().checkJobProgress(job_id3));

    ASSERT_TRUE(Scheduler::Instance().deleteJob(job_id3));
    ASSERT_EQ(Scheduler::Instance().checkJobProgress(job_id3), -1);	// job progress == -1 after being deleted
}

TEST_F(SchedulerTest, DeleteJob) {
    ASSERT_FALSE(Scheduler::Instance().deleteJob(-1));  // can't delete non-existent job

    // Short job - delete after finishing
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    Scheduler::Instance().setX(job_id, X);
    Scheduler::Instance().setY(job_id, y);
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));

    float progress = Scheduler::Instance().checkJobProgress(job_id);
    ASSERT_GE(progress, 0);
    float progress_2 = Scheduler::Instance().checkJobProgress(job_id);
    ASSERT_GE(progress_2, progress);
    while(Scheduler::Instance().checkJobProgress(job_id) < 1.0) {
        usleep(1);
    }
    ASSERT_EQ(1.0, Scheduler::Instance().checkJobProgress(job_id));
    ASSERT_TRUE(Scheduler::Instance().deleteJob(job_id));
    ASSERT_FALSE(Scheduler::Instance().deleteJob(job_id));  // can't delete job twice

    // Large job - delete while it is running
    job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().setX(job_id, LargeX));
    ASSERT_TRUE(Scheduler::Instance().setY(job_id, LargeY));
    ASSERT_EQ(0, Scheduler::Instance().checkJobProgress(job_id));   // job progress == 0 before being run
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));
    while(Scheduler::Instance().checkJobProgress(job_id) == 0) {
        usleep(1);
    }
    progress = Scheduler::Instance().checkJobProgress(job_id);   // 0 < job progress < 1 before end of run
    ASSERT_GE(progress, 0);
    ASSERT_LT(progress, 1);
    ASSERT_TRUE(Scheduler::Instance().deleteJob(job_id));   // should be able to safely delete a job while it's running (it gets cancelled)
    ASSERT_EQ(-1, Scheduler::Instance().checkJobProgress(job_id)); // deleted job has progress = -1

    // Should be able to do it all again.
    job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    ASSERT_TRUE(Scheduler::Instance().setX(job_id, LargeX));
    ASSERT_TRUE(Scheduler::Instance().setY(job_id, LargeY));
    ASSERT_EQ(0, Scheduler::Instance().checkJobProgress(job_id));   // job progress == 0 before being run
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));
    while(Scheduler::Instance().checkJobProgress(job_id) == 0) {
        usleep(1);
    }
    progress = Scheduler::Instance().checkJobProgress(job_id);   // 0 < job progress < 1 before end of run
    ASSERT_GE(progress, 0);
    ASSERT_LT(progress, 1);
    ASSERT_TRUE(Scheduler::Instance().deleteJob(job_id));   // should be able to safely delete a job while it's running (it gets cancelled)
    ASSERT_EQ(-1, Scheduler::Instance().checkJobProgress(job_id)); // deleted job has progress = -1
}

TEST_F(SchedulerTest, GetJobResult) {
    job_id_t job_id = Scheduler::Instance().newJob(JobOptions_t(alg_opts, model_opts));
    Scheduler::Instance().setX(job_id, X);
    Scheduler::Instance().setY(job_id, y);
    ASSERT_TRUE(Scheduler::Instance().startJob(job_id, NullFunc));

    MatrixXf results = Scheduler::Instance().getJobResult(job_id);
    while (Scheduler::Instance().checkJobProgress(job_id) < 1.0) {
        usleep(1);
    }
    results = Scheduler::Instance().getJobResult(job_id);
}


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <map>
#include <unordered_map>
#include <cstdlib>

#include <boost/algorithm/string.hpp>

#include "Day3.hpp"

std::unordered_map<int, std::unordered_map<int, int>> fillMap(std::vector<std::string>& instructions);
bool mapsOverlapAtCoordinate(std::unordered_map<int, std::unordered_map<int, int>>& map1, std::unordered_map<int, std::unordered_map<int, int>>& map2, int x, int y);

void day3() {
	std::ifstream inputFile("Data/Day3.txt");
	std::string line1, line2;
	std::getline(inputFile, line1);
	std::getline(inputFile, line2);
	std::cout << line1 << std::endl << std::endl << line2 << std::endl << std::endl;

	std::vector<std::string> instructionsLine1;
	std::vector<std::string> instructionsLine2;
	boost::split(instructionsLine1, line1, boost::is_any_of(","), boost::token_compress_on);
	boost::split(instructionsLine2, line2, boost::is_any_of(","), boost::token_compress_on);

	auto map1 = fillMap(instructionsLine1);
	auto map2 = fillMap(instructionsLine2);

	std::pair<int, int> closestIntersection(0, 0);
	int closestDistance = 99999999;

	for (std::unordered_map<int, std::unordered_map<int, int>>::iterator it = map1.begin(); it != map1.end(); ++it) {
		int x = it->first;
		for (std::unordered_map<int, int>::iterator it2 = it->second.begin(); it2 != it->second.end(); ++it2) {
			int y = it2->first;

			bool overlap = mapsOverlapAtCoordinate(map1, map2, x, y);

			if (overlap) {
				// Check for Part 1 (closest Manhattan Distance)
				//	int distance = abs(x) + abs(y);

				// Check for Part 2 (shortest path)
				int distance1 = map1[x][y];
				int distance2 = map2[x][y];
				int distance = distance1 + distance2;
				//std::cout << distance << " ";
				if (distance < closestDistance) {
					closestIntersection.first = x;
					closestIntersection.second = y;
					closestDistance = distance;
					std::cout << "New closest : " << closestIntersection.first << "," << closestIntersection.second << " - Distance : " << closestDistance << std::endl;
				}
			}
		}
	}

}

std::unordered_map<int, std::unordered_map<int, int>> fillMap(std::vector<std::string>& instructions) {
	std::unordered_map<int, std::unordered_map<int, int>> map;
	std::pair<int, int> currentPosition(0, 0);
	int distance = 0;

	for (auto instruction = instructions.begin(); instruction != instructions.end(); ++instruction) {
		char direction = instruction->at(0);
		int number = std::stoi(instruction->substr(1));
		//std::cout << *instruction << ": " << direction << " " << number << std::endl;

		switch (direction)
		{
		case 'U':
			for (int y = 0; y < number; ++y) {
				currentPosition.second++;
				distance++;
				map[currentPosition.first][currentPosition.second] = distance;
			}
			break;
		case 'R':
			for (int x = 0; x < number; ++x) {
				currentPosition.first++;
				distance++;
				map[currentPosition.first][currentPosition.second] = distance;
			}
			break;
		case 'D':
			for (int y = 0; y < number; ++y) {
				currentPosition.second--;
				distance++;
				map[currentPosition.first][currentPosition.second] = distance;
			}
			break;
		case 'L':
			for (int x = 0; x < number; ++x) {
				currentPosition.first--;
				distance++;
				map[currentPosition.first][currentPosition.second] = distance;
			}
			break;
		}
	}

	return map;
}

bool mapsOverlapAtCoordinate(std::unordered_map<int, std::unordered_map<int, int>>& map1, std::unordered_map<int, std::unordered_map<int, int>>& map2, int x, int y) {
	if (map1.find(x) != map1.end() && map2.find(x) != map2.end()) {
		if (map1[x].find(y) != map1[x].end() && map2[x].find(y) != map2[x].end()) {
			return true;
		}
	}
	return false;
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/all.h++>
#include <mla/vector/all.h++>
#include <mla/matrix/convert.h++>

#include <mla/operations/level2/gemv.h++>

typedef boost::mpl::list<
	mla::matrix::DenseRowMajor<float>,
	mla::matrix::DenseRowMajor<double>,
	mla::matrix::SparseCRS<float>,
	mla::matrix::SparseCRS<double>
> matrix_type_list;

BOOST_AUTO_TEST_SUITE(test_operations)

BOOST_AUTO_TEST_CASE_TEMPLATE( matrix_multiply, MatrixType, matrix_type_list )
{
	size_t matrix_size = 6;

	typedef typename MatrixType::scalar_type Scalar;

	MatrixType A(matrix_size, matrix_size);
	A.setEye();

	mla::vector::Dense<Scalar> x(matrix_size), y(matrix_size);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		x.setValue( i, (Scalar)1.0f);
	}

	Scalar const alfa = 1.0;
	Scalar const beta = 1.0;
	mla::gemv(alfa, A, x, beta, y);

	for(unsigned int i = 0; i < matrix_size; i++)
	{
		BOOST_CHECK_CLOSE( x.getValue(i), y.getValue(i), 0.001f);
	}
}

BOOST_AUTO_TEST_SUITE_END()


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

/*
 Copyright (C) 2004, 2005, 2008 Klaus Spanderen
 Copyright (C) 2007 StatPro Italia srl

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

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

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

/*! \file analytichestonengine.hpp
    \brief analytic Heston-model engine
*/

#ifndef quantlib_analytic_heston_engine_hpp
#define quantlib_analytic_heston_engine_hpp

#include <ql/math/integrals/integral.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>
#include <ql/pricingengines/genericmodelengine.hpp>
#include <ql/models/equity/hestonmodel.hpp>
#include <ql/instruments/vanillaoption.hpp>

#include <boost/function.hpp>
#include <complex>

namespace QuantLib {

    //! analytic Heston-model engine based on Fourier transform

    /*! Integration detail:
        Two algebraically equivalent formulations of the complex
        logarithm of the Heston model exist. Gatherals [2005]
        (also Duffie, Pan and Singleton [2000], and Schoutens,
        Simons and Tistaert[2004]) version does not cause
        discoutinuities whereas the original version (e.g. Heston [1993])
        needs some sort of "branch correction" to work properly.
        Gatheral's version does also work with adaptive integration
        routines and should be preferred over the original Heston version.
    */

    /*! References:

        Heston, Steven L., 1993. A Closed-Form Solution for Options
        with Stochastic Volatility with Applications to Bond and
        Currency Options.  The review of Financial Studies, Volume 6,
        Issue 2, 327-343.

        A. Sepp, Pricing European-Style Options under Jump Diffusion
        Processes with Stochastic Volatility: Applications of Fourier
        Transform (<http://math.ut.ee/~spartak/papers/stochjumpvols.pdf>)

        R. Lord and C. Kahl, Why the rotation count algorithm works,
        http://papers.ssrn.com/sol3/papers.cfm?abstract_id=921335

        H. Albrecher, P. Mayer, W.Schoutens and J. Tistaert,
        The Little Heston Trap, http://www.schoutens.be/HestonTrap.pdf

        J. Gatheral, The Volatility Surface: A Practitioner's Guide,
        Wiley Finance

        \ingroup vanillaengines

        \test the correctness of the returned value is tested by
              reproducing results available in web/literature
              and comparison with Black pricing.
    */
    class AnalyticHestonEngine
        : public GenericModelEngine<HestonModel,
                                    VanillaOption::arguments,
                                    VanillaOption::results> {
      public:
        class Integration;
        enum ComplexLogFormula { Gatheral, BranchCorrection };

        // Simple to use constructor: Using adaptive
        // Gauss-Lobatto integration and Gatheral's version of complex log.
        // Be aware: using a too large number for maxEvaluations might result
        // in a stack overflow as the Lobatto integration is a recursive
        // algorithm.
        AnalyticHestonEngine(const boost::shared_ptr<HestonModel>& model,
                             Real relTolerance, Size maxEvaluations);

        // Constructor using Laguerre integration
        // and Gatheral's version of complex log.
        AnalyticHestonEngine(const boost::shared_ptr<HestonModel>& model,
                             Size integrationOrder = 144);

        // Constructor giving full control
        // over the Fourier integration algorithm
        AnalyticHestonEngine(const boost::shared_ptr<HestonModel>& model,
                             ComplexLogFormula cpxLog, const Integration& itg);

        void calculate() const;
        Size numberOfEvaluations() const;

        static void doCalculation(Real riskFreeDiscount,
                                             Real dividendDiscount,
                                             Real spotPrice,
                                             Real strikePrice,
                                             Real term,
                                             Real kappa, Real theta, Real sigma, Real v0, Real rho,
                                             const TypePayoff& type,
                                             const Integration& integration,
                                             const ComplexLogFormula cpxLog,
                                             const AnalyticHestonEngine* const enginePtr,
                                             Real& value,
                                             Size& evaluations);

      protected:
        // call back for extended stochastic volatility
        // plus jump diffusion engines like bates model
        virtual std::complex<Real> addOnTerm(Real phi,
                                             Time t,
                                             Size j) const;

      private:
        class Fj_Helper;

        mutable Size evaluations_;
        const ComplexLogFormula cpxLog_;
        const boost::shared_ptr<Integration> integration_;

    };

    class AnalyticHestonEngine::Integration {
      public:
        // non adaptive integration algorithms based on Gaussian quadrature
        static Integration gaussLaguerre    (Size integrationOrder = 128);
        static Integration gaussLegendre    (Size integrationOrder = 128);
        static Integration gaussChebyshev   (Size integrationOrder = 128);
        static Integration gaussChebyshev2nd(Size integrationOrder = 128);

        // for an adaptive integration algorithm Gatheral's version has to
        // be used.Be aware: using a too large number for maxEvaluations might
        // result in a stack overflow as the these integrations are based on
        // recursive algorithms.
        static Integration gaussLobatto(Real relTolerance, Real absTolerance,
                                        Size maxEvaluations = 1000);

        // usually these routine have a poor convergence behaviour.
        static Integration gaussKronrod(Real absTolerance,
                                        Size maxEvaluations = 1000);
        static Integration simpson(Real absTolerance,
                                   Size maxEvaluations = 1000);
        static Integration trapezoid(Real absTolerance,
                                     Size maxEvaluations = 1000);

        Real calculate(Real c_inf,
                       const boost::function1<Real, Real>& f) const;

        Size numberOfEvaluations() const;
        bool isAdaptiveIntegration() const;

      private:
        enum Algorithm
            { GaussLobatto, GaussKronrod, Simpson, Trapezoid,
              GaussLaguerre, GaussLegendre,
              GaussChebyshev, GaussChebyshev2nd };

        Integration(Algorithm intAlgo,
                    const boost::shared_ptr<GaussianQuadrature>& quadrature);

        Integration(Algorithm intAlgo,
                    const boost::shared_ptr<Integrator>& integrator);

        const Algorithm intAlgo_;
        const boost::shared_ptr<Integrator> integrator_;
        const boost::shared_ptr<GaussianQuadrature> gaussianQuadrature_;
    };

    // inline

    inline
    std::complex<Real> AnalyticHestonEngine::addOnTerm(Real,
                                                       Time,
                                                       Size) const {
        return std::complex<Real>(0,0);
    }

}

#endif


#include <cmath>
#include <limits>
#include <stdexcept>
#include <iostream>
#include <boost/filesystem.hpp>
#include <sstream>
#include <cstdlib>
#include <fstream>
#include <cfenv>
#include <X11/xpm.h>
#include <iomanip>
#include <FL/x.H>
#include "utils.hpp"
#include "florb.xpm"

#define CLIPLEFT   (1)  // 0001
#define CLIPRIGHT  (2)  // 0010
#define CLIPBOTTOM (4)  // 0100
#define CLIPTOP    (8)  // 1000

#define CIRCUMFERENCEKM (2*M_PI*6372.7982)

florb::point2d<double> florb::utils::wsg842merc(const florb::point2d<double> &wsg84)
{
    if ((wsg84.x() > 180.0) || (wsg84.x() < -180.0))
        throw std::out_of_range(_("Invalid longitude"));
    if ((wsg84.y() > 90.0) || (wsg84.y() < -90.0))
        throw std::out_of_range(_("Invalid latitude"));

    // Anything above 85 degrees N or S is infinity in the mercator projection,
    // clip here.
    double lat = wsg84.y();
    if (lat > 85.0)
        lat = 85.0;
    else if (lat < -85)
        lat = -85.0;

    // Project to Mercator (360 by 360 square)
    return florb::point2d<double>(
            180.0 + wsg84.x(),
            180.0 - ((180.0/M_PI) * log(tan(M_PI/4.0+lat*(M_PI/180.0)/2.0))));
}

florb::point2d<unsigned long> florb::utils::merc2px(unsigned int z, const florb::point2d<double> &merc)
{
    unsigned long dimxy = dim(z);

    return florb::point2d<unsigned long>(
        (unsigned long)(((double)(dimxy-1)/360.0) * merc.x()),
        (unsigned long)(((double)(dimxy-1)/360.0) * merc.y()));
}

florb::point2d<unsigned long> florb::utils::wsg842px(unsigned int z, const florb::point2d<double> &wsg84)
{
    // Mercator projection
    florb::point2d<double> merc(wsg842merc(wsg84));

    // Unit conversion
    return merc2px(z, merc);
}

florb::point2d<double> florb::utils::px2wsg84(unsigned int z, const florb::point2d<unsigned long> &px)
{
    // Get map dimensions
    unsigned long dimxy = dim(z);

    // Make sure the coordinate is on the map
    if ((px.x() > dimxy) || (px.y() > dimxy))
        throw std::out_of_range(_("Invalid pixel position"));

    // Unit conversion
    florb::point2d<double> mdeg(
            (360.0/((double)dimxy/(double)px.x())),
            (360.0/((double)dimxy/(double)px.y())));

    // Convert mercator to GPS coordinate
    return merc2wsg84(mdeg);
}

florb::point2d<double> florb::utils::merc2wsg84(const florb::point2d<double>& wsg84)
{
    return florb::point2d<double>(
            wsg84.x() - 180.0,
            -((180.0/M_PI) * (2.0 * atan(exp((wsg84.y()-180.0)*M_PI/180.0)) - M_PI/2.0)));
}

florb::point2d<double> florb::utils::px2merc(unsigned int z, const florb::point2d<unsigned long> &px)
{
    unsigned long dimxy = dim(z);

    // Make sure the coordinate is on the map
    if ((px.x() >= dimxy) || (px.y() >= dimxy))
    {
        throw std::out_of_range(_("Invalid pixel position"));
    }

    double x = (px.x() == 0) ? 0.0 : (360.0/((double)(dimxy-1)/(double)px.x()));
    double y = (px.y() == 0) ? 0.0 : (360.0/((double)(dimxy-1)/(double)px.y()));
    return florb::point2d<double>(x,y);
}

unsigned long florb::utils::dim(unsigned int z)
{
    return pow(2.0, z) * 256;
}

double florb::utils::dist(const florb::point2d<double> &p1, const florb::point2d<double> &p2)
{
    if (p1 == p2)
        return 0.0;

    double d2r = (M_PI/180.0);
    double lon1 = p1.x()*d2r;
    double lat1 = p1.y()*d2r;
    double lon2 = p2.x()*d2r;
    double lat2 = p2.y()*d2r;

    double ret = (6378.388 * acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)));

    return (std::isnan(ret) > 0) ? 0.0 : ret;
}

double florb::utils::dist_merc(const florb::point2d<double> &p1, const florb::point2d<double> &p2)
{
    if (p1 == p2)
        return 0.0;

    double dstmerc = sqrt( pow(std::abs(p1.x()-p2.x()), 2.0) + pow(std::abs(p1.y()-p2.y()), 2.0)*0.9444444 );
    return (dstmerc * (CIRCUMFERENCEKM/360.0));
}

double florb::utils::meters_per_pixel(unsigned int z, double lat)
{
    return (CIRCUMFERENCEKM * cos(lat*(M_PI/180.0)) / pow(2.0,(z+8))) * 1000.0;
}

time_t florb::utils::iso8601_2timet(const std::string& iso)
{
    struct tm stm;
    strptime(iso.c_str(), "%FT%T%z", &stm);

    return mktime(&stm);
}

std::string florb::utils::timet2iso8601(time_t t)
{
    char buf[sizeof "2011-10-08T07:07:09Z"];
    strftime(buf, sizeof buf, "%FT%TZ", gmtime(&t));

    return std::string(buf);
}

std::string florb::utils::pathsep()
{
#if defined(WIN32) || defined(_WIN32)
return std::string("\\");
#else
return std::string("/");
#endif
}

std::string florb::utils::userdir()
{
    char *home = getenv("HOME");
    if (!home)
        throw 0;

    return std::string(home);
}

std::string florb::utils::appdir()
{
    std::ostringstream oss;
    oss << userdir();
    oss << pathsep();
    oss << ".florb";

    return oss.str();
}

bool florb::utils::mkdir(const std::string& path)
{
    bool ret = true;

    try {
        boost::filesystem::create_directory(path);
    } catch (...) {
        ret = false;
    }

    return ret;
}

void florb::utils::rm(const std::string& path)
{
    boost::filesystem::remove_all(path);
}

bool florb::utils::exists(const std::string& path)
{
    return boost::filesystem::exists(path);
}

std::string florb::utils::filestem(const std::string& path)
{
    return boost::filesystem::path(path).stem().string();
}

std::string florb::utils::extension(const std::string& path)
{
    return boost::filesystem::path(path).extension().string();
}

void florb::utils::touch(const std::string& path)
{
    std::fstream f(path, std::ios::out|std::ios::app);
	f.close();
}

void florb::utils::set_window_icon(Fl_Window *w)
{
    fl_open_display();
    Pixmap p, mask;
    XpmCreatePixmapFromData(fl_display, DefaultRootWindow(fl_display), const_cast<char**>(florb_xpm), &p, &mask, NULL);
    w->icon((char *)p);
}

std::vector<std::string> florb::utils::str_split(const std::string& str, const std::string& delimiter)
{
    std::size_t offs = 0, p1 = 0, p2 = std::string::npos;
    std::size_t len = str.length();

    std::vector<std::string> ret;
    while ((offs < len) && (p2 = str.find(delimiter, offs)) != std::string::npos)
    {
        std::string token(str.substr(p1, p2-p1));

        if (token.length() > 0)
            ret.push_back(str.substr(p1, p2-p1));

        p1 = p2 + delimiter.length();
        offs = p1;
    }

    if ((p1 != len))
        ret.push_back(str.substr(p1, len-p1));

    return ret;
}

std::size_t florb::utils::str_count(const std::string& str, const std::string& token)
{
    if (token.length() == 0)
        return 0;

    std::size_t ret = 0, offs = 0;

    while ((offs = str.find(token, offs)) != std::string::npos)
    {
        offs += token.length();
        ret++;
    }

    return ret;
}

void florb::utils::str_replace(std::string& str, const std::string& s, const std::string& r)
{
    std::size_t offs = 0;
    while ((offs = str.find(s, 0)) != std::string::npos)
    {
        str.replace(offs, s.length(), r);
    }
}

// Cohen–Sutherland clipping algorithm
bool florb::utils::clipline(
        florb::point2d<double> &p1,
        florb::point2d<double> &p2,
        const florb::point2d<double> &r1,
        const florb::point2d<double> &r2,
        bool &p1clip,
        bool &p2clip)
{
    bool ret = false;
    double xmin, xmax, ymin, ymax;
    p1clip = false;
    p2clip = false;

    if (r1.x() > r2.x())
    {
        xmin = r2.x();
        xmax = r1.x();
    }
    else
    {
        xmin = r1.x();
        xmax = r2.x();
    }

    if (r1.y() > r2.y())
    {
        ymin = r2.y();
        ymax = r1.y();
    }
    else
    {
        ymin = r1.y();
        ymax = r2.y();
    }

    bool overflowa = false;

    // Catch arithmetic overflow when calculating m and n
    double n = 0, m = 0;
    for (;;)
    {
        std::feclearexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO);

        m = (p2.y() - p1.y()) / (p2.x() - p1.x());
        if (std::fetestexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO) != 0)
        {
            overflowa = true;
            break;
        }

        std::feclearexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO);

        n = p1.y() - (m * p1.x());
        if (std::fetestexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO) != 0)
        {
            overflowa = true;
            break;
        }

        break;
    }

    // Max 2 clipping operations per point and one last check operation.
    for (int i=0;i<5;i++)
    {
        // Compute code for both points
        int code1 = 0, code2 = 0;

        if (p1.x() < xmin)
            code1 |= CLIPLEFT;
        if (p1.x() > xmax)
            code1 |= CLIPRIGHT;
        if (p1.y() < ymin)
            code1 |= CLIPTOP;
        if (p1.y() > ymax)
            code1 |= CLIPBOTTOM;

        if (p2.x() < xmin)
            code2 |= CLIPLEFT;
        if (p2.x() > xmax)
            code2 |= CLIPRIGHT;
        if (p2.y() < ymin)
            code2 |= CLIPTOP;
        if (p2.y() > ymax)
            code2 |= CLIPBOTTOM;

        // Both inside, draw line
        if ((code1 | code2) == 0)
        {
            ret = true;
            break;
        }

        // Both top, bottom, left or right outside, line need not be drawn
        if ((code1 & code2) != 0)
        {
            ret = false;
            break;
        }

        // Pick an endpoint for clipping
        florb::point2d<double> &ptmp = (code1 != 0) ? p1 : p2;
        int codetmp;
        if (code1 != 0)
        {
            codetmp = code1;
            p1clip = true;
        }
        else
        {
            codetmp = code2;
            p2clip = true;
        }

        // Clip top
        if (codetmp & CLIPTOP)
        {
            // This is what the following code does with just a little overflow
            // protection:
            // ptmp[0] = (ymin - n) / m;

            double cx;
            for (;!overflowa;)
            {
                std::feclearexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO);
                cx = (ymin - n) / m;

                if (std::fetestexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO) != 0)
                {
                    overflowa = true;
                    break;
                }

                break;
            }

            // In case of an arithmetic exception we assume a vertical line and
            // keep the original x coordinate
            if (!overflowa)
                ptmp[0] = cx;

            ptmp[1] = ymin;
        }
        // Clip Bottom
        else if (codetmp & CLIPBOTTOM)
        {
            // This is what the following code does with just a little overflow
            // protection:
            // ptmp[0] = (ymax - n) / m;

            double cx;
            for (;!overflowa;)
            {
                std::feclearexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO);
                cx = (ymax - n) / m;

                if (std::fetestexcept(FE_OVERFLOW|FE_UNDERFLOW|FE_DIVBYZERO) != 0)
                {
                    overflowa = true;
                    break;
                }

                break;
            }

            // In case of an arithmetic exception we assume a vertical line and
            // keep the original x coordinate
            if (!overflowa)
                ptmp[0] = cx;

            ptmp[1] = ymax;
        }
        // Clip Left
        else if (codetmp & CLIPLEFT)
        {
            // Vertical line left outside
            if (overflowa)
            {
                ret = false;
                break;
            }

            ptmp[0] = xmin;
            ptmp[1] = m * xmin + n;
        }
        // Clip Right
        else if (codetmp & CLIPRIGHT)
        {
            // Vertical line right outside
            if (overflowa)
            {
                ret = false;
                break;
            }

            ptmp[0] = xmax;
            ptmp[1] = m * xmax + n;
        }
    }

    return ret;
}


#include "include/MMVII_all.h"

// #include <Eigen/Dense>

namespace MMVII
{

/// Sigma of convol sqrt(A^2+B^2)
double SomSigm(double  aS1,double aS2) {return sqrt(Square(aS1)+Square(aS2));}
/// "Inverse" of SomSigm  sqrt(A^2-B^2)
double DifSigm(double  aS1,double aS2)
{
   MMVII_INTERNAL_ASSERT_tiny(aS1>=aS2,"DifSigm");
   return sqrt(Square(aS1)-Square(aS2));
}

/** We use a class essentially to be able to use typedef, most method will be static */
template <class Type> class cLinearFilter
{
    public :
      // in comments, note @ for convolution
      typedef Type                                  tVal;
      typedef typename tNumTrait<Type>::tBase       tBase;
      typedef typename tNumTrait<Type>::tFloatAssoc tFl;

      static void FilterExp
                  (
                      bool Normalize,  // If true, limit size effect and Cste => Cste (else lowe close to border)
                      cDataIm2D<Type> &,
                      int aNbIter,
                      const cRect2 &,
                      const tFl &aFx,
                      const  tFl &aFy
                  );
};

/**
     Convolution of aLineIm in place by expononential filter "F^|x|", use a buffer
*/
template <class TBuf,class TIm,class TFact,class TyNorm> void
   OneLineFilterExp
   (
       int aNbIter,
       TBuf *aDBuf,
       TIm* aLineIm,
       TFact aFact,
       int anX0,
       int anX1,
       TyNorm * aDataNorm
   )
{
   for (int aKIter=0 ; aKIter<aNbIter ; aKIter++)
   {
      aDBuf[anX0] = 0;
      // left to right ,at end  aDBuf  contains "I  @ (Fx^|x| * x<0)"
      for (int anX = anX0+1; anX<anX1 ; anX++)
      {
          aDBuf[anX] =  aFact *(aDBuf[anX-1] + aLineIm[anX-1]);
      }
      // right to left, full convolution
      for (int anX = anX1-2; anX>=anX0 ; anX--)
      {
          aLineIm[anX]  +=(TIm)(aFact*aLineIm[anX+1] );  // now Line contains "I @  (Fx^|x| * x>=0)
          aLineIm[anX+1]+=(TIm)(aDBuf[anX+1] ); // now contains   I  @ Fx^|x|
      }
   }
   if (aDataNorm)
   {
      for (int anX= anX0 ; anX<anX1 ; anX++)
         aLineIm[anX] /= aDataNorm[anX];
   }
}

template <class Type> Type * ImNormal(int aNbIter,cDataIm1D<Type> & aRes,Type aFact,int aX0,int aX1)
{
   aRes.Resize(cPt1di(aX0),cPt1di(aX1));
   aRes.InitCste(1.0);

   cIm1D<Type>  aBuf(aX0,aX1);
   OneLineFilterExp(aNbIter,aBuf.DIm().ExtractRawData1D(),aRes.ExtractRawData1D(),aFact,aX0,aX1,(float *)nullptr);

   return aRes.ExtractRawData1D();
}

template <class Type> void  cLinearFilter<Type>::FilterExp
                            (
                                 bool Normalise,
                                 cDataIm2D<Type> & aIm,
                                 int   aNbIter,
                                 const cRect2 & aRect,
                                 const tFl &aFx,
                                 const  tFl &aFy
                            )
{
   MMVII_INTERNAL_ASSERT_strong(aRect.IncludedIn(aIm),"cLinearFilter Rect is outside");

   // Local copy to have quick access
   Type  **  aDIm = aIm.ExtractRawData2D();
   int anX0 = aRect.P0().x();
   int anY0 = aRect.P0().y();
   int anX1 = aRect.P1().x();
   int anY1 = aRect.P1().y();

   // Create a buf  to avoid sides effect
   // int aNbBuf = Norm1(aRect.Sz());
   cIm1D<tBase> aImBuf(Norm1(aRect.Sz()));
   cIm1D<tVal> aBufDupCol(aRect.Sz().y());

   cIm1D<tFl>  aImNormal(Norm1(aRect.Sz()));
   // tFl * aBufNorm = nullptr;

       //  Filter the lines , I @  Fx^|x|
   if (aFx != 0)
   {
      tBase * aDBuf = aImBuf.DIm().RawDataLin()-anX0;
      tFl * aDataNorm = Normalise ? ImNormal(aNbIter,aImNormal.DIm(),aFx,anX0,anX1) : nullptr;

      for (int anY=anY0 ; anY<anY1 ; anY++)
      {
           OneLineFilterExp(aNbIter,aDBuf,aDIm[anY],aFx,anX0,anX1,aDataNorm);
      }
   }
       //  Filter the Column , I @  Fy^|y|
   if (aFy != 0)
   {
       tBase * aDBuf = aImBuf.DIm().RawDataLin()-anY0;
       tVal  * aDupCol = aBufDupCol.DIm().RawDataLin()-anY0;
       tFl * aDataNorm = Normalise ? ImNormal(aNbIter,aImNormal.DIm(),aFy,anY0,anY1) : nullptr;
       for (int anX=anX0 ; anX<anX1 ; anX++)
       {
          // Transferate Column X in line buf aDupCol
          for (int anY=anY0 ; anY<anY1 ; anY++)
          {
             aDupCol[anY] = aDIm[anY][anX];
          }
          // Filter dup col
          OneLineFilterExp(aNbIter,aDBuf,aDupCol,aFy,anY0,anY1,aDataNorm);
          // Inverse transfer
          for (int anY=anY0 ; anY<anY1 ; anY++)
          {
             aDIm[anY][anX] = aDupCol[anY] ;
          }
       }
   }
}

template <class Type>
void  ExponentialFilter(bool Normalise,cDataIm2D<Type> & aDIm,int  aNbIt,const cRect2 & aR2,double Fx,double Fy)
{
   cLinearFilter<Type>::FilterExp(Normalise,aDIm,aNbIt,aR2,Fx,Fy);
}

template <class Type>
void  ExponentialFilter(cDataIm2D<Type> & aIm,int   aNbIter,double aFact)
{
    ExponentialFilter(true,aIm,aNbIter,aIm,aFact,aFact);
}

template <class Type>
void  ExpFilterOfStdDev(cDataIm2D<Type> & aIm,int   aNbIter,double aStdDev)
{
     ExponentialFilter(aIm,aNbIter,FactExpFromSigma2(Square(aStdDev)/aNbIter));
}

template <class Type>
void  ExpFilterOfStdDev(cDataIm2D<Type> & aImOut,const cDataIm2D<Type> & aImIn,int   aNbIter,double aStdDev)
{
     aImIn.DupIn(aImOut);
     ExponentialFilter(aImOut,aNbIter,FactExpFromSigma2(Square(aStdDev)/aNbIter));
}

/* ========================== */
/*       cImGrad              */
/* ========================== */

template <class Type> cImGrad<Type>::cImGrad(const cIm2D<Type> & aGx,const cIm2D<Type> &  aGy) :
    mGx (aGx),
    mGy (aGy)
{
}
template <class Type> cImGrad<Type>::   cImGrad(const cPt2di & aSz) :
     cImGrad<Type>(cIm2D<Type>(aSz),cIm2D<Type>(aSz))
{
}

template <class Type> cImGrad<Type>::cImGrad(const cIm2D<Type> & aImIn) :
   cImGrad<Type>(aImIn.DIm().Sz())
{
}

/* ========================== */
/*     cDataGenUnTypedIm      */
/* ========================== */

#define MACRO_INSTANTIATE_ExpoFilter(Type)\
template  class cImGrad<Type>;\
template  class cLinearFilter<Type>;\
template void ExponentialFilter(bool,cDataIm2D<Type> &,int,const cRect2 &,double,double);\
template void  ExponentialFilter(cDataIm2D<Type> & aIm,int   aNbIter,double aFact);\
template void  ExpFilterOfStdDev(cDataIm2D<Type> & aIm,int   aNbIter,double aStdDev);\
template void  ExpFilterOfStdDev(cDataIm2D<Type> & aIm,const cDataIm2D<Type> & aImIn,int   aNbIter,double aStdDev);\

MACRO_INSTANTIATE_ExpoFilter(tREAL4);
MACRO_INSTANTIATE_ExpoFilter(tREAL8);
MACRO_INSTANTIATE_ExpoFilter(tREAL16);
MACRO_INSTANTIATE_ExpoFilter(tINT4);
MACRO_INSTANTIATE_ExpoFilter(tINT2);
MACRO_INSTANTIATE_ExpoFilter(tINT1);

/*
template class cDataTypedIm<aType,1>;\
template class cDataTypedIm<aType,2>;\
template class cDataTypedIm<aType,3>;

MACRO_INSTANTIATE_cDataTypedIm(tINT1)
MACRO_INSTANTIATE_cDataTypedIm(tINT2)
MACRO_INSTANTIATE_cDataTypedIm(tINT4)

MACRO_INSTANTIATE_cDataTypedIm(tU_INT1)
MACRO_INSTANTIATE_cDataTypedIm(tU_INT2)
MACRO_INSTANTIATE_cDataTypedIm(tU_INT4)

MACRO_INSTANTIATE_cDataTypedIm(tREAL4)
MACRO_INSTANTIATE_cDataTypedIm(tREAL8)
MACRO_INSTANTIATE_cDataTypedIm(tREAL16)
*/

};


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2003 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Guido Kanschat, University of Heidelberg, 2003
 *          Baerbel Janssen, University of Heidelberg, 2010
 *          Wolfgang Bangerth, Texas A&M University, 2010
 *          Timo Heister, Clemson University, 2018
 */

// @sect3{Include files}

// 同样，前几个include文件已经知道了，所以我们不会对它们进行评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>

#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>

#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>

// 现在，这些是多级方法所需的包括。第一个声明了如何处理多网格方法每个层次上的Dirichlet边界条件。对于自由度的实际描述，我们不需要任何新的包含文件，因为DoFHandler已经实现了所有必要的方法。我们只需要将自由度分配给更多的层次。

// 其余的包含文件涉及到作为线性算子（求解器或预处理器）的多重网格的力学问题。

#include <deal.II/multigrid/mg_constrained_dofs.h>
#include <deal.II/multigrid/multigrid.h>
#include <deal.II/multigrid/mg_transfer.h>
#include <deal.II/multigrid/mg_tools.h>
#include <deal.II/multigrid/mg_coarse.h>
#include <deal.II/multigrid/mg_smoother.h>
#include <deal.II/multigrid/mg_matrix.h>

// 我们将使用 MeshWorker::mesh_loop 来对单元格进行循环，所以在这里包括它。

#include <deal.II/meshworker/mesh_loop.h>

// 这就是C++。

#include <iostream>
#include <fstream>

using namespace dealii;

namespace Step16
{
// @sect3{The Scratch and Copy objects}

// 我们使用 MeshWorker::mesh_loop() 来组装我们的矩阵。为此，我们需要一个ScratchData对象来存储每个单元的临时数据（这只是FEValues对象）和一个CopyData对象，它将包含每个单元装配的输出。关于scratch和copy对象的用法的更多细节，请参见WorkStream命名空间。

  template <int dim>
  struct ScratchData
  {
    ScratchData(const Mapping<dim> &      mapping,
                const FiniteElement<dim> &fe,
                const unsigned int        quadrature_degree,
                const UpdateFlags         update_flags)
      : fe_values(mapping, fe, QGauss<dim>(quadrature_degree), update_flags)
    {}

    ScratchData(const ScratchData<dim> &scratch_data)
      : fe_values(scratch_data.fe_values.get_mapping(),
                  scratch_data.fe_values.get_fe(),
                  scratch_data.fe_values.get_quadrature(),
                  scratch_data.fe_values.get_update_flags())
    {}

    FEValues<dim> fe_values;
  };

  struct CopyData
  {
    unsigned int                         level;
    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;

    template <class Iterator>
    void reinit(const Iterator &cell, unsigned int dofs_per_cell)
    {
      cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
      cell_rhs.reinit(dofs_per_cell);

      local_dof_indices.resize(dofs_per_cell);
      cell->get_active_or_mg_dof_indices(local_dof_indices);
      level = cell->level();
    }
  };
// @sect3{The LaplaceProblem class template}

// 这个主类与  step-6  中的同一类相似。就成员函数而言，唯一增加的是。

// --  assemble_multigrid 的函数，该函数组装了对应于中间层离散运算符的矩阵。

// -  cell_worker 函数，它将我们的PDE集合在一个单元上。

  template <int dim>
  class LaplaceProblem
  {
  public:
    LaplaceProblem(const unsigned int degree);
    void run();

  private:
    template <class Iterator>
    void cell_worker(const Iterator &  cell,
                     ScratchData<dim> &scratch_data,
                     CopyData &        copy_data);

    void setup_system();
    void assemble_system();
    void assemble_multigrid();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    AffineConstraints<double> constraints;

    Vector<double> solution;
    Vector<double> system_rhs;

    const unsigned int degree;

// 以下成员是多网格方法的基本数据结构。前四个表示稀疏模式和多级层次结构中各个层次的矩阵，非常类似于上面的全局网格的对象。

// 然后，我们有两个新的矩阵，只需要在自适应网格上进行局部平滑的多网格方法。它们在细化区域的内部和细化边缘之间传递数据，在 @ref mg_paper "多网格论文 "中详细介绍过。

// 最后一个对象存储了每个层次上的边界指数信息和位于两个不同细化层次之间的细化边缘上的指数信息。因此，它的作用与AffineConstraints相似，但在每个层次上。

    MGLevelObject<SparsityPattern> mg_sparsity_patterns;
    MGLevelObject<SparsityPattern> mg_interface_sparsity_patterns;

    MGLevelObject<SparseMatrix<double>> mg_matrices;
    MGLevelObject<SparseMatrix<double>> mg_interface_matrices;
    MGConstrainedDoFs                   mg_constrained_dofs;
  };
// @sect3{The LaplaceProblem class implementation}

// 关于三角形的构造函数只有一个简短的评论：按照惯例，deal.II中所有自适应精化的三角形在单元格之间的面的变化不会超过一个级别。然而，对于我们的多网格算法，我们需要一个更严格的保证，即网格在连接两个单元的顶点上的变化也不超过细化级别。换句话说，我们必须防止出现以下情况。

//  @image html limit_level_difference_at_vertices.png ""

// 这可以通过向三角化类的构造函数传递 Triangulation::limit_level_difference_at_vertices 标志来实现。

  template <int dim>
  LaplaceProblem<dim>::LaplaceProblem(const unsigned int degree)
    : triangulation(Triangulation<dim>::limit_level_difference_at_vertices)
    , fe(degree)
    , dof_handler(triangulation)
    , degree(degree)
  {}

//  @sect4{LaplaceProblem::setup_system}

// 除了只是在DoFHandler中分配自由度之外，我们在每一层都做同样的事情。然后，我们按照之前的程序，在叶子网格上设置系统。

  template <int dim>
  void LaplaceProblem<dim>::setup_system()
  {
    dof_handler.distribute_dofs(fe);
    dof_handler.distribute_mg_dofs();

    std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << " (by level: ";
    for (unsigned int level = 0; level < triangulation.n_levels(); ++level)
      std::cout << dof_handler.n_dofs(level)
                << (level == triangulation.n_levels() - 1 ? ")" : ", ");
    std::cout << std::endl;

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);

    std::set<types::boundary_id> dirichlet_boundary_ids = {0};
    Functions::ZeroFunction<dim> homogeneous_dirichlet_bc;
    const std::map<types::boundary_id, const Function<dim> *>
      dirichlet_boundary_functions = {
        {types::boundary_id(0), &homogeneous_dirichlet_bc}};
    VectorTools::interpolate_boundary_values(dof_handler,
                                             dirichlet_boundary_functions,
                                             constraints);
    constraints.close();

    {
      DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
      DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints);
      sparsity_pattern.copy_from(dsp);
    }
    system_matrix.reinit(sparsity_pattern);

// 多网格约束必须被初始化。他们需要知道在哪里规定了Dirichlet边界条件。

    mg_constrained_dofs.clear();
    mg_constrained_dofs.initialize(dof_handler);
    mg_constrained_dofs.make_zero_boundary_constraints(dof_handler,
                                                       dirichlet_boundary_ids);

// 现在是关于多网格数据结构的事情。首先，我们调整多级对象的大小，以容纳每一级的矩阵和稀疏模式。粗略的级别是零（现在是强制性的，但在未来的修订中可能会改变）。注意，这些函数在这里采取的是一个完整的、包容的范围（而不是一个起始索引和大小），所以最细的级别是 n_levels-1  。我们首先要调整容纳SparseMatrix类的容器的大小，因为它们必须在调整大小时释放它们的SparsityPattern才能被销毁。

    const unsigned int n_levels = triangulation.n_levels();

    mg_interface_matrices.resize(0, n_levels - 1);
    mg_matrices.resize(0, n_levels - 1);
    mg_sparsity_patterns.resize(0, n_levels - 1);
    mg_interface_sparsity_patterns.resize(0, n_levels - 1);

// 现在，我们必须在每个级别上提供一个矩阵。为此，我们首先使用 MGTools::make_sparsity_pattern 函数在每个层次上生成一个初步的压缩稀疏模式（关于这个主题的更多信息，请参见 @ref Sparsity 模块），然后将其复制到我们真正想要的那一个。下一步是用拟合的稀疏度模式初始化接口矩阵。

// 值得指出的是，界面矩阵只包含位于较粗和较细的网格之间的自由度的条目。因此，它们甚至比我们的多网格层次结构中的各个层次的矩阵还要稀疏。因此，我们使用一个专门为此目的而建立的函数来生成它。

    for (unsigned int level = 0; level < n_levels; ++level)
      {
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_sparsity_pattern(dof_handler, dsp, level);

          mg_sparsity_patterns[level].copy_from(dsp);
          mg_matrices[level].reinit(mg_sparsity_patterns[level]);
        }
        {
          DynamicSparsityPattern dsp(dof_handler.n_dofs(level),
                                     dof_handler.n_dofs(level));
          MGTools::make_interface_sparsity_pattern(dof_handler,
                                                   mg_constrained_dofs,
                                                   dsp,
                                                   level);
          mg_interface_sparsity_patterns[level].copy_from(dsp);
          mg_interface_matrices[level].reinit(
            mg_interface_sparsity_patterns[level]);
        }
      }
  }
// @sect4{LaplaceProblem::cell_worker}

// cell_worker函数用于在给定的单元上组装矩阵和右手边。这个函数用于活动单元生成system_matrix，并在每个层次上建立层次矩阵。

// 注意，当从assemble_multigrid()调用时，我们也会组装一个右手边，尽管它没有被使用。

  template <int dim>
  template <class Iterator>
  void LaplaceProblem<dim>::cell_worker(const Iterator &  cell,
                                        ScratchData<dim> &scratch_data,
                                        CopyData &        copy_data)
  {
    FEValues<dim> &fe_values = scratch_data.fe_values;
    fe_values.reinit(cell);

    const unsigned int dofs_per_cell = fe_values.get_fe().n_dofs_per_cell();
    const unsigned int n_q_points    = fe_values.get_quadrature().size();

    copy_data.reinit(cell, dofs_per_cell);

    const std::vector<double> &JxW = fe_values.get_JxW_values();

    for (unsigned int q = 0; q < n_q_points; ++q)
      {
        const double coefficient =
          (fe_values.get_quadrature_points()[q][0] < 0.0) ? 1.0 : 0.1;

        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          {
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              {
                copy_data.cell_matrix(i, j) +=
                  coefficient *
                  (fe_values.shape_grad(i, q) * fe_values.shape_grad(j, q)) *
                  JxW[q];
              }
            copy_data.cell_rhs(i) += 1.0 * fe_values.shape_value(i, q) * JxW[q];
          }
      }
  }

//  @sect4{LaplaceProblem::assemble_system}

// 下面的函数将线性系统集合在网格的活动单元上。为此，我们向Mesh_loop()函数传递两个lambda函数。cell_worker函数重定向到同名的类成员函数，而copyer是这个函数特有的，它使用约束条件将本地矩阵和向量复制到相应的全局矩阵。

  template <int dim>
  void LaplaceProblem<dim>::assemble_system()
  {
    MappingQ1<dim> mapping;

    auto cell_worker =
      [&](const typename DoFHandler<dim>::active_cell_iterator &cell,
          ScratchData<dim> &                                    scratch_data,
          CopyData &                                            copy_data) {
        this->cell_worker(cell, scratch_data, copy_data);
      };

    auto copier = [&](const CopyData &cd) {
      this->constraints.distribute_local_to_global(cd.cell_matrix,
                                                   cd.cell_rhs,
                                                   cd.local_dof_indices,
                                                   system_matrix,
                                                   system_rhs);
    };

    const unsigned int n_gauss_points = degree + 1;

    ScratchData<dim> scratch_data(mapping,
                                  fe,
                                  n_gauss_points,
                                  update_values | update_gradients |
                                    update_JxW_values |
                                    update_quadrature_points);

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          CopyData(),
                          MeshWorker::assemble_own_cells);
  }
// @sect4{LaplaceProblem::assemble_multigrid}

// 下一个函数是建立矩阵，定义每一层网格上的多网格方法。集成的核心与上面的相同，但是下面的循环将遍历所有已存在的单元，而不仅仅是活动的单元，并且必须将结果输入正确的层矩阵。幸运的是，MeshWorker对我们隐藏了大部分的内容，因此这个函数和之前的函数的区别只在于汇编器的设置和循环中的不同迭代器。

// 我们为每个层次生成一个AffineConstraints对象，其中包含边界和界面道夫作为约束条目。然后，相应的对象被用来生成层次矩阵。

  template <int dim>
  void LaplaceProblem<dim>::assemble_multigrid()
  {
    MappingQ1<dim>     mapping;
    const unsigned int n_levels = triangulation.n_levels();

    std::vector<AffineConstraints<double>> boundary_constraints(n_levels);
    for (unsigned int level = 0; level < n_levels; ++level)
      {
        IndexSet dofset;
        DoFTools::extract_locally_relevant_level_dofs(dof_handler,
                                                      level,
                                                      dofset);
        boundary_constraints[level].reinit(dofset);
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_refinement_edge_indices(level));
        boundary_constraints[level].add_lines(
          mg_constrained_dofs.get_boundary_indices(level));
        boundary_constraints[level].close();
      }

    auto cell_worker =
      [&](const typename DoFHandler<dim>::level_cell_iterator &cell,
          ScratchData<dim> &                                   scratch_data,
          CopyData &                                           copy_data) {
        this->cell_worker(cell, scratch_data, copy_data);
      };

    auto copier = [&](const CopyData &cd) {
      boundary_constraints[cd.level].distribute_local_to_global(
        cd.cell_matrix, cd.local_dof_indices, mg_matrices[cd.level]);

      const unsigned int dofs_per_cell = cd.local_dof_indices.size();

// 接口条目在填充mg_matrices[cd.level]时被上面的boundary_constraints对象所忽略。相反，我们手动将这些条目复制到当前级别的界面矩阵中。

      for (unsigned int i = 0; i < dofs_per_cell; ++i)
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
          if (mg_constrained_dofs.is_interface_matrix_entry(
                cd.level, cd.local_dof_indices[i], cd.local_dof_indices[j]))
            {
              mg_interface_matrices[cd.level].add(cd.local_dof_indices[i],
                                                  cd.local_dof_indices[j],
                                                  cd.cell_matrix(i, j));
            }
    };

    const unsigned int n_gauss_points = degree + 1;

    ScratchData<dim> scratch_data(mapping,
                                  fe,
                                  n_gauss_points,
                                  update_values | update_gradients |
                                    update_JxW_values |
                                    update_quadrature_points);

    MeshWorker::mesh_loop(dof_handler.begin_mg(),
                          dof_handler.end_mg(),
                          cell_worker,
                          copier,
                          scratch_data,
                          CopyData(),
                          MeshWorker::assemble_own_cells);
  }

//  @sect4{LaplaceProblem::solve}

// 这是另外一个在支持多栅求解器（或者说，事实上，我们使用多栅方法的前提条件）方面有明显不同的函数。

// 让我们从建立多层次方法的两个组成部分开始：层次间的转移运算器和最粗层次上的求解器。在有限元方法中，转移算子来自所涉及的有限元函数空间，通常可以用独立于所考虑问题的通用方式计算。在这种情况下，我们可以使用MGTransferPrebuilt类，给定最终线性系统的约束和MGConstrainedDoFs对象，该对象知道每个层次的边界条件和不同细化层次之间接口的自由度，可以从具有层次自由度的DoFHandler对象中建立这些转移操作的矩阵。

// 下面几行的第二部分是关于粗略网格求解器的。由于我们的粗网格确实非常粗，我们决定采用直接求解器（最粗层次矩阵的Householder分解），即使其实现不是特别复杂。如果我们的粗网格比这里的5个单元多得多，那么这里显然需要更合适的东西。

  template <int dim>
  void LaplaceProblem<dim>::solve()
  {
    MGTransferPrebuilt<Vector<double>> mg_transfer(mg_constrained_dofs);
    mg_transfer.build(dof_handler);

    FullMatrix<double> coarse_matrix;
    coarse_matrix.copy_from(mg_matrices[0]);
    MGCoarseGridHouseholder<double, Vector<double>> coarse_grid_solver;
    coarse_grid_solver.initialize(coarse_matrix);

// 多级求解器或预处理器的下一个组成部分是，我们需要在每一级上设置平滑器。这方面常见的选择是使用松弛方法的应用（如SOR、Jacobi或Richardson方法）或求解器方法的少量迭代（如CG或GMRES）。 mg::SmootherRelaxation 和MGSmootherPrecondition类为这两种平滑器提供支持。这里，我们选择应用单一的SOR迭代。为此，我们定义一个适当的别名，然后设置一个平滑器对象。

// 最后一步是用我们的水平矩阵初始化平滑器对象，并设置一些平滑参数。 initialize() 函数可以有选择地接受额外的参数，这些参数将被传递给每一级的平滑器对象。在目前SOR平滑器的情况下，这可能包括一个松弛参数。然而，我们在这里将这些参数保留为默认值。对 set_steps() 的调用表明我们将在每个级别上使用两个前平滑步骤和两个后平滑步骤；为了在不同级别上使用可变数量的平滑器步骤，可以在对 mg_smoother 对象的构造函数调用中设置更多选项。

// 最后一步的结果是我们使用SOR方法作为平滑器的事实

// --这不是对称的

// 但我们在下面使用共轭梯度迭代（需要对称的预处理），我们需要让多级预处理确保我们得到一个对称的算子，即使是非对称的平滑器。

    using Smoother = PreconditionSOR<SparseMatrix<double>>;
    mg::SmootherRelaxation<Smoother, Vector<double>> mg_smoother;
    mg_smoother.initialize(mg_matrices);
    mg_smoother.set_steps(2);
    mg_smoother.set_symmetric(true);

// 下一个准备步骤是，我们必须将我们的水平和接口矩阵包裹在一个具有所需乘法函数的对象中。我们将为从粗到细的接口对象创建两个对象，反之亦然；多网格算法将在后面的操作中使用转置运算器，允许我们用已经建立的矩阵初始化该运算器的上下版本。

    mg::Matrix<Vector<double>> mg_matrix(mg_matrices);
    mg::Matrix<Vector<double>> mg_interface_up(mg_interface_matrices);
    mg::Matrix<Vector<double>> mg_interface_down(mg_interface_matrices);

// 现在，我们准备设置V型循环算子和多级预处理程序。

    Multigrid<Vector<double>> mg(
      mg_matrix, coarse_grid_solver, mg_transfer, mg_smoother, mg_smoother);
    mg.set_edge_matrices(mg_interface_down, mg_interface_up);

    PreconditionMG<dim, Vector<double>, MGTransferPrebuilt<Vector<double>>>
      preconditioner(dof_handler, mg, mg_transfer);

// 有了这一切，我们终于可以用通常的方法来解决这个线性系统了。

    SolverControl            solver_control(1000, 1e-12);
    SolverCG<Vector<double>> solver(solver_control);

    solution = 0;

    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    std::cout << "   Number of CG iterations: " << solver_control.last_step()
              << "\n"
              << std::endl;
    constraints.distribute(solution);
  }

//  @sect4{Postprocessing}

// 以下两个函数在计算出解决方案后对其进行后处理。特别是，第一个函数在每个周期开始时细化网格，第二个函数在每个周期结束时输出结果。这些函数与  step-6  中的函数几乎没有变化。

  template <int dim>
  void LaplaceProblem<dim>::refine_grid()
  {
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());

    KellyErrorEstimator<dim>::estimate(
      dof_handler,
      QGauss<dim - 1>(degree + 2),
      std::map<types::boundary_id, const Function<dim> *>(),
      solution,
      estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    estimated_error_per_cell,
                                                    0.3,
                                                    0.03);
    triangulation.execute_coarsening_and_refinement();
  }

  template <int dim>
  void LaplaceProblem<dim>::output_results(const unsigned int cycle) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "solution");
    data_out.build_patches();

    std::ofstream output("solution-" + std::to_string(cycle) + ".vtk");
    data_out.write_vtk(output);
  }
// @sect4{LaplaceProblem::run}

// 和上面的几个函数一样，这几乎是对  step-6  中相应函数的复制。唯一的区别是对 assemble_multigrid 的调用，它负责形成我们在多网格方法中需要的每一层的矩阵。

  template <int dim>
  void LaplaceProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 8; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_ball(triangulation);
            triangulation.refine_global(2);
          }
        else
          refine_grid();

        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;

        setup_system();

        assemble_system();
        assemble_multigrid();

        solve();
        output_results(cycle);
      }
  }
} // namespace Step16
// @sect3{The main() function}

// 这又是与 step-6 中相同的函数。

int main()
{
  try
    {
      using namespace Step16;

      LaplaceProblem<2> laplace_problem(1);
      laplace_problem.run();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/optimization.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../rand.h"

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.max_cost_assignment");

// ----------------------------------------------------------------------------------------

    std::vector<std::vector<long> > permutations (
        matrix<long,1,0> vals
    )
    {
        if (vals.size() == 0)
        {
            return std::vector<std::vector<long> >();
        }
        else if (vals.size() == 1)
        {
            return std::vector<std::vector<long> >(1,std::vector<long>(1,vals(0)));
        }

        std::vector<std::vector<long> > temp;

        for (long i = 0; i < vals.size(); ++i)
        {
            const std::vector<std::vector<long> >& res = permutations(remove_col(vals,i));

            for (unsigned long j = 0; j < res.size(); ++j)
            {
                temp.resize(temp.size()+1);
                std::vector<long>& part = temp.back();
                part.push_back(vals(i));
                part.insert(part.end(), res[j].begin(), res[j].end());
            }
        }

        return temp;
    }

// ----------------------------------------------------------------------------------------

    template <typename T>
    std::vector<long> brute_force_max_cost_assignment (
        matrix<T> cost
    )
    {
        if (cost.size() == 0)
            return std::vector<long>();

        const std::vector<std::vector<long> >& perms = permutations(range(0,cost.nc()-1));

        T best_cost = std::numeric_limits<T>::min();
        unsigned long best_idx = 0;
        for (unsigned long i = 0; i < perms.size(); ++i)
        {
            const T temp = assignment_cost(cost, perms[i]);
            if (temp > best_cost)
            {
                best_idx = i;
                best_cost = temp;
            }
        }

        return perms[best_idx];
    }

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

    class test_max_cost_assignment : public tester
    {
    public:
        test_max_cost_assignment (
        ) :
            tester ("test_max_cost_assignment",
                    "Runs tests on the max_cost_assignment function.")
        {}

        dlib::rand rnd;

        template <typename T>
        void test_hungarian()
        {
            long size = rnd.get_random_32bit_number()%7;
            long range = rnd.get_random_32bit_number()%100;
            matrix<T> cost = matrix_cast<T>(randm(size,size,rnd)*range) - range/2;

            // use a uniform cost matrix sometimes
            if ((rnd.get_random_32bit_number()%100) == 0)
                cost = rnd.get_random_32bit_number()%100;

            // negate the cost matrix every now and then
            if ((rnd.get_random_32bit_number()%100) == 0)
                cost = -cost;

            std::vector<long> assign = brute_force_max_cost_assignment(cost);
            T true_eval = assignment_cost(cost, assign);
            assign = max_cost_assignment(cost);
            DLIB_TEST(assignment_cost(cost,assign) == true_eval);
            assign = max_cost_assignment(matrix_cast<signed char>(cost));
            DLIB_TEST(assignment_cost(cost,assign) == true_eval);

            cost = matrix_cast<T>(randm(size,size,rnd)*range);
            assign = brute_force_max_cost_assignment(cost);
            true_eval = assignment_cost(cost, assign);
            assign = max_cost_assignment(cost);
            DLIB_TEST(assignment_cost(cost,assign) == true_eval);
            assign = max_cost_assignment(matrix_cast<unsigned char>(cost));
            DLIB_TEST(assignment_cost(cost,assign) == true_eval);
            assign = max_cost_assignment(matrix_cast<typename unsigned_type<T>::type>(cost));
            DLIB_TEST(assignment_cost(cost,assign) == true_eval);
        }

        void perform_test (
        )
        {
            for (long i = 0; i < 1000; ++i)
            {
                if ((i%100) == 0)
                    print_spinner();

                test_hungarian<short>();
                test_hungarian<int>();
                test_hungarian<long>();
                test_hungarian<int64>();
            }
        }
    } a;

}


#include "gtest/gtest.h"
#include <Eigen/Core>
#include "Face2D.h"
#include <algorithm>

namespace {
  using namespace Geotree;

  class Loop2DTest : public ::testing::Test {
  protected:
  };

  TEST_F(Loop2DTest, Basic)
  {
    Loop2D loop;

    Point2D p0 = {0,0};
    Point2D p1 = {1,0};
    Point2D p2 = {0,1};

    loop.points.push_back(p0);
    loop.points.push_back(p1);
    loop.points.push_back(p2);

    Point2D p3 = {0.5,0.5};

    EXPECT_TRUE(loop.contains(p3));
  }
}


#include <stan/math/prim/mat.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>

using Eigen::Dynamic;
using Eigen::Matrix;
using std::vector;

TEST(ProbDistributionsMultiNormal, NotVectorized) {
  Matrix<double, Dynamic, 1> y(3, 1);
  y << 2.0, -2.0, 11.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double, Dynamic, Dynamic> Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;
  EXPECT_FLOAT_EQ(-11.73908, stan::math::multi_normal_log(y, mu, Sigma));
}

TEST(ProbDistributionsMultiNormal, Vectorized) {
  vector<Matrix<double, Dynamic, 1> > vec_y(2);
  vector<Matrix<double, 1, Dynamic> > vec_y_t(2);
  Matrix<double, Dynamic, 1> y(3);
  Matrix<double, 1, Dynamic> y_t(3);
  y << 2.0, -2.0, 11.0;
  vec_y[0] = y;
  vec_y_t[0] = y;
  y << 4.0, -2.0, 1.0;
  vec_y[1] = y;
  vec_y_t[1] = y;
  y_t = y;

  vector<Matrix<double, Dynamic, 1> > vec_mu(2);
  vector<Matrix<double, 1, Dynamic> > vec_mu_t(2);
  Matrix<double, Dynamic, 1> mu(3);
  Matrix<double, 1, Dynamic> mu_t(3);
  mu << 1.0, -1.0, 3.0;
  vec_mu[0] = mu;
  vec_mu_t[0] = mu;
  mu << 2.0, -1.0, 4.0;
  vec_mu[1] = mu;
  vec_mu_t[1] = mu;
  mu_t = mu;

  Matrix<double, Dynamic, Dynamic> Sigma(3, 3);
  Sigma << 10.0, -3.0, 0.0, -3.0, 5.0, 0.0, 0.0, 0.0, 5.0;

  // y and mu vectorized
  EXPECT_FLOAT_EQ(-11.928077 - 6.5378327,
                  stan::math::multi_normal_log(vec_y, vec_mu, Sigma));
  EXPECT_FLOAT_EQ(-11.928077 - 6.5378327,
                  stan::math::multi_normal_log(vec_y_t, vec_mu, Sigma));
  EXPECT_FLOAT_EQ(-11.928077 - 6.5378327,
                  stan::math::multi_normal_log(vec_y, vec_mu_t, Sigma));
  EXPECT_FLOAT_EQ(-11.928077 - 6.5378327,
                  stan::math::multi_normal_log(vec_y_t, vec_mu_t, Sigma));

  // y vectorized
  EXPECT_FLOAT_EQ(-10.44027 - 6.537833,
                  stan::math::multi_normal_log(vec_y, mu, Sigma));
  EXPECT_FLOAT_EQ(-10.44027 - 6.537833,
                  stan::math::multi_normal_log(vec_y_t, mu, Sigma));
  EXPECT_FLOAT_EQ(-10.44027 - 6.537833,
                  stan::math::multi_normal_log(vec_y, mu_t, Sigma));
  EXPECT_FLOAT_EQ(-10.44027 - 6.537833,
                  stan::math::multi_normal_log(vec_y_t, mu_t, Sigma));

  // mu vectorized
  EXPECT_FLOAT_EQ(-6.26954 - 6.537833,
                  stan::math::multi_normal_log(y, vec_mu, Sigma));
  EXPECT_FLOAT_EQ(-6.26954 - 6.537833,
                  stan::math::multi_normal_log(y_t, vec_mu, Sigma));
  EXPECT_FLOAT_EQ(-6.26954 - 6.537833,
                  stan::math::multi_normal_log(y, vec_mu_t, Sigma));
  EXPECT_FLOAT_EQ(-6.26954 - 6.537833,
                  stan::math::multi_normal_log(y_t, vec_mu_t, Sigma));
}
TEST(ProbDistributionsMultiNormal, Sigma) {
  Matrix<double, Dynamic, 1> y(2, 1);
  y << 2.0, -2.0;
  Matrix<double, Dynamic, 1> mu(2, 1);
  mu << 1.0, -1.0;
  Matrix<double, Dynamic, Dynamic> Sigma(2, 2);
  Sigma << 9.0, -3.0, -3.0, 4.0;
  EXPECT_NO_THROW(stan::math::multi_normal_log(y, mu, Sigma));

  // non-symmetric
  Sigma(0, 1) = -2.5;
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal, Mu) {
  Matrix<double, Dynamic, 1> y(3, 1);
  y << 2.0, -2.0, 11.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double, Dynamic, Dynamic> Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;
  EXPECT_NO_THROW(stan::math::multi_normal_log(y, mu, Sigma));

  mu(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = -std::numeric_limits<double>::infinity();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal, MultiNormalOneRow) {
  Matrix<double, 1, Dynamic> y(1, 3);
  y << 2.0, -2.0, 11.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double, Dynamic, Dynamic> Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;
  EXPECT_FLOAT_EQ(-11.73908, stan::math::multi_normal_log(y, mu, Sigma));
}

TEST(ProbDistributionsMultiNormal, SigmaMultiRow) {
  Matrix<double, 1, Dynamic> y(1, 2);
  y << 2.0, -2.0;
  Matrix<double, Dynamic, 1> mu(2, 1);
  mu << 1.0, -1.0;
  Matrix<double, Dynamic, Dynamic> Sigma(2, 2);
  Sigma << 9.0, -3.0, -3.0, 4.0;
  EXPECT_NO_THROW(stan::math::multi_normal_log(y, mu, Sigma));

  // non-symmetric
  Sigma(0, 1) = -2.5;
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  Matrix<double, Dynamic, 1> z(2, 1);

  // wrong dimensions
  z << 2.0, -2.0;
  EXPECT_THROW(stan::math::multi_normal_log(z, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal, MuMultiRow) {
  Matrix<double, 1, Dynamic> y(1, 3);
  y << 2.0, -2.0, 11.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 1.0, -1.0, 3.0;
  Matrix<double, Dynamic, Dynamic> Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;
  EXPECT_NO_THROW(stan::math::multi_normal_log(y, mu, Sigma));

  mu(0) = std::numeric_limits<double>::infinity();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = -std::numeric_limits<double>::infinity();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
  mu(0) = std::numeric_limits<double>::quiet_NaN();
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma), std::domain_error);
}
TEST(ProbDistributionsMultiNormal, SizeMismatch) {
  Matrix<double, 1, Dynamic> y(1, 3);
  y << 2.0, -2.0, 11.0;
  Matrix<double, Dynamic, 1> mu(2, 1);
  mu << 1.0, -1.0;
  Matrix<double, Dynamic, Dynamic> Sigma(2, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0;
  EXPECT_THROW(stan::math::multi_normal_log(y, mu, Sigma),
               std::invalid_argument);
}

TEST(ProbDistributionsMultiNormal, error_check) {
  boost::random::mt19937 rng;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 2.0, -2.0, 11.0;

  Matrix<double, Dynamic, Dynamic> sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  EXPECT_NO_THROW(stan::math::multi_normal_rng(mu, sigma, rng));

  mu << stan::math::positive_infinity(), -2.0, 11.0;
  EXPECT_THROW(stan::math::multi_normal_rng(mu, sigma, rng), std::domain_error);

  mu << 2.0, -2.0, 11.0;
  sigma << 9.0, -3.0, 0.0, 3.0, 4.0, 0.0, -2.0, 1.0, 3.0;
  EXPECT_THROW(stan::math::multi_normal_rng(mu, sigma, rng), std::domain_error);
}

TEST(ProbDistributionsMultiNormal, marginalOneChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double, Dynamic, Dynamic> sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 2.0, -2.0, 11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(2.0, 3.0);
  boost::math::chi_squared mydist(K - 1);

  double loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu, sigma, rng);
    int i = 0;
    while (i < K - 1 && a(0) > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsMultiNormal, marginalTwoChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double, Dynamic, Dynamic> sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 2.0, -2.0, 11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(-2.0, 2.0);
  boost::math::chi_squared mydist(K - 1);

  double loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu, sigma, rng);
    int i = 0;
    while (i < K - 1 && a(1) > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsMultiNormal, marginalThreeChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  Matrix<double, Dynamic, Dynamic> sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 16.0;
  Matrix<double, Dynamic, 1> mu(3, 1);
  mu << 2.0, -2.0, 11.0;
  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(11.0, 4.0);
  boost::math::chi_squared mydist(K - 1);

  double loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, i * std::pow(K, -1.0));

  int count = 0;
  int bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N / K;
  }
  Eigen::VectorXd a(mu.rows());
  while (count < N) {
    a = stan::math::multi_normal_rng(mu, sigma, rng);
    int i = 0;
    while (i < K - 1 && a(2) > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;
  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(multiNormalRng, nonPosDefErrorTest) {
  using stan::math::multi_normal_rng;
  Eigen::MatrixXd S(2, 2);
  S << 0, 1, 1, 0;  // not pos definite
  Eigen::VectorXd mu(2);
  mu << 1, 2;
  boost::random::mt19937 rng;
  EXPECT_THROW(multi_normal_rng(mu, S, rng), std::domain_error);
}


/// \file
/// Maintainer: Felice Serena
///
///

#include "statistical_outlier_detection.h"
#include "uniform_grid.h"
#include <set>

#include <boost/test/unit_test.hpp>

namespace utf = boost::unit_test;

using namespace Eigen;
using namespace MouseTrack;

BOOST_AUTO_TEST_CASE(outlier_2d) {
  typedef UniformGrid2d UG;
  UG::PointList all(2, 6);
  all.col(0) = Vector2d(0.0, 0.0);
  all.col(1) = Vector2d(2.0, 2.0);
  all.col(2) = Vector2d(0.1, 0.0);
  all.col(3) = Vector2d(-0.1, 0.0);
  all.col(4) = Vector2d(0.0, 0.1);
  all.col(5) = Vector2d(0.0, -0.1);

  std::multiset<PointIndex> expected;
  expected.insert(1);

  UG oracle(10.0, 0.05);
  oracle.compute(all);

  std::vector<PointIndex> result =
      statisticalOutlierDetection<UG::PointList, UG::Precision>(all, &oracle,
                                                                2.0, 10);

  std::multiset<PointIndex> received(result.begin(), result.end());

  BOOST_CHECK_MESSAGE(
      expected.size() == received.size(),
      "Expected and received sets have different cardinalities.");
  BOOST_CHECK_MESSAGE(
      expected == received,
      "Expected and received set do not contain same elements.");
}


#include <boost/bind.hpp>

#include <ql/time/date.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/integrals/segmentintegral.hpp>

#include <calibrator/models/shortrate/twofactormodels/generalg2.hpp>

namespace HJCALIBRATOR
{
	GeneralizedG2::GeneralizedG2( boost::shared_ptr<Gaussian2FactorDynamics> dynamics,
								  Real integralSignificance,
								  boost::shared_ptr<Integrator> integrator )
		: TwoFactorModel( 5 )
		, AffineModel()
		, TermStructureConsistentModel( dynamics->termStructure() )
		, a_( arguments_[0] ), sigma_( arguments_[1] )
		, b_( arguments_[2] ), eta_( arguments_[3] )
		, rho_( arguments_[4] )
		, integralSignificance_( integralSignificance )
		, integrator_( integrator )
		, dynamics_( dynamics )
	{
		a_ = dynamics->a(0);
		b_ = dynamics->a(1);
		sigma_ = dynamics->sigma(0);
		eta_ = dynamics->sigma(1);
		rho_ = dynamics->rho( 0, 1 );

		generateArguments();

		registerWith( dynamics->termStructure() );
	}

	void GeneralizedG2::generateArguments()
	{
		dynamics_->a( a_, 0 );
		dynamics_->sigma( sigma_, 0 );
		dynamics_->a( b_, 1 );
		dynamics_->sigma( eta_, 1 );
		dynamics_->rho( rho_, 1, 0 );
	}

	Real GeneralizedG2::A( Time t, Time T ) const
	{
		return dynamics_->A( t, T );
	}

	Real GeneralizedG2::discountBond( Time now,
									  Time maturity,
									  Array factors ) const
	{
		Real x = factors[0];
		Real y = factors[1];

		Real Bx = dynamics_->B( 0, now, maturity );
		Real By = dynamics_->B( 1, now, maturity );

		return A( now, maturity ) * exp( - Bx * x - By * y );
	}

	Real GeneralizedG2::discountBondOption( Option::Type type,
											Real strike,
											Time maturity,
											Time bondMaturity ) const
	{
		auto dynamics = dynamics_;

		Real Bx = dynamics->B( 0, maturity, bondMaturity );
		Real By = dynamics->B( 1, maturity, bondMaturity );
		Real rho = dynamics->rho( 0, 1 )(0.0);

		Real Vpratio = 0;

		auto integrand00 = [maturity, dynamics]( Time u )
		{
			Real sigma = dynamics->sigma( 0 )(u);
			Real E = dynamics->E( 0, u, maturity );

			return sigma * sigma / E / E;
		};

		Vpratio += Bx * By * integrator_->operator()( integrand00, 0, maturity );

		auto integrand11 = [maturity, dynamics]( Time u )
		{
			Real sigma = dynamics->sigma( 1 )(u);
			Real E = dynamics->E( 1, u, maturity );

			return sigma * sigma / E / E;
		};

		Vpratio += Bx * By * integrator_->operator()( integrand11, 0, maturity );

		auto integrand01 = [maturity, dynamics]( Time u )
		{
			return dynamics->sigma( 0 )(u) * dynamics->sigma( 1 )(u)
				/ dynamics->E( 0, u, maturity ) / dynamics->E( 1, u, maturity );
		};

		Vpratio += 2 * Bx * Bx * rho * integrator_->operator()( integrand01, 0, maturity );

		Real stdDev = sqrt( std::max( Vpratio, 0.0 ) );

		Real f = termStructure()->discount( bondMaturity );
		Real k = termStructure()->discount( maturity )*strike;

		return blackFormula( type, k, f, stdDev );
	}

	Real GeneralizedG2::discountBondOption( Option::Type type,
											Real strike,
											Time maturity,
											Time bondStart,
											Time bondMaturity ) const
	{
		return discountBondOption( type, strike, bondStart, bondMaturity );
	}

	// Brigo Ch. 4.2
	Real GeneralizedG2::swaption( const Swaption::arguments& arg, Real strike ) const
	{
		Date settlement = termStructure()->referenceDate();
		DayCounter dayCounter = termStructure()->dayCounter();
		Time T = dayCounter.yearFraction( settlement,
											  arg.floatingResetDates[0] );
		Real w = (arg.type == VanillaSwap::Payer ? 1 : -1);

		std::vector<Time> t;
		for ( auto fixedPayDate : arg.fixedPayDates )
		{
			t.push_back( dayCounter.yearFraction( settlement,
													fixedPayDate ) );
		}
		Size N_timestep = t.size();

		Array cA( N_timestep );
		Array Bx( N_timestep );
		Array By( N_timestep );

		for ( Size i = 0; i < N_timestep; i++ )
		{
			Time tau_i = i == 0 ? t[i] - T : t[i] - t[i - 1];
			Real c = i == N_timestep - 1 ? 1 + strike * tau_i : strike * tau_i;
			cA[i] = c * dynamics_->A( T, t[i] );
			Bx[i] = dynamics_->B( 0, T, t[i] );
			By[i] = dynamics_->B( 1, T, t[i]);
		}

		Real mu_x = dynamics_->meanTforward( 0, T, 0, T );
		Real mu_y = dynamics_->meanTforward( 1, T, 0, T );
		Real sigma_x = sqrt(dynamics_->variance( 0, 0, 0, T ));
		Real sigma_y = sqrt(dynamics_->variance( 1, 1, 0, T ));
		Real rho = dynamics_->rho( 0, 1 )(0.0);
		Real var = dynamics_->variance( 0, 1, 0, T );
		Real rho_xy = rho * var / sigma_x / sigma_y;
		Real rhosqrt = sqrt( 1 - rho_xy * rho_xy );

		auto integrand = [&, N_timestep, w, mu_x, mu_y, sigma_x, sigma_y, rho_xy, rhosqrt]( Real x )
		{
			Real dev = (x - mu_x) / sigma_x;

			Array lambda( N_timestep );
			Array kappa( N_timestep );
			for ( Size i = 0; i < N_timestep; i++ )
			{
				lambda[i] = cA[i] * exp( -Bx[i] * x );
				kappa[i] = -By[i] * (mu_y - 0.5 * rhosqrt * rhosqrt * sigma_y * sigma_y * By[i]
											+ rho_xy * sigma_y * (x - mu_x) / sigma_x);
			}

			auto hyperplane = [N_timestep, &lambda, &By]( Real y )
			{
				Real value = 1.;
				for ( Size i = 0; i < N_timestep; i++ )
				{
					Real val = lambda[i] * exp( -By[i] * y );
					value -= val;
				}

				return value;
			};

			Brent solver;
			solver.setMaxEvaluations( 1000 );
			Real ybar = solver.solve( hyperplane, 1e-6, 0.00, -100.0, 100.0 );

			Real h1 = (ybar - mu_y) / (sigma_y * rhosqrt)
				- rho_xy * (x - mu_x) / (sigma_x * rhosqrt);

			CumulativeNormalDistribution Phi;
			Real val = Phi( -w * h1 );
			for ( Size i = 0; i < N_timestep; i++ )
			{
				Real h2 = h1 + By[i] * sigma_y * rhosqrt;

				val -= lambda[i] * exp( kappa[i] ) * Phi( -w * h2 );
			}

			return exp( -0.5*dev*dev ) * val;
		};

		Real N = arg.nominal;
		Real P0T = termStructure()->discount( T );
		Real upper = mu_x + integralSignificance_ *sigma_x;
		Real lower = mu_x - integralSignificance_ *sigma_x;

		Real val = N * w * P0T * integrator_->operator()( integrand, lower, upper ) / sqrt( 2. * M_PI ) / sigma_x;
		return val;
	}
}


/*
 * NormalVectorsFilter.hpp
 *
 *  Created on: May 05, 2015
 *      Author: Peter Fankhauser, Martin Wermelinger
 *   Institute: ETH Zurich, ANYbotics
 */

#pragma once

#include <filters/filter_base.hpp>
#include <grid_map_core/grid_map_core.hpp>

#include <Eigen/Core>
#include <string>

namespace grid_map {

/*!
 * Compute the normal vectors of a layer in a map.
 */
template <typename T>
class NormalVectorsFilter : public filters::FilterBase<T> {
 public:
  /*!
   * Constructor
   */
  NormalVectorsFilter();

  /*!
   * Destructor.
   */
  ~NormalVectorsFilter() override;

  /*!
   * Configures the filter from parameters on the Parameter Server.
   * This comprehend in order:
   *    1) algorithm, used for choosing between area and raster algorithm (default=area)
   *    2)If algorithm is not raster estimationRadius_ is read an a basic check on its sign is made.
   *      If something is wrong it switches to raster algorithm.
   *    3) parallelization_enabled, used for choosing between serial and parallel algorithm (default=false)
   *    4) thread_count, used to set the number of threads to be used in parallelization is enabled (default=auto)
   * The parallelization_enabled and algorithm parameters allow to choose between the 4 different methods { AreaSerial, AreaParallel,
   * RasterSerial, RasterParallel }
   *    4) normal_vector_positive_axis, used to define the upward positive direction for the normals
   *    5) input_layer, defines in which layer of the grid map lie the information needed (usually elevation or elevation_filtered)
   *    6) output_layers_prefix, defines the prefix for the new 3 layers (x,y,z) that will define the normal vectors
   * Those parameters have to be written in a .yaml file that will be processed as a sequence of filters.
   * An example can be seen in grid_map_demos/config/normal_filter_comparison.yaml.
   */
  bool configure() override;

  /*!
   * Compute the normal vectors of a layer in a map and
   * saves it as additional grid map layer.
   * @param mapIn grid map containing the layer for which the normal vectors are computed for.
   * @param mapOut grid map containing mapIn and the new layers for the normal vectors.
   */
  bool update(const T& mapIn, T& mapOut) override;

 private:
  /*!
   * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function.
   * This function makes use of the area method and is the serial version of such normal vector computation using a
   * simple for cycle to iterate over the whole map.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithAreaSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at each point of the input layer by using the areaSingleNormalComputation function.
   * This function makes use of the area method and is the parallel version of such normal vector computation using
   * a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithAreaParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using points within
   * a circle of specified radius.
   *
   * The eigen decomposition of the covariance matrix (3x3) of all data points is used to establish the normal direction.
   * Four cases can be identified when the eigenvalues are ordered in ascending order:
   *    1) The data is in a cloud -> all eigenvalues are non-zero
   *    2) The data is on a plane -> The first eigenvalue is zero
   *    3) The data is on a line -> The first two eigenvalues are zero.
   *    4) The data is in one point -> All eigenvalues are zero
   *
   * Only case 1 & 2 provide enough information the establish a normal direction.
   * The degenerate cases (3 or 4) are identified by checking if the second eigenvalue is zero.
   *
   * The numerical threshold (1e-8) for the eigenvalue being zero is given by the accuracy of the decomposition, as reported by Eigen:
   * https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html
   *
   * Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   * @param index: Index of point in the grid map for which this function calculates the normal vector.
   */
  void areaSingleNormalComputation(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix,
                                          const grid_map::Index& index);
  /*!
   * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function.
   * This function makes use of the raster method and is the serial version of such normal vector computation using a
   * simple for cycle to iterate over the whole map.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithRasterSerial(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at each point of the input layer by using the rasterSingleNormalComputation function.
   * This function makes use of the raster method and is the parallel version of such normal vector computation using
   * a parallel_for cycle to iterate over the whole map. The parallel_for construct is provided by Intel TBB library.
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   */
  void computeWithRasterParallel(GridMap& map, const std::string& inputLayer, const std::string& outputLayersPrefix);

  /*!
   * Estimate the normal vector at one point of the input layer, specified by the index parameter, by using neighboring points
   * around the point for which the normal vector is being calculated.
   *
   * The neighboring cells are used to linearly approximate the first derivative in both directions.
   *
   *   |T|   ^
   * |L|C|R| | x Axis
   *   |B|   |
   * <-------|
   * Y Axis
   *
   * Those values are then used to reconstruct the normal vector in the point of interest.
   * This algorithm doesn't make use of the central cell if all the neighboring point heights are present.
   * However, thanks to that, it can accommodate missing point heights, up to one in each direction, by using
   * exactly the central height value.
   * In this case, the resulting approximation, will suffer a loss of quality depending on the local surface shape.
   * This implementation skips the outermost values in order to avoid checks on the validity of height values
   * and therefore have a faster algorithm.
   *
   * Inspiration for algorithm: http://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
   *
   * Finally, the sign normal vector is correct to be in the same direction as the user defined "normal vector positive axis"
   *
   * @param map: grid map containing the layer for which the normal vectors are computed for.
   * @param inputLayer: Layer the normal vector should be computed for.
   * @param outputLayersPrefix: Output layer name prefix.
   * @param dataMap: Matrix containing the input layer of the grid map in question.
   * @param index: Index of point in the grid map for which this function calculates the normal vector.
   */
  void rasterSingleNormalComputation(GridMap& map, const std::string& outputLayersPrefix, const grid_map::Matrix& dataMap,
                                            const grid_map::Index& index);

  enum class Method { AreaSerial, AreaParallel, RasterSerial, RasterParallel };

  Method method_;

  //! Radius of submap for normal vector estimation.
  double estimationRadius_;

  //! Parameter that specifies whether to parallelize or not.
  bool parallelizationEnabled_;

  //! Parameter that specifies the number of thread used.
  int threadCount_;

  //! Normal vector positive axis.
  Eigen::Vector3d normalVectorPositiveAxis_;

  //! Input layer name.
  std::string inputLayer_;

  //! Output layer name.
  std::string outputLayersPrefix_;

  //! Grid Map Resolution.
  double gridMapResolution_;
};

}  // namespace grid_map