//	Objective: To implement the option class that is defined in the header file: AsianGeometricOption.hpp
//
// (c) Sudhansh Dua

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

using namespace std;
using namespace boost::math;

double AsianGeometricOption::CallPrice() const
{
	return ::AsianGeometricCallPrice(S, K, T, r, sig, b, type);
}

double AsianGeometricOption::PutPrice() const
{
	return ::AsianGeometricPutPrice(S, K, T, r, sig, b, type);
}

void AsianGeometricOption::init()					// Initialising all the default values
{
	//	Default values
	T = 0.25;
	r = 0.05;
	sig = 0.2;
	K = 85;
	S = 80;
	b = 0.08;

	type = "C";			//	Call option as the default

}

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

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

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

//	Constructor that accepts values
AsianGeometricOption::AsianGeometricOption(const double& S1, const double& K1, const double& T1, const double& r1, const double& sig1,
	const double& b1, const string type1) : Option(), S(S1), K(K1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

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

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

// Functions that calculate the option price
double AsianGeometricOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

// Modifier functions
void AsianGeometricOption::toggle()								//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

// Global Functions
double AsianGeometricCallPrice(const double S, const double K, const double T, const double r, const double sig, const double b, const string type)
{
	double sig_adj = sig / sqrt(3);						//	adjusted volatility
	double b_adj = 0.5 * (b - ((sig * sig) / 6));		//	adjusted cost-of-carry

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

	normal_distribution<> standard_normal(0.0, 1.0);
	return ((S * exp((b_adj - r) * T) * cdf(standard_normal, d1)) - (K * exp(-r * T) * cdf(standard_normal, d2)));
}

double AsianGeometricPutPrice(const double S, const double K, const double T, const double r, const double sig, const double b, const string type)
{
	double sig_adj = sig / sqrt(3);						//	adjusted volatility
	double b_adj = 0.5 * (b - ((sig * sig) / 6));		//	adjusted cost-of-carry

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

	normal_distribution<> standard_normal(0.0, 1.0);
	return ((K * exp(-r * T) * cdf(standard_normal, -d2)) - (S * exp((b_adj - r) * T) * cdf(standard_normal, -d1)));
}


#include "gtest/gtest.h"

#include <string>
#include <sstream>

#include <boost/numeric/ublas/io.hpp>

#include "wali/QueueWorklist.hpp"
#include "wali/wpds/WPDS.hpp"
#include "wali/wfa/WFA.hpp"
#include "wali/witness/Witness.hpp"
#include "wali/witness/WitnessWrapper.hpp"
#include "wali/domains/matrix/Matrix.hpp"
#include "wali/domains/matrix/MatrixRanker.hpp"

#include "opennwa/query/ShortWitnessVisitor.hpp"
#include "opennwa/query/PathVisitor.hpp"

#include "matrix-equal.hpp"

using namespace wali::witness;

namespace wali {
namespace domains {

namespace {

const int inf = std::numeric_limits<MinPlusIntMatrix::value_type::value_type>::max();

struct Weights1
{
    MinPlusIntMatrix::BackingMatrix
        bm_id,
        bm_initial,
        bm_loop,
        bm_step,
        bm_expected;

    ref_ptr<MinPlusIntMatrix>
        zero,
        id,
        initial,
        loop,
        step,
        expected;

    Weights1()
        : bm_id(2,2)
        , bm_initial(2,2)
        , bm_loop(2,2)
        , bm_step(2,2)
        , bm_expected(2,2)
    {
        int m_id[2][2] = {
            {   0, inf},
            { inf,   0},
        };

        int m_initial[2][2] = {
            {   1, inf},
            { inf, inf},
        };

        int m_loop[2][2] = {
            {   1,   1},
            { inf,   1},
        };

        int m_step[2][2] = {
            {   1, inf},
            { inf,   1},
        };

        int m_expected[2][2] = {
            {   2,   3},
            { inf, inf},
        };

        for (size_t i = 0; i < 2; ++i) {
            for (size_t j = 0; j < 2; ++j) {
                bm_id(i, j).set_value(m_id[i][j]);
                bm_initial(i, j).set_value(m_initial[i][j]);
                bm_loop(i, j).set_value(m_loop[i][j]);
                bm_step(i, j).set_value(m_step[i][j]);
                bm_expected(i, j).set_value(m_expected[i][j]);
            }
        }

        id       = new MinPlusIntMatrix(bm_id);
        initial  = new MinPlusIntMatrix(bm_initial);
        loop     = new MinPlusIntMatrix(bm_loop);
        step     = new MinPlusIntMatrix(bm_step);
        expected = new MinPlusIntMatrix(bm_expected);

        zero = dynamic_cast<MinPlusIntMatrix*>(id->zero().get_ptr());
    }
};

struct Keys
{
    Key start, loop, finish, top, bottom, join, middle, p, acc;

    Keys()
        : start(getKey("start"))
        , loop(getKey("loop"))
        , finish(getKey("finish"))
        , top(getKey("top"))
        , bottom(getKey("bottom"))
        , join(getKey("join"))
        , middle(getKey("middle"))
        , p(getKey("p"))
        , acc(getKey("acc"))
    {}
};

struct WpdsWfa1
{
    wpds::WPDS wpds;
    wfa::WFA wfa;

    Weights1 weights;
    Keys keys;

    WpdsWfa1(ref_ptr<wpds::Wrapper> wrapper)
        : wpds(wrapper)
    {
        // WPDS for the following CFG:
        //
        //                     ___
        //                    /   \ <loop>
        //                    \   /
        // (start) --------> (loop) --------> (finish)
        //         <inital>         <step>
        //
        // <...> indicate the weight

        wpds.add_rule(keys.p, keys.start, keys.p, keys.loop,   weights.initial);
        wpds.add_rule(keys.p, keys.loop,  keys.p, keys.loop,   weights.loop);
        wpds.add_rule(keys.p, keys.loop,  keys.p, keys.finish, weights.step);

        // And the automaton for poststar:
        //
        //          (start) <id>
        //  (p) ----------------------> (acc)

        wfa.addState(keys.p,   weights.zero);
        wfa.addState(keys.acc, weights.zero);

        wfa.setInitialState(keys.p);
        wfa.addFinalState(keys.acc);

        wfa.addTrans(keys.p, keys.start, keys.acc, weights.id);
    }

    sem_elem_t
    poststar_finish_weight()
    {
        wfa::WFA ps_wfa = wpds.poststar(wfa);

        wfa::TransSet ts = ps_wfa.match(keys.p, keys.finish);

        if (ts.size() != 1) {
            return NULL;
        }
        else {
            return (*ts.begin())->weight();
        }
    }

    ref_ptr<MinPlusIntMatrix>
    poststar_finish_matrix()
    {
        return dynamic_cast<MinPlusIntMatrix*>(
            poststar_finish_weight().get_ptr());
    }

    ref_ptr<Witness>
    poststar_finish_witness()
    {
        return dynamic_cast<Witness*>(
            poststar_finish_weight().get_ptr());
    }
};

///////////////////////

class PathVisitor
    : public CalculatingVisitor<std::string>
{
public:
    //! Overload to calculate the value of an extend node. Modifications to
    //! the AnswerType& parameters will not persist.
    virtual std::string calculateExtend(
        WitnessExtend * w ATTR_UNUSED, std::string & leftValue, std::string & rightValue )
    {
        return leftValue + rightValue;
    }

    //! Overload to calculate the value of an combine node. Modifications to
    //! the list parameter will not persist.
    virtual std::string calculateCombine(
        WitnessCombine * w ATTR_UNUSED,
        std::list<std::string> & childrenValues ATTR_UNUSED)
    {
        return "[!COMBINE NODE!]";
    }

    //! Overload to calculate the value of an merge node. Modifications to
    //! the std::string& parameters will not persist.
    virtual std::string calculateMerge(
        WitnessMerge * w ATTR_UNUSED,
        std::string & callerValue ATTR_UNUSED,
        std::string & ruleValue ATTR_UNUSED,
        std::string & calleeValue ATTR_UNUSED)
    {
        return "[!MERGE NODE!]";
    }

    //! Overload to calculate the value of a rule node.
    virtual std::string calculateRule( WitnessRule * w)
    {
        std::stringstream ss;
        ss << "[" << key2str(w->getRuleStub().from_stack())
           << "->" << key2str(w->getRuleStub().to_stack1())
           << "]";
        if (w->getRuleStub().to_stack2() != WALI_EPSILON) {
            ss << " !! " << key2str(w->getRuleStub().to_stack2());
        }
        return ss.str();
    }

    //! Overload to calculate the value of a trans node.
    virtual std::string calculateTrans( WitnessTrans * w ATTR_UNUSED)
    {
        return "";
    }
};

}

TEST(example$matrix$shortest$path, no$witness$gets$right$matrix)
{
    WpdsWfa1 f(NULL);
    ref_ptr<MinPlusIntMatrix> m = f.poststar_finish_matrix();

    ASSERT_TRUE(m.is_valid());
    EXPECT_EQ(m->matrix(), f.weights.bm_expected);
}

static std::string s_get_path1(
    MatrixRanker<MinPlusIntMatrix> const * ranker)
{
    WpdsWfa1 f(new WitnessWrapper());
    ref_ptr<Witness> w = f.poststar_finish_witness();

    opennwa::query::ShortWitnessVisitor swv(ranker);
    w->accept(swv);

    if (swv.answer()->equal(swv.answer()->zero())) {
        return "[!!ZERO WITNESS!!]";
    }

    PathVisitor pv;
    swv.answer()->accept(pv);

    return pv.answer();
}

TEST(example$matrix$shortest$path, witness$gets$shortest$path)
{
    EXPECT_EQ(
        "[start->loop][loop->finish]",
        s_get_path1(NULL));
}

TEST(example$matrix$shortest$path, witness$gets$shortest$path$to$state$0)
{
    details::MinPlus<int> d_zero;
    d_zero.set_value(0);

    MatrixRanker<MinPlusIntMatrix> ranker(2);
    ranker.set_initial(0, d_zero);
    ranker.set_final(0, d_zero);

    EXPECT_EQ(
        "[start->loop][loop->finish]",
        s_get_path1(&ranker));
}

TEST(example$matrix$shortest$path, witness$gets$shortest$path$to$state$1)
{
    details::MinPlus<int> d_zero;
    d_zero.set_value(0);

    MatrixRanker<MinPlusIntMatrix> ranker(2);
    ranker.set_initial(0, d_zero);
    ranker.set_final(1, d_zero);

    EXPECT_EQ(
        "[start->loop][loop->loop][loop->finish]",
        s_get_path1(&ranker));
}

/////////////////////////

struct Weights2
{
    MinPlusIntMatrix::BackingMatrix
        bm_s1_to_s1,
        bm_s2_to_s2,
        bm_reverse;

    ref_ptr<MinPlusIntMatrix>
        id,
        s1_to_s1,
        s2_to_s2,
        reverse,
        zero;

    Weights2()
        : bm_s1_to_s1(2,2)
        , bm_s2_to_s2(2,2)
        , bm_reverse(2,2)
    {
        int m_s1[2][2] = {
            {   1, inf},
            { inf, inf},
        };

        int m_s2[2][2] = {
            { inf, inf},
            { inf,   1},
        };

        int m_reverse[2][2] = {
            { inf,   1},
            {   1, inf},
        };

        for (size_t i = 0; i < 2; ++i) {
            for (size_t j = 0; j < 2; ++j) {
                bm_s1_to_s1(i, j).set_value(m_s1[i][j]);
                bm_s2_to_s2(i, j).set_value(m_s2[i][j]);
                bm_reverse(i, j).set_value(m_reverse[i][j]);
            }
        }

        s1_to_s1  = new MinPlusIntMatrix(bm_s1_to_s1);
        s2_to_s2  = new MinPlusIntMatrix(bm_s2_to_s2);
        reverse   = new MinPlusIntMatrix(bm_reverse);

        Weights1 w1;
        id = w1.id;
        zero = w1.zero;
    }
};

struct WpdsWfa2
{
    wpds::WPDS wpds;
    wfa::WFA wfa;

    Weights2 weights;
    Keys keys;

    WpdsWfa2(ref_ptr<wpds::Wrapper> wrapper)
        : wpds(wrapper)
    {
        wpds.setWorklist(new QueueWorklist<wali::wfa::ITrans>());

        // WPDS for the following CFG:
        //
        //       <1 to 1>       <id>
        //         /--> (top) --\.
        //        /              \.
        // (start)               (join)---->(middle)---->(finish)
        //        \             /      <id>        <reverse>
        //         \->(bottom)-/
        //      <2 to 2>      <id>
        //
        // <...> indicate the weight.
        //
        // If you want to wind up in state 1, it looks like you should
        // take the top path if you don't look past
        // join/middle.

        wpds.add_rule(keys.p, keys.start,  keys.p, keys.top,    weights.s1_to_s1);
        wpds.add_rule(keys.p, keys.start,  keys.p, keys.bottom, weights.s2_to_s2);

        wpds.add_rule(keys.p, keys.top,    keys.p, keys.join,   weights.id);
        wpds.add_rule(keys.p, keys.bottom, keys.p, keys.join,   weights.id);

        wpds.add_rule(keys.p, keys.join,   keys.p, keys.middle, weights.id);
        wpds.add_rule(keys.p, keys.middle, keys.p, keys.finish, weights.reverse);

        // And the automaton for poststar:
        //
        //          (start) <id>
        //  (p) ----------------------> (acc)

        wfa.addState(keys.p,   weights.zero);
        wfa.addState(keys.acc, weights.zero);

        wfa.setInitialState(keys.p);
        wfa.addFinalState(keys.acc);

        wfa.addTrans(keys.p, keys.start, keys.acc, weights.id);
    }

    sem_elem_t
    poststar_finish_weight()
    {
        wfa::WFA ps_wfa = wpds.poststar(wfa);

        wfa::TransSet ts = ps_wfa.match(keys.p, keys.finish);

        if (ts.size() != 1) {
            return NULL;
        }
        else {
            return (*ts.begin())->weight();
        }
    }

    ref_ptr<Witness>
    poststar_finish_witness()
    {
        return dynamic_cast<Witness*>(
            poststar_finish_weight().get_ptr());
    }
};

static std::string s_get_path2(
    MatrixRanker<MinPlusIntMatrix> const * ranker)
{
    WpdsWfa2 f(new WitnessWrapper());
    ref_ptr<Witness> w = f.poststar_finish_witness();

    opennwa::query::ShortWitnessVisitor swv(ranker);
    w->accept(swv);

    if (swv.answer()->equal(swv.answer()->zero())) {
        return "[!!ZERO WITNESS!!]";
    }

    PathVisitor pv;
    swv.answer()->accept(pv);

    return pv.answer();
}

TEST(example$matrix$shortest$path$midpath$combine, witness$gets$shortest$path$to$state$1)
{
    details::MinPlus<int> d_zero;
    d_zero.set_value(0);

    MatrixRanker<MinPlusIntMatrix> ranker(2);
    ranker.set_initial(0, d_zero);
    ranker.set_initial(1, d_zero);
    ranker.set_final(0, d_zero);

    EXPECT_EQ(
        "[start->bottom][bottom->join][join->middle][middle->finish]",
        s_get_path2(&ranker));
}

}
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[centroid
//` Shows calculation of a centroid of a polygon

#include <iostream>
#include <list>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>
/*<-*/ #include "create_svg_two.hpp" /*->*/

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;

    polygon_type poly;
    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", poly);

    point_type p;
    boost::geometry::centroid(poly, p);

    std::cout << "centroid: " << boost::geometry::dsv(p) << std::endl;
    /*<-*/ create_svg("centroid.svg", poly, p); /*->*/
    return 0;
}

//]

//[centroid_output
/*`
Output:
[pre
centroid: (4.04663, 1.6349)
[$img/algorithms/centroid.png]
]
Note that the centroid might be located in a hole or outside a polygon, easily.
*/
//]


/*
 * tracer/avg_voltage.hpp --
 *
 * This file is part of nettcl2d application.
 *
 * Copyright (c) 2012 Andrey V. Nakin <andrey.nakin@gmail.com>
 * All rights reserved.
 *
 * See the file "COPYING" for information on usage and redistribution
 * of this file, and for a DISCLAIMER OF ALL WARRANTIES.
 *
 */

#ifndef AVG_VOLTAGE_HPP_
#define AVG_VOLTAGE_HPP_

#include <math.h>
#include <string>
#include <fstream>
#include <iomanip>
#include <boost/scoped_ptr.hpp>
#include <phlib/tracestream.h>
#include "../calc/abstract_tracer.hpp"
#include "../util/statistics.hpp"
#include "contact_tracer.hpp"

namespace tracer {

	class AverageVoltage : public ContactTracer {

		virtual void doTrace(const Network& network, double const time, std::ostream& s) {
			const Statistics stat = calcStat(network);
			s << stat.getMean();
		}

		virtual void writeHeader(std::ostream& s) {
			s << "# time\t<voltage>\n";
		}

	public:

		struct Params : public ContactTracerParams {
			Params() : ContactTracerParams("s") {}
		};

		AverageVoltage(const Params& params) : ContactTracer(new Params(params)) {}

	private:

		Statistics calcStat(const Network& network) {
			Statistics stat;

			for (Network::IndexVector::const_iterator i = indices.begin(), last = indices.end(); i != last; ++i) {
				stat.accum(network.contact(*i).voltage);
			}

			return stat;
		}

	};

}

#endif /* AVG_VOLTAGE_HPP_ */


#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

typedef struct {
    MatrixXd A;
    MatrixXd B;
    VectorXd states0;
    double dt;
} ReferenceModel_Config_t;

class ReferenceModel {
protected:
    double dt;
    unsigned int nStates, nInputs, nOutputs;

    VectorXd states0;
    VectorXd states;
    MatrixXd Aref, Bref, Cref;

public:
    ReferenceModel(void);
    ~ReferenceModel(void);

    void initialize(const ReferenceModel_Config_t _config);
    void reset();
    void update(const VectorXd _inputs);
    void get_outputs(VectorXd &_outputs);
    void get_states(VectorXd &_states);
};


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

#include "multiprecision_config.hpp"

#ifndef DISABLE_MP_TESTS
#include <boost/integer/mod_inverse.hpp>
#include <boost/cstdint.hpp>
#include <boost/optional/optional.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/integer/common_factor.hpp>

using boost::multiprecision::int128_t;
using boost::multiprecision::int256_t;
using boost::integer::mod_inverse;
using boost::integer::gcd;

template<class Z>
void test_mod_inverse()
{
    //Z max_arg = std::numeric_limits<Z>::max();
    Z max_arg = 500;
    for (Z modulus = 2; modulus < max_arg; ++modulus)
    {
        if (modulus % 1000 == 0)
        {
            std::cout << "Testing all inverses modulo " << modulus << std::endl;
        }
        for (Z a = 1; a < modulus; ++a)
        {
            Z gcdam = gcd(a, modulus);
            Z inv_a = mod_inverse(a, modulus);
            // Should fail if gcd(a, mod) != 1:
            if (gcdam > 1)
            {
                BOOST_TEST(inv_a == 0);
            }
            else
            {
                BOOST_TEST(inv_a > 0);
                // Cast to a bigger type so the multiplication won't overflow.
                int256_t a_inv = inv_a;
                int256_t big_a = a;
                int256_t m = modulus;
                int256_t outta_be_one = (a_inv*big_a) % m;
                BOOST_TEST_EQ(outta_be_one, 1);
            }
        }
    }
}

int main()
{
    test_mod_inverse<boost::int16_t>();
    test_mod_inverse<boost::int32_t>();
    test_mod_inverse<boost::int64_t>();
    test_mod_inverse<int128_t>();

    return boost::report_errors();
}
#else
int main()
{
  return 0;
}
#endif


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

Point2d cam2pixel ( const Mat& p, const Mat& K );

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    // 匹配对只是从RGB图里找，深度图作为3D坐标值来源
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点，这是世界坐标系的坐标
    // 问题：不需要第2个view的depth图？
    // 原因：3D2D求解，一般assume first image是世界坐标系，因此只需要second image的2d坐标即可！
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    // 用于存储世界坐标系的3D点
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    // 准备solve_pnp的输入，分别是2d和3d的匹配对，其中3d即为第1个view的世界坐标系下的坐标值
    for ( DMatch m:matches )
    {
        // ()[]是基本的mat操作，取数据值的方法
        // []()是Lambda expression
        // 首先定义了一个指针d1.ptr<..> (int i)，表示第i行
        // 然后根据首地址，取其中的元素，直接调取第j个元素，从而获得了深度值
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        // VIP：depth异常值处理，直接跳过
        if ( d == 0 )   // bad depth
            continue;
        // TUM定义：The depth images are scaled by a factor of 5000
        // 不同的dataset的设定不同，之后自己实现要注意细节
        float dd = d/5000.0;
        // 图像坐标系转化成相机坐标系的归一化坐标
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        // 再转成相机坐标系的坐标
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        // 问题：trainIdx和queryIdx的区别？
        // queryIdx refers to keypoints1 and trainIdx refers to keypoints2
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }
    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    // OutputArray在opencv里是一个template，_OutputArray (Mat &m)
    // InputArray是_InputArray (const Mat &m)，加了const保证不能修改输入
    // 输入需要intrinsic matrix，用于投影变换
    // 其中，Mat()对应distortion matrix
    // 默认方法：SOLVEPNP_EPNP，这个不好解，结果不稳定！
    // TO-DO: 回头自己也重新写下底层的实现
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    // 输出的r是一个向量
    cout<<"r: "<<r<<" t: "<<t<<endl;
    Mat R;
    // opencv实现了罗德里格斯公式，将旋转向量转换为旋转矩阵
    cv::Rodrigues ( r, R ); // r为旋转向量形式
    // R=
    // [0.9979193252225623, -0.05138618904650231, 0.03894200717249974;
    //  0.05033852907726567, 0.9983556574294724, 0.02742286945057129;
    //  -0.04028712992610883, -0.02540552801740052, 0.998865109165634]
    // t=
    // [-0.1255867099726712;
    //  -0.007363525262960201;
    //  0.06099926588593754]
    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;
    // 问题: PNP问题不就是用ba解的吗？为何重复两次操作？
    // 一般是拿普通解法（如P3P或EPNP）得到一个好的初值，用BA做进一步优化
    // 如果直接用BA会报segment fault:11，是内存用尽
    // 说明初值给的不好，无法收敛
    // 有意思的是，每次优化的结果是一样的，
    // 而且BA后结果差得不多，说明solvepnp的结果已经不错了！
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );

    // reprojection error的单位为Pixel
    float avg_err;
    float error = 0;
    for ( int i=0; i<pts_3d.size(); i++ )
    {
        // 将p1_3d->第2个view，得到p1_2_3d
        Mat_<double> p1_2_3d = R * (Mat_<double>(3,1)<<pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) + t;
        // 将p1_2_3d转变成2d图像坐标系，应该与p2接近
        Point2d p1_2_2d = cam2pixel(p1_2_3d, K);
        // calculate error
        float e = abs(p1_2_2d.x - pts_2d[i].x) +
                  abs(p1_2_2d.y - pts_2d[i].y);
        error += e;
    }
    // 误差量级大概是横竖各差1.5个pixel
    // 那比2D2D要多啊，之前是0.5个pixel
    avg_err = error / pts_3d.size();
    cout<<"average error is "<<avg_err<<endl;
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

// 3d相机坐标系转换成2d图像坐标系
Point2d cam2pixel ( const Mat& p, const Mat& K )
{
    return Point2d
           (
               p.at<double> ( 0,0 ) * K.at<double> ( 0,0 ) / p.at<double> ( 2,0 ) + K.at<double> ( 0,2 ),
               p.at<double> ( 1,0 ) * K.at<double> ( 0,0 ) / p.at<double> ( 2,0 ) + K.at<double> ( 1,2 )
           );
}

// 用到了g2o求解器
void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    // pose:XYZ,3个角度;landmark:XYZ
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    // 从结果上来看，3D2D比3D3D要难，3D2D必须要给定好的初值，而3D3D要求很宽松
    // 从原理上，是因为ICP的极小值就是全局最优值，因此可以任意选择初值
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex，首先设置顶点
    // se3经过指数映射得到SE3
    // se3-李代数，SE3-李群
    // 最终希望用李代数，即se3表示的相机位姿
    // 使用g2o自带的相机位姿顶点类VertexSE3Expmap
    // SE3 Vertex parameterized internally with a transformation matrix
    // and externally with its exponential map
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    // 拷贝一下rotation matrix，不直接用可能是怕被修改了
    R_mat <<
               R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 ); // 首先设置Id
    // Quat就是quaternion，四元数
    // sets the initial estimate from an array of double
    // 此处需要外界的初始化，因此必须先用solvepnp
    // 当然也可以把这里的初值人为设定了
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            // translation vector
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                            // 以下用于测试，即给定一个固定的初值
                            // Eigen::Matrix3d::Identity(),
                            // Eigen::Vector3d( 0,0,0 )
                        ) );
    optimizer.addVertex ( pose );

    // 加完了pose，接下来加3D坐标点
    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        // 3D路标点类VertexSBAPointXYZ
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        // 设置Id
        point->setId ( index++ );
        // 设置初始值，将landmark坐标赋值
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        // TO-DO: 等之后研究，即为marginalization
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        // 最后加到optimizer里去
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        // f和(cx,cy)，默认fx=fy
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges，然后设置边
    index = 1;
    for ( const Point2f p:points_2d )
    {
        // 重投影误差边类EdgeProjectXYZ2UV
        // 把XYZ投影到UV平面上，做成了一个误差
        // 在这里有一个computeError函数，可以看到reprojection error的定义！
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        // 设置Id
        edge->setId ( index );
        // 每个边的两侧分别是pose和Landmark
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    // 最后求解
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true ); // 打开调试输出
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;
    cout<<endl<<"after optimization:"<<endl;
    // 这里的T表示transformation matrix，是4*4的矩阵
    cout<<"Transformation matrix="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
    // assign value to R,T
    Eigen::Matrix4d output = Eigen::Isometry3d( pose->estimate() ).matrix();
    Eigen::Matrix3d R_out = output.block(0,0,3,3); // 利用Eigen的block函数获取子矩阵
    Eigen::Vector3d T_out = output.block(0,3,3,4);
    cout<<"R_out="<<endl<<R_out<<endl;
    cout<<"T_out="<<endl<<T_out<<endl;
    // 将eigen的matrix和vector形式转换成Mat格式
    eigen2cv(R_out, R);
    eigen2cv(T_out, t);
    cout<<"R="<<endl<<R<<endl;
    cout<<"T="<<endl<<t<<endl;
}


/*MinimalFEM

Author: Stanislav Pidhorskyi (Podgorskiy)
stanislav@podgorskiy.com
stpidhorskyi@mix.wvu.edu

The source code available here: https://github.com/podgorskiy/MinimalFEM/

The MIT License (MIT)

Copyright (c) 2015 Stanislav Pidhorskyi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.*/

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>

struct Element
{
	void CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets);

	Eigen::Matrix<float, 3, 6> B;
	int nodesIds[3];
};

struct Constraint
{
	enum Type
	{
		UX = 1 << 0,
		UY = 1 << 1,
		UXY = UX | UY
	};
	int node;
	Type type;
};

int                      	nodesCount;
Eigen::VectorXf          	nodesX;
Eigen::VectorXf          	nodesY;
Eigen::VectorXf          	loads;
std::vector< Element >   	elements;
std::vector< Constraint >	constraints;

void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
	Eigen::Vector3f x, y;
	x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
	y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];

	Eigen::Matrix3f C;
	C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;

	Eigen::Matrix3f IC = C.inverse();

	for (int i = 0; i < 3; i++)
	{
		B(0, 2 * i + 0) = IC(1, i);
		B(0, 2 * i + 1) = 0.0f;
		B(1, 2 * i + 0) = 0.0f;
		B(1, 2 * i + 1) = IC(2, i);
		B(2, 2 * i + 0) = IC(2, i);
		B(2, 2 * i + 1) = IC(1, i);
	}
	Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * std::abs(C.determinant()) / 2.0f;

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));

			triplets.push_back(trplt11);
			triplets.push_back(trplt12);
			triplets.push_back(trplt21);
			triplets.push_back(trplt22);
		}
	}
}

void SetConstraints(Eigen::SparseMatrix<float>::InnerIterator& it, int index)
{
	if (it.row() == index || it.col() == index)
	{
		it.valueRef() = it.row() == it.col() ? 1.0f : 0.0f;
	}
}

void ApplyConstraints(Eigen::SparseMatrix<float>& K, const std::vector<Constraint>& constraints)
{
	std::vector<int> indicesToConstraint;

	for (std::vector<Constraint>::const_iterator it = constraints.begin(); it != constraints.end(); ++it)
	{
		if (it->type & Constraint::UX)
		{
			indicesToConstraint.push_back(2 * it->node + 0);
		}
		if (it->type & Constraint::UY)
		{
			indicesToConstraint.push_back(2 * it->node + 1);
		}
	}

	for (int k = 0; k < K.outerSize(); ++k)
	{
		for (Eigen::SparseMatrix<float>::InnerIterator it(K, k); it; ++it)
		{
			for (std::vector<int>::iterator idit = indicesToConstraint.begin(); idit != indicesToConstraint.end(); ++idit)
			{
				SetConstraints(it, *idit);
			}
		}
	}
}

int main(int argc, char *argv[])
{
	if ( argc != 3 )
    {
        std::cout<<"usage: "<< argv[0] <<" <input file> <output file>\n";
        return 1;
    }

	std::ifstream infile(argv[1]);
	std::ofstream outfile(argv[2]);

	float poissonRatio, youngModulus;
	infile >> poissonRatio >> youngModulus;

	Eigen::Matrix3f D;
	D <<
		1.0f,        	poissonRatio,	0.0f,
		poissonRatio,	1.0,         	0.0f,
		0.0f,        	0.0f,        	(1.0f - poissonRatio) / 2.0f;

	D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));

	infile >> nodesCount;
	nodesX.resize(nodesCount);
	nodesY.resize(nodesCount);

	for (int i = 0; i < nodesCount; ++i)
	{
		infile >> nodesX[i] >> nodesY[i];
	}

	int elementCount;
	infile >> elementCount;

	for (int i = 0; i < elementCount; ++i)
	{
		Element element;
		infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
		elements.push_back(element);
	}

	int constraintCount;
	infile >> constraintCount;

	for (int i = 0; i < constraintCount; ++i)
	{
		Constraint constraint;
		int type;
		infile >> constraint.node >> type;
		constraint.type = static_cast<Constraint::Type>(type);
		constraints.push_back(constraint);
	}

	loads.resize(2 * nodesCount);
	loads.setZero();

	int loadsCount;
	infile >> loadsCount;

	for (int i = 0; i < loadsCount; ++i)
	{
		int node;
		float x, y;
		infile >> node >> x >> y;
		loads[2 * node + 0] = x;
		loads[2 * node + 1] = y;
	}

	std::vector<Eigen::Triplet<float> > triplets;
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		it->CalculateStiffnessMatrix(D, triplets);
	}

	Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
	globalK.setFromTriplets(triplets.begin(), triplets.end());

	ApplyConstraints(globalK, constraints);

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);

	Eigen::VectorXf displacements = solver.solve(loads);

	outfile << displacements << std::endl;

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		Eigen::Matrix<float, 6, 1> delta;
		delta << displacements.segment<2>(2 * it->nodesIds[0]),
		         displacements.segment<2>(2 * it->nodesIds[1]),
		         displacements.segment<2>(2 * it->nodesIds[2]);

		Eigen::Vector3f sigma = D * it->B * delta;
		float sigma_mises = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);

		outfile << sigma_mises << std::endl;
	}
	return 0;
}


// fir_filter.cpp example program showing a FIR filter using error-free custom posit configurations
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-DSP project, which is released under an MIT Open Source license.

#define MTL_INITIALIZER_LISTS
#include <boost/numeric/mtl/mtl.hpp>
// select and configure the number system
#include <universal/posit/posit>

const double pi = 3.14159265358979323846;  // best practice for C++

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::unum;

	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;
	int nrOfFailedTestCases = 0;

	posit<nbits, es> p;
	vector< posit<nbits, es> > sinusoid(vecSize), weights(vecSize);

	for (int i = 0; i < vecSize; i++) {
		sinusoid[i] = sin((float(i) / float(vecSize)) *2.0 * pi);

		weights[i] = 0.5f;
	}

	// dot product
	posit<nbits, es> fir;
	fir = 0.0f;
	for (int i = 0; i < vecSize; i++) {
		fir += sinusoid[i] * weights[i];
	}
	cout << "Value is " << fir << endl;

	return (nrOfFailedTestCases > 0 ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/array.hpp>
#include <stapl/algorithm.hpp>
#include <stapl/utility/do_once.hpp>
#include <boost/lexical_cast.hpp>

typedef unsigned long long ulong_type;

////////////////////////////////////////////////////////////////////////////////
/// @brief Determines whether a number is fibonacci or not.
///        If the fibonacci number is even, we then keep the number.
///        If the number is not fibonacci number and even, 0 is returned.
////////////////////////////////////////////////////////////////////////////////
struct not_even_fib
{
  typedef ulong_type result_type;
  template <typename T>
  ulong_type operator()(T i)
  {
    // Statements used to determine if a number is Fibonacci or not is
    //   (5*(N^2))-4 OR (5*(N^2))+4
    //   If the result of either statement is a perfect square number,
    //   Then N is a fibonacci number.
    ulong_type fib_check_1 = 5 * (static_cast <ulong_type>(
      pow(static_cast <long double>(i), 2.0))) - 4;
    ulong_type fib_check_2 = 5 * (static_cast <ulong_type>(
      pow(static_cast <long double>(i), 2.0))) + 4;

    // Square root taken to determine if the number is a perfect square.
    ulong_type root_1 = static_cast <ulong_type>(
      sqrt(static_cast<long double>(fib_check_1)));
    ulong_type root_2 = static_cast <ulong_type>(
      sqrt(static_cast<long double>(fib_check_2)));

    // Checks if the number is even and verifies as a perfect square
    //   number.
    if ((root_1 * root_1 == fib_check_1 ||
         root_2 * root_2 == fib_check_2) && (i%2 == 0))
      return i;

   return 0;
  }
};

stapl::exit_code stapl_main(int argc, char** argv)
{
  ulong_type num = boost::lexical_cast<ulong_type> (argv[1]);

  // Creates a non-storage view in counting order from 1 to n.
  auto view_2 = stapl::counting_view<ulong_type>(num, 1);

  // Maps not_even_fib functor to set numbers in the view that are not
  //   divisible by 3 or 5 to 0. Then stapl::plus is called to add the numbers
  //   in the view.
  ulong_type ans = stapl::map_reduce(
   not_even_fib(), stapl::plus<ulong_type>(), view_2
  );

  // Prints the total sum.
  stapl::do_once([&]{
    std::cout << "The total is: " << ans << std::endl;
  });

  return EXIT_SUCCESS;
}


#include <Eigen/Geometry>

#include <sphericalsfm/sfm_types.h>
#include <sphericalsfm/so3.h>

namespace sphericalsfm {
    Pose::Pose()
    : t( Eigen::Vector3d::Zero() ), r( Eigen::Vector3d::Zero() ), P( Eigen::Matrix4d::Identity() )
    {

    }

    Pose::Pose( const Eigen::Vector3d &_t, const Eigen::Vector3d &_r )
    : t( _t ), r( _r ), P( Eigen::Matrix4d::Identity() )
    {
        P.block<3,3>(0,0) = so3exp(r);
        P.block<3,1>(0,3) = t;
    }

    Pose Pose::inverse() const
    {
        Pose poseinv;
        poseinv.P.block<3,3>(0,0) = P.block<3,3>(0,0).transpose();
        poseinv.t = -P.block<3,3>(0,0).transpose()*t;
        poseinv.r = -r;
        poseinv.P.block<3,1>(0,3) = poseinv.t;
        return poseinv;
    }

    void Pose::postMultiply( const Pose &pose )
    {
        P = P * pose.P;
        t = P.block<3,1>(0,3);
        r = so3ln( P.block<3,3>(0,0) );
    }

    Point Pose::apply( const Point &point ) const
    {
        return P.block<3,3>(0,0) * point + t;
    }

    Point Pose::applyInverse( const Point &point ) const
    {
        return P.block<3,3>(0,0).transpose() * ( point - t );
    }

    Eigen::Vector3d Pose::getCenter() const
    {
        return P.block<3,3>(0,0).transpose() * (-t);
    }
}


#include "sir.h"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

using State = boost::array<double, 3>;
using Stepper = boost::numeric::odeint::runge_kutta_dopri5<State>;

double smooth_trans(double u0, double u1, double t, double tc, double teps)
{
  const double t0 = tc - teps;
  const double t1 = tc + teps;
  return t <= t0 ? u0 : t >= t1 ? u1 : u0 + (u1 - u0) * (1 - std::cos(M_PI / (t1 - t0) * (t - t0))) * 0.5;
}

double linear_trans(double u0, double u1, double t, double tc, double teps)
{
  const double t0 = tc - teps;
  const double t1 = tc + teps;
  return t <= t0 ? u0 : t >= t1 ? u1 : u0 + (u1 - u0) * (t - t0) / (t1 - t0);
}

static auto make_sir_rhs(double N, double beta, double gamma, double tint, double dint, double kbeta)
{
  /// RHS of the SIR model. We preprocess beta/N for efficiency.
  return [beta_over_N = beta / N, gamma, tint, dint, kbeta](const State &x, State &dxdt, double t) {
    double S = x[0];
    double I = x[1];
    double R = x[2];

    //auto trans = smooth_trans;
    auto trans = linear_trans;
    const double b_N = beta_over_N * trans(1., kbeta, t, tint, dint * 0.5);
    double A = b_N * S * I;
    double B = gamma * I;
    dxdt[0] = -A;
    dxdt[1] = A - B;
    dxdt[2] = B;
  };
}

/**
    Arguments:
        S0, I0, R0: Initial state (at day=0)
        days: Number of days to evaluate.
        N: Population
        beta, gamma: SIR parameters

    Return value:
        Array {N - S(t) | t = 1, ..., days}
  */
std::vector<double>
sir_infected_so_far(double S0, double I0, double R0, int days, long long N, double beta, double gamma, double tint, double dint, double kbeta)
{
  const int steps_per_day = 10;
  const double dt = 1.0 / steps_per_day;

  auto rhs = make_sir_rhs(N, beta, gamma, tint, dint, kbeta);

  std::vector<double> result;
  result.reserve(days);

  // Observer gets called for each time step evaluated by the integrator.
  // We consider only every `steps_per_day` steps
  // (skipping the first one as well), and store N - S(t).
  auto observer = [&result, N, cnt = 0](State y, double t) mutable {
    if (cnt % steps_per_day == 0 && cnt > 0)
    {
      // printf("t=%g  y=%g %g %g\n", t, y[0], y[1], y[2]);
      result.push_back(N - y[0]);
    }
    ++cnt;
  };

  State y0{S0, I0, R0};
  boost::numeric::odeint::integrate_n_steps(
    Stepper{}, rhs, y0, 0.0, dt, days * steps_per_day, observer);
  return result;
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2012 Dominic Marcello
//  Copyright (c) 2012 Bryce Adelstein-Lelbach
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#if !defined(OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F)
#define OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F

#include <octopus/assert.hpp>
#include <octopus/trivial_serialization.hpp>
#include <octopus/state.hpp>

#include <boost/cstdint.hpp>

#include <algorithm>
#include <cmath>
#include <vector>

namespace octopus
{

template <typename T>
inline bool compare_real(T x, T y)
{
    T const epsilon = std::numeric_limits<T>::epsilon();

    if ((x + epsilon >= y) && (x - epsilon <= y))
        return true;
    else
        return false;
}

template <typename T>
inline bool compare_real(T x, T y, T epsilon)
{
    if ((x + epsilon >= y) && (x - epsilon <= y))
        return true;
    else
        return false;
}

template <typename T>
inline boost::uint64_t hex_real(T x)
{
    boost::uint64_t* hex = (boost::uint64_t*) &x;
    return *hex;
}

inline double maximum(double a, double b)
{
    return (std::max)(a, b);
}

inline double maximum(double a, double b, double c)
{
    return (std::max)(a, (std::max)(b, c));
}

inline double minimum(double a, double b)
{
    return (std::min)(a, b);
}

inline double minimum(double a, double b, double c)
{
    return (std::min)(a, (std::min)(b, c));
}

// Serializable minimum
struct minimum_functor : octopus::trivial_serialization
{
    template <typename T>
    T const& operator()(T const& a, T const& b) const
    {
        return (std::min)(a, b);
    }
};

// Serializable maximum
struct maximum_functor : octopus::trivial_serialization
{
    template <typename T>
    T const& operator()(T const& a, T const& b) const
    {
        return (std::max)(a, b);
    }
};

template <typename T>
inline T sign(T a)
{
    if (a > 0)
        return 1;
    else if (a < 0)
        return -1;
    else
        return 0;
}

///////////////////////////////////////////////////////////////////////////////
inline double minmod(double a, double b)
{
    return 0.5 * (sign(a) + sign(b)) * (std::min)(std::fabs(a), std::fabs(b));
}

inline double minmod(double a, double b, double c)
{
    return minmod(a, minmod(b, c));
}

inline double minmod_theta(double a, double b, double theta)
{
    return minmod(theta * a, theta * b, 0.5 * (a + b));
}

///////////////////////////////////////////////////////////////////////////////
template <std::size_t Size, typename Rep0, typename Rep1>
inline array<double, Size> minmod(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
    )
{
    //OCTOPUS_ASSERT(v1.size() == v2.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(v1[i], v2[i]);

    return mm;
}

template <std::size_t Size, typename Rep0, typename Rep1, typename Rep2>
inline array<double, Size> minmod(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
  , array<double, Size, Rep2> const& v3)
{
    //OCTOPUS_ASSERT(v1.size() == v2.size());
    //OCTOPUS_ASSERT(v1.size() == v3.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(v1[i], v2[i], v3[i]);

    return mm;
}

template <std::size_t Size, typename Rep0, typename Rep1>
inline array<double, Size> minmod_theta(
    array<double, Size, Rep0> const& v1
  , array<double, Size, Rep1> const& v2
  , double theta
    )
{
    OCTOPUS_ASSERT(v1.size() == v2.size());

    array<double, Size> mm;
    //mm.reserve(v1.size());

    for (std::size_t i = 0; i < v1.size(); ++i)
        mm[i] = minmod(
            theta * v1[i]
          , theta * v2[i]
          , 0.5 * (v1[i] + v2[i]));

    return mm;
}

}

#endif // OCTOPUS_754BFB65_F28B_4F97_83FF_69C5DEAB500F


#include <Eigen/Dense>

#include "LJLike_0.hpp"

struct ModelSystem {
  // Calculate the objective function value at position x.
  double value(const Eigen::VectorXd& x) const;

  // Calculate the gradient at position x.
  Eigen::VectorXd gradient(const Eigen::VectorXd& x) const;

  // Get the initial step length in the direction of p.
  double initial_step_length(
      const Eigen::VectorXd& x, // the current set of parameters
      const Eigen::VectorXd& p  // the current search direction
  ) const;

  // Check if change from old to new parameter set is acceptable.
  double change_acceptable(
      const Eigen::VectorXd& x_old, // the current set of parameters
      const Eigen::VectorXd& x_new  // the new set of parameters
  ) const;

  // Convert representation to Cartesian coordinates.
  Eigen::Vector2d to_cartesian(const Eigen::VectorXd& x) const;

  ModelSystem(const Eigen::Vector2d& x0 = Eigen::Vector2d(1.0, 1.0));

  Eigen::Vector2d x0;
  double d_max = 0.5;
  double r;

  AutoGeneratedSplines::LJLike_0::Spline spline;
};


#include "fast_mass_springs_precomputation_dense.h"
#include "signed_incidence_matrix_dense.h"
#include <Eigen/Dense>

bool fast_mass_springs_precomputation_dense(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & E,
  const double k,
  const Eigen::VectorXd & m,
  const Eigen::VectorXi & b,
  const double delta_t,
  Eigen::VectorXd & r,
  Eigen::MatrixXd & M,
  Eigen::MatrixXd & A,
  Eigen::MatrixXd & C,
  Eigen::LLT<Eigen::MatrixXd> & prefactorization)
{
  /////////////////////////////////////////////////////////////////////////////
  // Set up the list of edge lengths (rest spring length)
  r.resize(E.rows());
  for (int e = 0; e < E.rows(); e++) {
    int i = E(e, 0);
    int j = E(e, 1);

    // Eigen::Vector3d p0(V(i, 0), V(i, 1), V(i, 2));
    Eigen::Vector3d p_i = V.row(i);
    Eigen::Vector3d p_j = V.row(j);

    // Set r
    r(e) = (p_i - p_j).norm();
  }

  // Get the mass matrix
  M = m.asDiagonal();

  // Get the signed incidence matrix A
  signed_incidence_matrix_dense(V.rows(), E, A);

  // Set the selection matrix
  C = Eigen::MatrixXd::Zero(b.size(), V.rows());
  for (int i = 0; i < b.size(); i++) {
    for (int j = 0; j < V.rows(); j++) {
      if (b(i) == j) {
        C(i, j) = 1;
      } else {
        C(i, j) = 0;
      }
    }
  }

  // Compute W = w C^T * C                         See iPad
  Eigen::MatrixXd W = 1.0e10 * C.transpose() * C;

  // Compute Q --- NOTE: We add W to Q for the pinned vertices
  Eigen::MatrixXd Q = k * A.transpose() * A + (1.0 / pow(delta_t, 2.0)) * M + W;
  /////////////////////////////////////////////////////////////////////////////
  prefactorization.compute(Q);
  return prefactorization.info() != Eigen::NumericalIssue;
}


#include "gtest.h"

#include <fstream>
#include <utility>
#include <iomanip>

#include <Eigen/MPRealSupport>
#include <Eigen/SVD>
#include <irlib/common.hpp>
#include <irlib/detail/gauss_legendre.hpp>
#include <irlib/detail/legendre_polynomials.hpp>

using namespace irlib;

TEST(mpmath, cast) {
    ir_set_default_prec<mpreal>(167);

    for (int i=0; i<100; ++i) {
        ASSERT_TRUE(mpreal(i) - mpreal(std::to_string(i)) == 0);
    }

    ASSERT_TRUE(mpreal(0.5) - mpreal("0.5") == 0);

}

TEST(mpmath, SVD) {

    ir_set_default_prec<mpreal>(167);

    //mpreal x{1};
    //test<mpreal>(x, x);
    //make_pair_test<mpreal,mpreal>(x, x);

    // Declare matrix and vector types with multi-precision scalar type
    typedef Eigen::Matrix<mpreal,Eigen::Dynamic,Eigen::Dynamic>  MatrixXmp;
    typedef Eigen::Matrix<mpreal,Eigen::Dynamic,1>        VectorXmp;

    int N = 10;
    MatrixXmp A = MatrixXmp::Random(N,N);
    VectorXmp b = VectorXmp::Random(N);

    Eigen::BDCSVD<MatrixXmp> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    ASSERT_TRUE(svd.matrixU().rows() == N);
    ASSERT_TRUE(svd.matrixU().cols() == N);
    ASSERT_TRUE(svd.matrixV().rows() == N);
    ASSERT_TRUE(svd.matrixV().cols() == N);
    MatrixXmp A_reconst = svd.matrixU() * svd.singularValues().asDiagonal() * svd.matrixV().adjoint();

    ASSERT_TRUE((A - A_reconst).cwiseAbs().maxCoeff() < 1e-40);
}

TEST(mpmath, gauss_legenre) {
    ir_set_default_prec<mpreal>(167);

    //Integrate x**2 over [-1, 1]
    for (int degree : std::vector<int>{6, 12, 24}) {
        std::vector<std::pair<mpreal,mpreal>> nodes = irlib::detail::gauss_legendre_nodes<mpreal>(degree);

        mpreal sum = 0.0;
        for (auto n : nodes) {
            sum += (n.first*n.first) * n.second;
        }
        ASSERT_TRUE(abs(sum - mpreal(2.0)/mpreal(3.0)) < 1e-48);
    }
}

TEST(mpmath, legendre_polynomials) {
    ir_set_default_prec<mpreal>(167);

    int Nl = 100;

    mpreal x("0.5");

    std::vector<mpreal> vals(Nl);

    for (int l=0; l<Nl; ++l) {
        vals[l] = irlib::legendre_p(l, x);
    }

    for (int l=1; l<Nl-1; ++l) {
        auto mp_l(l);
        auto left_side = (mp_l+1) * vals[l+1];
        auto right_side = (2*mp_l+1) * x * vals[l] - mp_l * vals[l-1];
        ASSERT_TRUE(abs(left_side-right_side) < 1e-40);
    }
}

TEST(mpmath, normalized_legendre_polynomials_derivatives) {
    ir_set_default_prec<mpreal>(167);

    int Nl = 3;
    mpreal x(1);
    auto deriv = irlib::normalized_legendre_p_derivatives(Nl, x);

    //0-th normalized Legendre polynomial
    ASSERT_TRUE(abs(deriv[0][0]-1/sqrt(2)) < 1e-40);
    ASSERT_TRUE(abs(deriv[0][1]) < 1e-40);

    //1-th normalized Legendre polynomial
    ASSERT_TRUE(abs(deriv[1][0]-sqrt(mpreal(3)/mpreal(2))) < 1e-40);
    ASSERT_TRUE(abs(deriv[1][1]-sqrt(mpreal(3)/mpreal(2))) < 1e-40);

    //2-th normalized Legendre polynomial
    auto f0 = sqrt(mpreal(5)/mpreal(2));
    auto f1 = sqrt(mpreal(5)/mpreal(2)) * mpreal(3);
    ASSERT_TRUE(abs(deriv[2][0]-f0) < 1e-40);
    ASSERT_TRUE(abs(deriv[2][1]-f1) < 1e-40);
    ASSERT_TRUE(abs(deriv[2][2]-f1) < 1e-40);

}

/*
TEST(mpmath, io) {
    ir_set_default_prec<mpreal>(200);

    mpreal mp_tmp = mpreal("0.000001")/mpreal("3");
    ir_set_default_prec<mpreal>(20);

    {
        std::ofstream ofs("mp_math_io.txt");
        ofs << std::setprecision(mpfr::bits2digits(mp_tmp.get_prec())) << mp_tmp << std::endl;
        std::cout << std::setprecision(mpfr::bits2digits(mp_tmp.get_prec())) << mp_tmp << std::endl;
    }

    {
        mpreal mp_tmp2;
        std::ifstream ifs("mp_math_io.txt");
        ir_set_default_prec<mpreal>(20);
        ifs >> mp_tmp2;

        mpreal mp_tmp3 = mpreal("0.000001")/mpreal("3");
        mpreal diff = mp_tmp - mp_tmp2;
        std::cout << "diff " << mp_tmp.get_prec() << " " << mp_tmp2.get_prec() << " " << diff.get_prec() << " " << mp_tmp3.get_prec() << std::endl;
        std::cout << std::setprecision(mpfr::bits2digits(mp_tmp.get_prec())) << mp_tmp - mp_tmp2 << std::endl;
    }

}
*/


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>

#include <OpenTissue/core/math/optimization/optimization_blocking_constraint_search.h>
#include <OpenTissue/core/math/optimization/optimization_make_constant_bounds.h>
#include <OpenTissue/core/math/optimization/optimization_bounds2constraint.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_blocking_constraint);

BOOST_AUTO_TEST_CASE(compile_test_case)
{
  double tau = 0.0;
  size_t blocking_idx = 1000;

  ublas::vector<double> x;
  ublas::vector<double> p;
  ublas::vector<double> values;

  x.resize(3,false);
  p.resize(3,false);
  values.resize(3,false);

  values(0) = 1.0;
  values(1) = 2.0;
  values(2) = 3.0;
  x(0) = 4.0;
  x(1) = 4.0;
  x(2) = 4.0;
  p(0) = 0.0;
  p(1) = -4.0;
  p(2) = 0.0;

  bool is_blocked = OpenTissue::math::optimization::blocking_constraint_search(
    x
    , p
    , OpenTissue::math::optimization::make_constraint_from_lower_bounds( OpenTissue::math::optimization::make_constant_bounds( values ) )
    , tau
    , blocking_idx
    );

  BOOST_CHECK(is_blocked);
  BOOST_CHECK(blocking_idx == 1);
  BOOST_CHECK_CLOSE( tau, 0.5, 0.1 );

  values(0) = -2.0;
  values(1) = -2.0;
  values(2) = -2.0;
  x(0) = 0.0;
  x(1) = 0.0;
  x(2) = 0.0;
  p(0) = -1.0;
  p(1) = -1.0;
  p(2) = -1.0;

  is_blocked = OpenTissue::math::optimization::blocking_constraint_search(
    x
    , p
    , OpenTissue::math::optimization::make_constraint_from_lower_bounds( OpenTissue::math::optimization::make_constant_bounds( values ) )
    , tau
    , blocking_idx
    );

  BOOST_CHECK(!is_blocked);
  BOOST_CHECK(blocking_idx == 4);
  BOOST_CHECK_CLOSE( tau, 1.0, 0.1 );

  values(0) = 2.0;
  values(1) = 2.0;
  values(2) = 2.0;
  x(0) = 0.0;
  x(1) = 0.0;
  x(2) = 0.0;
  p(0) = 1.0;
  p(1) = 4.0;
  p(2) = 1.0;

  is_blocked = OpenTissue::math::optimization::blocking_constraint_search(
    x
    , p
    , OpenTissue::math::optimization::make_constraint_from_upper_bounds(   OpenTissue::math::optimization::make_constant_bounds( values ) )
    , tau
    , blocking_idx
    );

  BOOST_CHECK(is_blocked);
  BOOST_CHECK(blocking_idx == 1);
  BOOST_CHECK_CLOSE( tau, 0.5, 0.1 );
}

BOOST_AUTO_TEST_SUITE_END();


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_vector3_norm_1);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;

  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;

  V v0 = V(0.1, -0.01, 0.0 );
  T n0 = OpenTissue::math::norm_1( v0 );
  BOOST_CHECK_CLOSE( n0, 0.1, 0.01);

  V v1 = V(-0.1, -0.01, 0.0 );
  T n1 = OpenTissue::math::norm_1( v1 );
  BOOST_CHECK_CLOSE( n1, 0.1, 0.01);

  V v2 = V( -0.01, 0.1, 0.0 );
  T n2 = OpenTissue::math::norm_1( v2 );
  BOOST_CHECK_CLOSE( n2, 0.1, 0.01);

  V v3 = V( -0.01, -0.1, 0.0 );
  T n3 = OpenTissue::math::norm_1( v3 );
  BOOST_CHECK_CLOSE( n3, 0.1, 0.01);

  V v4 = V( -0.01, 0.0, 0.1 );
  T n4 = OpenTissue::math::norm_1( v4 );
  BOOST_CHECK_CLOSE( n4, 0.1, 0.01);

  V v5 = V( -0.01, 0.0, -0.1 );
  T n5 = OpenTissue::math::norm_1( v5 );
  BOOST_CHECK_CLOSE( n5, 0.1, 0.01);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * distributions_boost.hpp
 *
 *  Created on: 7 Sep 2018
 *      Author: admin
 */

#ifndef SRC_DISTRIBUTIONS_BOOST_HPP_
#define SRC_DISTRIBUTIONS_BOOST_HPP_

#include <random>
#include <Eigen/Eigen>
#include "boost/random.hpp"
#include "boost/generator_iterator.hpp"
class Distributions_boost{
    boost::mt19937 rng;
    unsigned int seed;
public:
    Distributions_boost(unsigned int seed);
    virtual ~Distributions_boost();
    double rgamma(double shape, double scale);
    Eigen::VectorXd dirichlet_rng(Eigen::VectorXd alpha);
    double inv_gamma_rng(double shape,double scale);
    double gamma_rng(double shape,double scale);
    double inv_gamma_rate_rng(double shape,double rate);
    double gamma_rate_rng(double shape,double rate);
    double inv_scaled_chisq_rng(double dof,double scale);
    double norm_rng(double mu, double sigma2);
    double component_probs(double b,Eigen::VectorXd pi);
    double categorical(Eigen::VectorXd probs);
    double exp_rng(double a);
    double unif_rng();
};

#endif /* SRC_DISTRIBUTIONS_BOOST_HPP_ */


#define BZ_TAU_PROFILING

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    TAU_PROFILE("main()", "int ()", TAU_DEFAULT);

    const int N = 32;

    Array<float,2> A(N,N), B(N,N), C(N,N), D(N,N), E(N,N);
    A = 5.0;
    B = 0.0;
    C = 0.0;

    for (int i=0; i < 20; ++i)
    {
        D = A + B + C;
        D /= sum(pow2(D));
        A = B * cos(D) + C * sin(D);
        B += exp(-D);

        float x = sum(A);
        float y = sum(A+B);
        float z = sum(sqr(A)+sqr(B));
        C = x*A+y*B+z*C;

        D = exp(-sqr(A)-sqr(B));
        E = A + B + C + D;
        float q = min(A);
        float r = max(B);
    }

    return 0;
}


/* test_chi_squared.cpp
 *
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id: test_chi_squared.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/chi_squared.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::chi_squared_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME chi_squared
#define BOOST_MATH_DISTRIBUTION boost::math::chi_squared
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME n
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


/* Cam Mannett 2020
 *
 * See LICENSE file
 */

#pragma once

#include <boost/mp11.hpp>

namespace malbolge
{
namespace math
{
/** Compile-time integral exponentiation.
 *
 * Raises @a Base to the power @a Expo.
 * @tparam R Return type
 * @tparam Base Base value
 * @tparam Expo Exponent value
 * @return @a Base raised to the power @a Expo
 */
template <typename R, auto Base, auto Expo>
[[nodiscard]]
constexpr R ipow() noexcept
{
    static_assert(std::is_unsigned_v<R> &&
                  std::is_unsigned_v<decltype(Base)> &&
                  std::is_unsigned_v<decltype(Expo)>,
                  "All parameters must be unsigned integrals");

    using seq = boost::mp11::mp_drop_c<boost::mp11::mp_iota_c<Expo+1>, 1>;

    auto result = R{1};
    boost::mp11::mp_for_each<seq>([&](auto) {
        result *= Base;
    });

    return result;
}

/** Overload that allows for runtime integral exponentiation.
 *
 * Raises @a base to the power @a expo.
 * @tparam R Return type
 * @tparam B Base type
 * @tparam E Exponent type
 * @param base Base value
 * @param expo Exponent value
 * @return @a base raised to the power @a expo
 */
template <typename R, typename B, typename E>
[[nodiscard]]
constexpr R ipow(B&& base, E&& expo) noexcept
{
    static_assert(std::is_unsigned_v<std::decay_t<R>> &&
                  std::is_unsigned_v<std::decay_t<B>> &&
                  std::is_unsigned_v<std::decay_t<E>>,
                  "All parameters must be unsigned integrals");

    auto result = R{1};
    for (auto i = 1u; i <= expo; ++i) {
        result *= base;
    }

    return result;
}
}
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/tools/big_constant.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>

#ifdef _MSC_VER
#pragma warning(disable:4127)
#endif

template <class Real, class T>
void do_test_1F1(const T& data, const char* type_name, const char* test_name)
{
   typedef Real                   value_type;

   typedef value_type(*pg)(value_type, value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::log_hypergeometric_1F1<value_type, value_type>;
#else
   pg funcp = boost::math::log_hypergeometric_1F1;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test hypergeometric_2F0 against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func<Real>(funcp, 0, 1, 2),
      extract_result<Real>(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "log_hypergeometric_1F1", test_name);
   std::cout << std::endl;
}

#ifndef SC_
#define SC_(x) BOOST_MATH_BIG_CONSTANT(T, 1000000, x)
#endif

template <class T>
void test_spots1(T, const char* type_name)
{
#include "hypergeometric_1f1_log_large.ipp"

   do_test_1F1<T>(hypergeometric_1f1_log_large, type_name, "Large random values - log");
}

template <class T>
void test_spots2(T, const char* type_name)
{
#include "hypergeometric_1f1_log_large_unsolved.ipp"

   do_test_1F1<T>(hypergeometric_1f1_log_large_unsolved, type_name, "Large random values - log - unsolved");
}

template <class T>
void test_spots(T z, const char* type_name)
{
   test_spots1(z, type_name);
#ifdef TEST_UNSOLVED
   test_spots2(z, type_name);
#endif
}


//
// Created by yujie on 2020-04-16.
//

#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include "CSV_reader.h"
#include "dataset.h"
#include <iomanip>
#include <blaze/math/DynamicMatrix.h>
#include <blaze/math/Columns.h>

using namespace std;
using blaze::DynamicMatrix;

int main()
{

    // Creating an object of CSVWriter
    CSV_reader reader("/home/yujie/project/regression_algorithm/data/raw/housing.data");

    // Get the data from CSV File
    std::vector<std::vector<std::string> > dataList = reader.getData();

    // set train and test ratio
    float test_ratio = 0.1;

    int test_size = int(test_ratio*dataList.size());
    int train_size = dataList.size() - test_size;

    cout << "train_size is "<< train_size << endl;
    cout << "test_size is "<< test_size << endl;

    cout << dataList.size() <<endl;

    DynamicMatrix<float> A(dataList.size(), 14);

    size_t row = 0;
    size_t col = 0;

//    CRIM: Per capita crime rate by town
//    ZN: Proportion of residential land zoned for lots over 25,000 sq. ft
//    INDUS: Proportion of non-retail business acres per town
//    CHAS: Charles River dummy variable (= 1 if tract bounds river; 0 otherwise)
//    NOX: Nitric oxide concentration (parts per 10 million)
//    RM: Average number of rooms per dwelling
//    AGE: Proportion of owner-occupied units built prior to 1940
//    DIS: Weighted distances to five Boston employment centers
//    RAD: Index of accessibility to radial highways
//    TAX: Full-value property tax rate per $10,000
//    PTRATIO: Pupil-teacher ratio by town
//    B: 1000(Bk — 0.63)², where Bk is the proportion of [people of African American descent] by town
//    LSTAT: Percentage of lower status of the population
//    MEDV: Median value of owner-occupied homes in $1000s

    vector<string> column_names({"crime", "zn",
    "indus", "chas", "nox", "rm", "age", "dis", "rad", "tax",
    "ptratio", "bk", "lstat", "medv"});

//    dataList.insert(dataList.begin(), column_names);

    // Print the content of row by row on screen
    for(std::vector<std::string> vec : dataList)
    {
        //for(std::string data : vec)
        vector<float> vec_tmp;

        col = 0;

        for(auto it=vec.begin(); it<vec.end(); it++)
        {
            string a = *it;
            int b = a.compare("");
            if((*it).compare("")== 0) {
//                vec.erase(it);
            }
            else{
//                std::cout<<*it<<" "<<left<<std::setw(9);
                float ft_num = stof(*it);

                 A(row, col) = ft_num;

                col++;

            }

        }

        row++;

    }

    // fill the data into the the dataset

    DynamicMatrix<float> train_set(train_size, 14);
    DynamicMatrix<float> test_set(test_size, 14);

    size_t train_row = 0;
    size_t train_col = 0;

    size_t test_row = 0;
    size_t test_col = 0;

    // fill in the train set
    for( size_t i=0; i<train_size; ++i) {

        train_col =0;
        for( DynamicMatrix<float,rowMajor>::Iterator it=A.begin(i); it!=A.end(i); ++it ) {

            train_set(train_row, train_col) = *it;
            train_col ++;

        }

        train_row ++;
    }

    // create a final dataset for training and testing

    dataset final_data(train_set, test_set, test_ratio);

    // get the scaler

    for(size_t col_new = 0; col_new < train_set.columns(); col_new++ ){

        auto col_vec = columns(train_set, {col_new});

        float col_mean = mean(col_vec);

        float col_std = stddev(col_vec);

        if(col_new < (train_set.columns()-1)) {
            final_data.set_X_scaler(col_mean, col_std);
        }
        else{
            final_data.set_y_scaler(col_mean, col_std);
        }
    }

    vector<float> X_mean_vec = final_data.get_X_mean();
    vector<float> X_std_vec = final_data.get_X_std();

    vector<float> y_mean_vec = final_data.get_y_mean();
    vector<float> y_std_vec = final_data.get_y_std();

    // fill in the test set

    // fill in the train set
    for( size_t i=0; i<train_size; ++i) {

        train_col =0;
        for( DynamicMatrix<float,rowMajor>::Iterator it=A.begin(i); it!=A.end(i); ++it ) {

            if(it < (A.end(i)-1)){
                train_set(train_row, train_col) =  (train_set(train_row, train_col)-X_mean_vec.at(test_col))/X_std_vec.at(test_col);
                train_col ++;
            }
            else{
                train_set(test_row, test_col) = (train_set(train_row, train_col)-y_mean_vec.at(test_col))/y_std_vec.at(test_col);
            }

        }

        train_row ++;
    }

    for( size_t i=train_size; i<A.rows(); ++i) {

        test_col =0;

        for( DynamicMatrix<float,rowMajor>::Iterator it=A.begin(i); it!=A.end(i); ++it ) {

            if(it < (A.end(i)-1)){
                test_set(test_row, test_col) = (test_set(test_row, test_col)-X_mean_vec.at(test_col))/X_std_vec.at(test_col);
                test_col ++;}
            else{
                test_set(test_row, test_col) = (test_set(test_row, test_col)-y_mean_vec.at(0))/y_std_vec.at(0);
            }

        }

        test_row ++;
    }

    return 0;

}


#ifndef GLAPLACE_HPP
#define GLAPLACE_HPP

#include <concepts>
#include <vector>
#include <armadillo>

#include "gauss_lobatto.hpp"
#include "Quad.hpp"

// computes the Galerkin projection of the laplaction of u: (-Lu, v) ==> (Du, Dv)
// algorithm 105 in:
// D. A. Kopriva. Implementing Spectral Methods for Partial Differential
// Equations: Algorithms for Scientists and Engineers. Scientific computation.
// Springer Netherlands, Dordrecht, 1. aufl. edition, 2009. ISBN 9048122600.
template <std::floating_point real>
matrix<real> glaplace(const matrix<real>& u, const Quad<real>& element, const matrix<real>& D, const quad_rule<real>& quadrature)
{
    matrix<real> u_xi = D * u;
    matrix<real> u_eta = u * D.t();

    matrix<real> F = D.t() * (arma::diagmat(quadrature.w) * (element.A % u_xi - element.B % u_eta));
    matrix<real> G = ((element.C % u_eta - element.B % u_xi) * arma::diagmat(quadrature.w)) * D;

    matrix<real> lap_u = F * arma::diagmat(quadrature.w) + arma::diagmat(quadrature.w) * G;

    return lap_u;
}

#endif