#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <bitset>
#include <boost/foreach.hpp>
#include "CRCEncoder.cpp"
#include "HammingEncoder.cpp"

using namespace std;
using namespace boost;

#define SYN char(22)

struct CorruptFrame
{
	string frameData;
	int frameNum;
};

class BinaryParser
{
public:
	//public parser entry point for encode and decode
	vector<string> Parse(string filename, bool read)
	{
		Read(filename, read);

		if (read)
			DecodeData(this->_data);
		else
			EncodeData();

		return this->_encodedData;
	}

private:
	vector<string> _data;
	vector<string> _encodedData;
	CRCEncoder _crcEncoder;
	HammingEncoder _hammingEncoder;

	//reads in content of given file
	void Read(string filename, bool read)
	{
		fstream stream;
		string lineIn;
		string temp;

		stream.open(filename);

		if (stream.good())
		{
			cout << "Reading contents of " << filename << "..." << endl;
			while (stream.good())
			{
				getline(stream, lineIn);

				if (!read && lineIn.length() > 64)
				{
					while (lineIn.length() > 64)
					{
						temp = lineIn.substr(0, 64);
						this->_data.push_back(temp);
						lineIn = lineIn.substr(64);
					}
					this->_data.push_back(lineIn);
				}
				else
					this->_data.push_back(lineIn);
			}
			stream.close();
		}
		else
			cout << "ERROR: Could not open file: " << filename << endl;
	}

	//converts given string to binary with options of parity or decimal conversion
	string ConvertToBinary(string str, bool withParity, bool isInt = false)
	{
		string temp = "";
		string binStr = "";
		char bit;
		if (isInt)
			return bitset<8>(stoi(str)).to_string();

		if (withParity)
		{
			for (int i = 0; i < str.length(); i++)
			{
				temp += bitset<7>(str[i]).to_string();

				bit = EvaluateParity(temp);
				binStr += bit + temp;
				temp.clear();
			}
			temp = binStr;
		}
		else
		{
			for (int i = 0; i < str.length(); i++)
				temp += bitset<8>(str[i]).to_string();
		}
		return temp;
	}

	//determines odd parity for given binary string
	char EvaluateParity(string binStr)
	{
		int numOnes = 0;

		for each (auto elem in binStr)
		{
			if (elem == '1')
				numOnes++;
		}

		if (numOnes % 2 == 0)
			return '1';

		return '0';
	}

	//encodes SYN, SYN, Length, and Message with hamming and CRC hash
	void EncodeData()
	{
		string temp;
		temp = "";

		for each (auto str in this->_data)
		{
			temp += ConvertToBinary(to_string(SYN), false, true);
			temp += ConvertToBinary(to_string(SYN), false, true);

			temp += ConvertToBinary(to_string(str.length()), false, true);

			str = ConvertToBinary(str, true);
			str = EncodeBytesToHamming(str);

			temp += str;
			temp = this->_crcEncoder.EncodeCRC(temp);

			this->_encodedData.push_back(temp);
			temp.clear();
		}
	}

	//encodes individual bytes of given message to hamming
	string EncodeBytesToHamming(string message)
	{
		string encodedHamming = "";

		for (int i = 0; i < message.length(); i += 8)
			encodedHamming += this->_hammingEncoder.EncodeHamming(message.substr(i, 8));

		return encodedHamming;
	}

	//decodes data back to original ASCII representation and determines which strings fail the CRC check
	void DecodeData(vector<string> data)
	{
		string syn1;
		string syn2;
		int length;
		string message;
		string temp = "";
		int pos = 0;
		char bit;
		string result;
		int count = 0;

		int numSuccessful = 0;
		int numUnsuccessful = 0;
		CorruptFrame corruptData;
		vector<CorruptFrame> corruptFrames;

		cout << "Contents: " << endl;
		for each(auto item in data)
		{
			if (item.size() == 0)
			{
				count++;
				continue;
			}
			result = this->_crcEncoder.DecodeCRC(item);

			if (result.length() != 0)
			{
				syn1 = result.substr(0, 8);
				syn2 = result.substr(8, 8);

				length = bitset<8>(result.substr(16, 8)).to_ulong();
				if (length == 0)
				{
					count++;
					continue;
				}

				message = result.substr(24);

				item = DecodeBytesFromHamming(message);
				message = item;
			}
			else
			{
				cerr << "CRC check failure at frame: " << count << endl;

				corruptData.frameData = item;
				corruptData.frameNum = count;
				corruptFrames.push_back(corruptData);

				count++;
				numUnsuccessful++;

				continue;
			}

			while ((pos / 8) < length)
			{
				for (int i = pos; i < pos + 8; i++)
					temp += message.at(i);

				pos = pos + 8;
				bit = EvaluateParity(temp);
				if (bit == '0')
				{
					temp = char(bitset<7>(temp.substr(1)).to_ulong());

					cout << temp;
				}
				else
					cout << "Parity check failed.";
				temp.clear();
			}
			pos = 0;
			cout << endl;
			numSuccessful++;
			count++;
		}

		cout << "successful decodes: " << numSuccessful << endl;
		cout << "unsuccessful decodes: " << numUnsuccessful << endl;

		if (numUnsuccessful != 0)
			CorrectFrame(corruptFrames);
	}

	//decodes individual bytes of given message from hamming
	string DecodeBytesFromHamming(string message)
	{
		string decodedHamming = "";

		for (int i = 0; i < message.length(); i += 12)
			decodedHamming += this->_hammingEncoder.DecodeHamming(message.substr(i, 12));

		return decodedHamming;
	}

	//corrects the bad bits of corrupt data using hamming bit correction
	void CorrectFrame(vector<CorruptFrame> corruptData)
	{
		vector<string> correctedData;

		cout << endl << "Performing error correction..." << endl;

		for each(auto item in corruptData)
		{
			string temp;
			string errorLocations = "";

			string controlLengthStr;
			string crcHash;
			string message;
			string correctedMessage = "";

			cout << "Correcting frame: " << item.frameNum << endl;

			crcHash = item.frameData.substr(item.frameData.length() - 16, 16);
			item.frameData = item.frameData.substr(0, item.frameData.length() - 16);//remove crc
			controlLengthStr = item.frameData.substr(0, 24);//save syn syn length

			message = item.frameData.substr(24);//message

			for (int i = 0; i < message.length(); i += 12)
			{
				string currentStr;
				currentStr = message.substr(i, 12);
				cout << "checking byte: " << (i / 12) + 1 << endl;
				temp = DecodeBytesFromHamming(currentStr);

				if (temp[0] == '_')
				{
					errorLocations = (temp.substr(1));

					if (currentStr[stoi(errorLocations) - 1] == '1')
						currentStr[stoi(errorLocations) - 1] = '0';
					else
						currentStr[stoi(errorLocations) - 1] = '1';

				}
				correctedMessage += currentStr;
				errorLocations.clear();
				temp.clear();
			}

			correctedData.push_back(controlLengthStr + correctedMessage + crcHash);
		}

		cout << endl << "Decoding corrected data..." << endl;
		DecodeData(correctedData);
	}
};


#include <boost/test/unit_test.hpp>
#include "inputs/interp_tests.h"
#include "utils/interpolations.h"
#include "utils/algorithms.hpp"
#include "libs/sources_handling/source_data.h"
#include "slice4missing.hpp"
#include <vector>

BOOST_AUTO_TEST_CASE(test_linear)
{
    std::vector<double> input_data(std::begin(values), std::end(values));
    auto missing_begin = std::begin(xi_interp);
    auto missing_end = std::end(xi_interp);
    BOOST_TEST_REQUIRE(utils::size(expected_linear_interp) == utils::size(xi_interp));  // make sure input is valid!!
    std::size_t count = 0u;
    while (missing_begin != missing_end) {
        auto expected = expected_linear_interp[count];
        //utils::fx_list_type::value_type x = static_cast<utils::fx_list_type::value_type>(*missing_begin);
        //const utils::fx_list_type xi{x -1, x + 1};
        auto input = slice_data(input_data, missing_begin, 1u);
        //std::cout<<"input data at "<<*missing_begin<<"[";
        //out_range(std::cout, input, " ");
        //std::cout<<"]"<<std::endl;
        auto calc = utils::linear_interpolation(input, xi_values(missing_begin, 1),
                static_cast<math::fx_list_type::value_type>(*missing_begin));/*, xi, x*/
        if (!round_cmp(expected, calc, 0.0001)) {
            std::cout<<"input is [";
            out_range(std::cout, input, " ");
            std::cout<<"]";
            BOOST_TEST(false, "- fix value "<<calc<<" != expected "<<expected<<" at "<<*missing_begin);
        }
        ++missing_begin;
        ++count;
    }
}

BOOST_AUTO_TEST_CASE(test_linear_with_gaps)
{
    auto dataset = from_double_array(std::begin(values), std::end(values), "rand_name");
    // this unit test is more like the way we run in the system - we have a list of
    // all the values and the row numbers and we are extracting the valid values with the
    // row numbers one at the time and verifing that it is correct
    auto missing_begin = std::begin(xi_interp);
    auto missing_end = std::end(xi_interp);
    BOOST_TEST_REQUIRE(utils::size(expected_linear_interp) == utils::size(xi_interp));  // make sure input is valid!!
    unsigned int count = 0u;
    while (missing_begin != missing_end) {
        auto expected = expected_linear_interp[count];
        auto subset = slice(dataset, missing_begin, 1u);    // the the subset that we need to fix
        BOOST_TEST_REQUIRE(utils::size(subset) == 2u);        // win size * 2
        BOOST_TEST_REQUIRE(subset[0].value != *missing_begin);
        BOOST_TEST_REQUIRE(subset[1].value != *missing_begin);
        const auto vals = transform_slice(std::begin(subset), std::end(subset));
        const auto cords = row_nums_transform(std::begin(subset), std::end(subset));
        BOOST_TEST_REQUIRE(vals.size() == cords.size());
        BOOST_TEST_REQUIRE(vals.size() == subset.size());
        auto calc = utils::linear_interpolation(vals, cords,
                static_cast<math::fx_list_type::value_type>(*missing_begin));/*, xi, x*/
        if (!round_cmp(expected, calc, 0.0001)) {
            std::cout<<"input is [";
            out_range(std::cout, vals, " ");
            std::cout<<"]";
            BOOST_TEST(false, "- fix value "<<calc<<" != expected "<<expected<<" at "<<*missing_begin);
        }
        ++missing_begin;
        ++count;

    }
}


#ifndef PARMCB_SVA_TREES_HPP_
#define PARMCB_SVA_TREES_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under 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 <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/detail/tuple_basic.hpp>
#include <boost/timer/timer.hpp>

#include <cstddef>
#include <functional>
#include <iostream>
#include <iterator>
#include <limits>
#include <set>
#include <vector>

#include <parmcb/forestindex.hpp>
#include <parmcb/spvecgf2.hpp>
#include <parmcb/util.hpp>
#include <parmcb/sptrees.hpp>
#include <parmcb/detail/cycles.hpp>

namespace parmcb {

    template<class Graph, class WeightMap, class CycleOutputIterator, class CyclesBuilder, bool ParallelUsingTBB>
    typename boost::property_traits<WeightMap>::value_type _mcb_sva_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<WeightMap>::value_type WeightType;

        /*
         * Index the graph
         */
        ForestIndex<Graph> forest_index(g);
        auto csd = forest_index.cycle_space_dimension();
        std::cout << "Cycle space dimension: " << csd << std::endl;

        /*
         * Initialize support vectors
         */
        std::vector<SpVecGF2<std::size_t>> support;
        for (std::size_t k = 0; k < csd; k++) {
            support.emplace_back(k);
        }

        boost::timer::cpu_timer cycle_timer;
        cycle_timer.stop();
        boost::timer::cpu_timer support_timer;
        support_timer.stop();
        boost::timer::cpu_timer trees_timer;
        trees_timer.stop();

        /*
         * Initialize all shortest path trees
         */
        trees_timer.resume();
        std::vector<parmcb::SPTree<Graph, WeightMap>> trees;
        std::vector<parmcb::CandidateCycle<Graph, WeightMap>> cycles;
        CyclesBuilder cycles_builder;
        cycles_builder(g, weight_map, trees, cycles);
        std::cout << "Total candidate cycles: " << cycles.size() << std::endl;
        const bool sorted_cycles = true;
        if (sorted_cycles) {
            // sort
            std::cout << "Sorting cycles" << std::endl;
            std::sort(cycles.begin(), cycles.end(), [](const auto &a, const auto &b) {
                return a.weight() < b.weight();
            });
        }
        ShortestOddCycleLookup<Graph, WeightMap, ParallelUsingTBB> cycle_lookup(g, weight_map, trees, cycles,
                sorted_cycles);
        trees_timer.stop();

        /*
         * Main loop
         */
        WeightType mcb_weight = WeightType();
        for (std::size_t k = 0; k < csd; k++) {
            if (k % 250 == 0) {
                std::cout << k << std::endl;
            }

            /*
             * Compute shortest odd cycle
             */
            std::set<Edge> signed_edges;
            convert_edges(support[k], std::inserter(signed_edges, signed_edges.end()), forest_index);
            cycle_timer.resume();
            std::tuple<std::set<Edge>, WeightType, bool> best = cycle_lookup(signed_edges);
            cycle_timer.stop();

            /*
             * Update support vectors
             */
            support_timer.resume();
            std::set<std::size_t> cyclek;
            convert_edges(std::get<0>(best), std::inserter(cyclek, cyclek.end()), forest_index);
            for (std::size_t l = k + 1; l < csd; l++) {
                if (support[l] * cyclek == 1) {
                    support[l] += support[k];
                }
            }
            support_timer.stop();

            /*
             * Output new cycle
             */
            std::list<Edge> cyclek_edgelist;
            std::copy(std::get<0>(best).begin(), std::get<0>(best).end(), std::back_inserter(cyclek_edgelist));
            *out++ = cyclek_edgelist;
            mcb_weight += std::get<1>(best);
        }

        std::cout << "trees   timer" << trees_timer.format();
        std::cout << "cycle   timer" << cycle_timer.format();
        std::cout << "support timer" << support_timer.format();

        return mcb_weight;
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>,
                false>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_fvs_trees_tbb(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::FVSCyclesBuilder<Graph, WeightMap>,
                true>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>,
                false>(g, weight_map, out);
    }

    template<class Graph, class WeightMap, class CycleOutputIterator>
    typename boost::property_traits<WeightMap>::value_type mcb_sva_iso_trees_tbb(const Graph &g, WeightMap weight_map,
            CycleOutputIterator out) {
        return _mcb_sva_trees<Graph, WeightMap, CycleOutputIterator, parmcb::detail::ISOCyclesBuilder<Graph, WeightMap>,
                true>(g, weight_map, out);
    }

} // namespace parmcb

#endif


#include <iostream>
#include <fstream>

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

#define USE_PERIODIC_FFT
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/laplace.hpp>
#include <imgproc/image_operator.hpp>

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

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

constexpr int runs = 1;

constexpr int ENTRY_SQR = 1;
constexpr int ENTRY_RGB = 2;
constexpr int ENTRY_NO_3 = 4;
constexpr int ENTRY_NO_5 = 8;

typedef double FT;

constexpr FT mag_th = static_cast<FT>(0.05);

struct Entry {
    Entry() {}

    Entry(const cv::Ptr<FilterI<uchar>>& a, const std::string& b, const std::string& w, int f = 0)
        : filter(a), name(b), what(w), flags(f) {}

    cv::Ptr<FilterI<uchar>> filter;
    std::string name, what;
    int flags;

    Value value(const std::string &name) {
        return dynamic_cast<ValueManager*>(&(*filter))->value(name);
    }

    void value(const std::string &name, const Value &v) {
        dynamic_cast<ValueManager*>(&(*filter))->value(name, v);
    }

    inline bool rgb() const {
        return flags & ENTRY_RGB;
    }

    inline bool sqr() const {
        return flags & ENTRY_SQR;
    }

    inline int border() const {
        if (flags & ENTRY_NO_3)
            return 2;
        if (flags & ENTRY_NO_5)
            return 3;
        return 0;
    }

    inline cv::Mat process(const cv::Mat src) {
        filter->process(src);
        return filter->results()[what].data.clone();
    }
};

void parseFolder(const fs::path &folder, std::vector<fs::path> &files) {
    fs::directory_iterator end_iter;
    for_each(fs::directory_iterator(folder), fs::directory_iterator(), [&files](const fs::path& file) {
        if (fs::is_regular_file(file))
        {
            std::string ext = file.extension().generic_string();
            boost::algorithm::to_lower(ext);
            if (ext == ".jpg" || ext == ".png") {
                files.push_back(file);
            }
        }
        if (fs::is_directory(file))
            parseFolder(file, files);
    });
}

double processError(Entry &e, const fs::path& path, int n) {
    std::cout << "processing noise" << n << " (" << e.name <<")...";
    std::vector<fs::path> files;
    parseFolder(path, files);
    GaussianNoiseOperator op_noise(n);

    double ret = 0;
    int count = 0;
    std::for_each(files.begin(), files.end(), [&](const fs::path& file) {
        cv::Mat rgb = cv::imread(file.generic_string());
        if (rgb.empty())
        {
            cout << "Can not open " << file.generic_string() << endl;
            return;
        }

        cv::Mat src_n, src = rgb, res, res_n;
        if (!e.rgb())
            cv::cvtColor(rgb, src, CV_BGR2GRAY);
        src_n = src.clone();
        op_noise(src_n);

        res = e.process(src);
        res_n = e.process(src_n);

        if (res.type() != cv::DataType<FT>::type) {
            res.convertTo(res, cv::DataType<FT>::type);
            res_n.convertTo(res_n, cv::DataType<FT>::type);
        }

        double vmin, vmax;
        cv::minMaxIdx(res, &vmin, &vmax);
        res /= vmax;
        res_n /= vmax;

        ret += sum(abs(res - res_n))[0] / res.size().area();
        ++count;
    });
    ret /= count;
    std::cout << ret << std::endl;
    return ret;
}

int main(int argc, char** argv)
{
    fs::path path = "../../images/MDB/MiddEval3-Q";

    std::vector<Entry> filter;
    /*filter.push_back(Entry(new DerivativeGradient<uchar,short, FT, FT, RobertsDerivative<uchar,short>>,"Roberts (2x2)", "mag"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, PrewittDerivative<uchar, short>>, "Prewitt (3x3)", "mag"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short, FT, FT, ScharrDerivative<uchar, short>>, "Scharr (3x3)", "mag"));

    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative<uchar, short>>, "Sobel (3x3)", "mag"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative<uchar, short>>({ NV("grad_kernel_size",5) }), "Sobel (5x5)", "mag"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative<uchar, short>>({ NV("grad_kernel_size",7) }), "Sobel (7x7)", "mag"));
    filter.push_back(Entry(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative<uchar, short>>({ NV("grad_kernel_size",9) }), "Sobel (9x9)", "mag"));*/

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

    /*filter.push_back(Entry(new SusanGradient<short, int>(20), "Susan (37)", "mag"));
    filter.push_back(Entry(new SusanGradient<short, int>(20,true), "Susan (3x3)", "mag"));
    filter.push_back(Entry(new RCMGradient<uchar, 1, short, int>(3,1), "RMG (3x3)", "mag", ENTRY_NO_3));
    filter.push_back(Entry(new RCMGradient<uchar, 1, short, int>(5, 3), "RMG (5x5)", "mag", ENTRY_NO_5));
    filter.push_back(Entry(new RCMGradient<uchar, 3, short, int>(3, 1), "RCMG (3x3)", "mag", ENTRY_NO_3 | ENTRY_SQR | ENTRY_RGB));*/

    /*filter.push_back(Entry(new LaplaceSimple<uchar, short>, "Laplace (3x3)", "laplace"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(5), "Laplace (5x5)", "laplace"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(7), "Laplace (7x7)", "laplace"));
    filter.push_back(Entry(new LaplaceCV<uchar, short>(9), "Laplace (9x9)", "laplace"));*/

    /*filter.push_back(Entry(new LoG<uchar, FT>(3, 1.240080), "LoG (3x3)", "laplace"));
    filter.push_back(Entry(new LoG<uchar, FT>(5, 1.008030), "LoG (5x5)", "laplace"));
    filter.push_back(Entry(new LoG<uchar, FT>(7, 0.873228), "LoG (7x7)", "laplace"));
    filter.push_back(Entry(new LoG<uchar, FT>(9, 0.781859), "LoG (9x9)", "laplace"));*/

    /*filter.push_back(Entry(new QuadratureG2<uchar, FT>(3, 1.24008), "QF StG (3x3)", "energy"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(5, 1.008),   "QF StG (5x5)", "energy"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(7, 0.873226), "QF StG (7x7)", "energy"));
    filter.push_back(Entry(new QuadratureG2<uchar, FT>(9, 0.781854), "QF StG (9x9)", "energy"));

    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 3, 1.2)), "SQF Po (3x3)", "energy"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 5, 1.2)), "SQF Po (5x5)", "energy"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 7, 1.2)), "SQF Po (7x7)", "energy"));
    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSq<uchar, FT, FT>(1, 2, 9, 1.2)), "SQF Po (9x9)", "energy"));

    filter.push_back(Entry(dynamic_cast<Quadrature<uchar, FT, FT, FT, FT>*>(new PCLSqf<uchar, FT>(1, 3, 2.5)), "SQFF Po", "energy"));
    filter.push_back(Entry(new QuadratureLGF<uchar, FT>(5, 0.55), "SQFF Lg", "energy"));

    filter.push_back(Entry(new PCLgf<uchar, FT>(4, 3,2.1,0.55), "PCF Lg", "pc"));
    filter.push_back(Entry(new PCMatlab<uchar>(4, 3, 2.1, 0.55), "PCF Ml", "pc"));
    filter.push_back(Entry(new PCSqf<uchar, FT>(1,3,2.5), "PCF Po", "pc"));*/

    filter.push_back(Entry(dynamic_cast<LaplaceI<uchar, FT>*>(new PCLSqf<uchar, FT>(1.0, 2.0, 1.2)), "Laplace SQFF", "pclmag"));

    int rows = filter.size() + 1;
    int cols = 6;
    std::vector<std::vector<std::string>> table;
    table.resize(rows);
    for_each(table.begin(), table.end(), [&](std::vector<std::string> &col) {
        col.resize(cols);
    });

    table[0][0] = "Method";
    table[0][1] = "noise 10";
    table[0][2] = "noise 20";
    table[0][3] = "noise 30";
    table[0][4] = "noise 40";
    table[0][5] = "noise 50";

    int row = 1;
    for_each(filter.begin(), filter.end(), [&](Entry &e) {
        table[row++][0] = e.name;
    });

    for (int col = 1; col != 6; ++col) {
        int row = 1;
        for_each(filter.begin(), filter.end(), [&](Entry &e) {
            table[row++][col] = boost::str(boost::format("%.3f") % (processError(e, path, 10 * col)));
        });
    }

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

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

    ofs.close();

    return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <range/v3/all.hpp>
#include <optional>
#include <complex>
#include <cx_md5.h>
#include "matrix.h"
namespace views = ranges::views;
auto md5 = [init = std::string("pslxynzg")](const std::string &s) {
  auto sum = cx::md5_new((init + s).c_str());
  const uint8_t *m = reinterpret_cast<const uint8_t *>(sum.h);
  std::string sumstr;
  std::ostringstream ost(sumstr);
  ost << std::hex << std::setfill('0');
  for (size_t i = 0; i < sizeof(sum.h); ++i) {
    ost << std::setw(2) << +m[i];
  }
  return ost.str();
};
struct Game
{
  FixedMatrix<int, 4, 4> networks_;
  std::array<std::complex<int>, 4> step_ = {
    std::complex{ 0, -1 },
    std::complex{ 0, 1 },
    std::complex{ -1, 0 },
    std::complex{ 1, 0 }
  };
  Game()
  {
    //step_[0]=
  }
  std::optional<std::string> shortestpath(const std::string &path, std::complex<int> pos)
  {
    if (pos.real() < 0 || pos.real() >= 4 || pos.imag() < 0 || pos.imag() >= 4) {
      return std::nullopt;
    }
    if (pos == std::complex{ 3, 3 }) {
      return std::string{};
    } else {
      std::optional<std::string> result = std::nullopt;
      auto hashresult = md5(path);
      auto d = hashresult | views::take(4) | views::transform([](char c) {
        return ('b' <= c && c <= 'f');
      });

      std::string s("UDLR");
      for (int i = 0; i < 4; ++i) {
        if (d[i]) {
          auto nextshort = shortestpath(path + s[i], pos + step_[i]);
          if (nextshort) {
            if (result) {
              if ((*nextshort).size() + 1 < (*result).size()) {
                result = s[i] + *nextshort;
              }
            } else {
              result = s[i] + *nextshort;
            }
          }
        }
      }
      return result;
    }
  }
};
int main()
{
  Game g;
  std::complex a{ 0, 0 };
  auto r = g.shortestpath("", a);
  if (r) {

    std::cout << *r << std::endl;
  }
}


#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <math.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <std_msgs/Bool.h>
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <deque>
#include "geometry_msgs/PoseStamped.h"
#include "message_filters/subscriber.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud.h"
#include "tf/message_filter.h"
#include "tf/transform_listener.h"
//PCL
#include <culling/occlusion_culling.h>
#include <culling/utils.h>
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>

// octomap
#include <octomap/ColorOcTree.h>
#include <octomap/OcTree.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include <string>
using namespace std;
using namespace octomap;

typedef pcl::PointXYZRGB pointType;
//typedef pcl::PointXYZ pointType;
typedef pcl::PointCloud<pointType> PointCloud;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "occlusion_culling_test");
    ros::NodeHandle nh;

    ros::Publisher originalCloudPub =
        nh.advertise<sensor_msgs::PointCloud2>("original_pointcloud", 100);
    ros::Publisher occludedCloudPub =
        nh.advertise<sensor_msgs::PointCloud2>("occluded_pointcloud", 100);
    ros::Publisher frustumCloudPub =
        nh.advertise<sensor_msgs::PointCloud2>("frustum_pointcloud", 100);
    ros::Publisher currentPosePub = nh.advertise<geometry_msgs::PoseStamped>("currentPose", 100);
    ros::Publisher sensorPosePub = nh.advertise<geometry_msgs::PoseArray>("sensor_pose", 10);
    ros::Publisher generagedPathPub =
        nh.advertise<visualization_msgs::Marker>("generated_path", 10);
    ros::Publisher sensorFovPub = nh.advertise<visualization_msgs::MarkerArray>("sensor_fov", 100);
    PointCloud occludedCloud;
    PointCloud frustumCloud;

    PointCloud::Ptr originalCloud(new PointCloud);
    PointCloud::Ptr occludedCloudPtr(new PointCloud);
    PointCloud::Ptr frustumCloudPtr(new PointCloud);

    std::string pcdFileName, viewpointsFile, frameId;
    double sensor_roll, sensor_pitch, sensor_yaw, sensor_x, sensor_y, sensor_z;

    // Load Params
    nh.param<std::string>("pcd_input_file", pcdFileName, std::string("sphere_verydensed.pcd"));
    nh.param<std::string>("viewpoints_file", viewpointsFile, std::string("viewpoints.txt"));
    nh.param<std::string>("frame_id", frameId, std::string("world"));
    nh.param<double>("sensor_roll", sensor_roll, 0.0);
    nh.param<double>("sensor_pitch", sensor_pitch, 0.0);
    nh.param<double>("sensor_yaw", sensor_yaw, 0.0);
    nh.param<double>("sensor_x", sensor_x, 0.0);
    nh.param<double>("sensor_y", sensor_y, 0.0);
    nh.param<double>("sensor_z", sensor_z, 0.0);

    std::vector<double> sensorRPY2Robot;
    sensorRPY2Robot.push_back(sensor_roll);
    sensorRPY2Robot.push_back(sensor_pitch);
    sensorRPY2Robot.push_back(sensor_yaw);

    std::vector<double> sensorXYZ2Robot;
    sensorXYZ2Robot.push_back(sensor_x);
    sensorXYZ2Robot.push_back(sensor_y);
    sensorXYZ2Robot.push_back(sensor_z);

    std::string path = ros::package::getPath("culling");
    std::string pcdFilePath = path + "/data/pcd/" + pcdFileName;

    pcl::io::loadPCDFile<pointType>(pcdFilePath, *originalCloud);

    // viewpoints test file
    std::string viewpointsFileStr = path + "/data/" + viewpointsFile;

    double locationx, locationy, locationz, yaw;
    geometry_msgs::PoseArray viewpoints;
    geometry_msgs::PoseStamped robotLocation;
    double viewPointCount = 0;
    double timeSum = 0;

    FILE *file = fopen(viewpointsFileStr.c_str(), "r");
    if (!file)
    {
        std::cout << "\nCan not open the File";
        fclose(file);
    }

    OcclusionCulling<pointType> occlusionCulling(nh, pcdFilePath);
    while (!feof(file))
    {
        fscanf(file, "%lf %lf %lf %lf\n", &locationx, &locationy, &locationz, &yaw);
        ROS_INFO("Robot location x,y,z:%f,%f,%f ", locationx, locationy, locationz);

        robotLocation.pose.position.x = locationx;
        robotLocation.pose.position.y = locationy;
        robotLocation.pose.position.z = locationz;
        tf::Quaternion tf_q;
        tf_q = tf::createQuaternionFromYaw(yaw);
        robotLocation.pose.orientation.x = tf_q.getX();
        robotLocation.pose.orientation.y = tf_q.getY();
        robotLocation.pose.orientation.z = tf_q.getZ();
        robotLocation.pose.orientation.w = tf_q.getW();
        robotLocation.header.frame_id = frameId;
        currentPosePub.publish(robotLocation);

        geometry_msgs::Pose sensorViewPointPose =
            uav2camTransformation(robotLocation.pose, sensorRPY2Robot, sensorXYZ2Robot);
        viewpoints.poses.push_back(sensorViewPointPose);

        PointCloud tempCloud;
        ros::Time tic = ros::Time::now();

        ///////////////////////////////////////////////////////////////
        tempCloud = occlusionCulling.extractVisibleSurface(sensorViewPointPose);
        frustumCloud = occlusionCulling.getFrustumCloud();
        ////////////////////////////////////////////////////////////////

        ros::Time toc = ros::Time::now();
        double elapsed = toc.toSec() - tic.toSec();
        timeSum += elapsed;
        ROS_INFO("Occulision Culling duration (s) = %f", elapsed);
        occludedCloud += tempCloud;
        frustumCloud += frustumCloud;
        viewPointCount++;
        break;
    }
    ROS_INFO("On Average Occulision Culling takes (s) =%f", timeSum / viewPointCount);
    occludedCloudPtr->points = occludedCloud.points;
    frustumCloudPtr->points = frustumCloud.points;

    ///////////////////////////////////////////////////////////////////////////////////////////////////////
    // Draw Path
    std::vector<geometry_msgs::Point> lineSegments;
    geometry_msgs::Point p;
    for (uint i = 0; i < viewpoints.poses.size(); i++)
    {
        if (i + 1 < viewpoints.poses.size())
        {
            p.x = viewpoints.poses[i].position.x;
            p.y = viewpoints.poses.at(i).position.y;
            p.z = viewpoints.poses.at(i).position.z;
            lineSegments.push_back(p);

            p.x = viewpoints.poses.at(i + 1).position.x;
            p.y = viewpoints.poses.at(i + 1).position.y;
            p.z = viewpoints.poses.at(i + 1).position.z;
            lineSegments.push_back(p);
        }
    }
    int c_color[3];
    c_color[0] = 1;
    c_color[1] = 0;
    c_color[2] = 1;
    int id = 0;
    visualization_msgs::Marker linesList = drawLines(lineSegments, id, c_color, frameId, 0.15);

    // *****************Rviz Visualization ************
    ros::Rate loop_rate(1);
    sensor_msgs::PointCloud2 cloud1;
    sensor_msgs::PointCloud2 cloud2;
    sensor_msgs::PointCloud2 cloud5;

    //***original cloud & occlusion cull publish***
    pcl::toROSMsg(*originalCloud, cloud1);  //cloud of original
    pcl::toROSMsg(*occludedCloudPtr,
                  cloud2);  //cloud of the not occluded voxels (blue) using occlusion culling
    pcl::toROSMsg(*frustumCloudPtr,
                  cloud5);  //cloud of the not occluded voxels (blue) using occlusion culling

    while (ros::ok())
    {
        viewpoints.header.frame_id = "world";
        viewpoints.header.stamp = ros::Time::now();

        cloud1.header.stamp = ros::Time::now();
        cloud2.header.stamp = ros::Time::now();
        cloud5.header.stamp = ros::Time::now();

        cloud1.header.frame_id = "world";
        cloud2.header.frame_id = "world";
        cloud5.header.frame_id = "world";

        originalCloudPub.publish(cloud1);
        occludedCloudPub.publish(cloud2);
        frustumCloudPub.publish(cloud5);
        sensorPosePub.publish(viewpoints);
        generagedPathPub.publish(linesList);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


#include <frovedis.hpp>
#include <frovedis/matrix/pblas_wrapper.hpp>
#include <frovedis/matrix/scalapack_wrapper.hpp>

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

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    auto bm = make_blockcyclic_matrix_load<float> ("./sample_3x3");
    auto inv_bm = inv(bm);

    frovedis::lvec<int> ipiv; // empty ipiv array
    getrf<float> (bm,ipiv); // bm will be factorized and ipiv will contain pivoting info
    getri<float> (bm,ipiv); // bm will be overwritten with inversed matrix

    // checking whether the above operations successfully taken place
    bm.save("./out_3x3_1");
    inv_bm.save("./out_3x3_2");
    BOOST_CHECK (system("diff ./out_3x3_1 ./ref_3x3") == 0);
    BOOST_CHECK (system("diff ./out_3x3_2 ./ref_3x3") == 0);
    system("rm -f ./out_3x3_1");
    system("rm -f ./out_3x3_2");
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
/*
  Copyright (C) 2014, 2016 Peter Caspers

  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 lineartsrpricer.cpp
*/

#include <ql/cashflows/lineartsrpricer.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cmscoupon.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/time/schedule.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/math/integrals/kronrodintegral.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/termstructures/volatility/atmsmilesection.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

   const Real LinearTsrPricer::defaultLowerBound = 0.0001,
             LinearTsrPricer::defaultUpperBound = 2.0000;

    LinearTsrPricer::LinearTsrPricer(
        const Handle<SwaptionVolatilityStructure> &swaptionVol,
        const Handle<Quote> &meanReversion,
        const Handle<YieldTermStructure> &couponDiscountCurve,
        const Settings &settings,
        const boost::shared_ptr<Integrator> &integrator)
        : CmsCouponPricer(swaptionVol), meanReversion_(meanReversion),
          couponDiscountCurve_(couponDiscountCurve), settings_(settings),
          volDayCounter_(swaptionVol->dayCounter()), integrator_(integrator) {

        if (!couponDiscountCurve_.empty())
            registerWith(couponDiscountCurve_);

        if (integrator_ == NULL)
            integrator_ =
                boost::make_shared<GaussKronrodNonAdaptive>(1E-10, 5000, 1E-10);
    }

    Real LinearTsrPricer::GsrG(const Date &d) const {

        Real yf = volDayCounter_.yearFraction(fixingDate_, d);
        if (std::fabs(meanReversion_->value()) < 1.0E-4)
            return yf;
        else
            return (1.0 - std::exp(-meanReversion_->value() * yf)) /
                   meanReversion_->value();
    }

    Real LinearTsrPricer::singularTerms(const Option::Type type,
                                        const Real strike) const {

        Real omega = (type == Option::Call ? 1.0 : -1.0);
        Real s1 = std::max(omega * (swapRateValue_ - strike), 0.0) *
                  (a_ * swapRateValue_ + b_);
        Real s2 = (a_ * strike + b_) *
                  smileSection_->optionPrice(strike, strike < swapRateValue_
                                                         ? Option::Put
                                                         : Option::Call);
        return s1 + s2;
    }

    Real LinearTsrPricer::integrand(const Real strike) const {
        return 2.0 * a_ * smileSection_->optionPrice(
                              strike, strike < swapRateValue_ ? Option::Put
                                                              : Option::Call);
    }

    void LinearTsrPricer::initialize(const FloatingRateCoupon &coupon) {

        coupon_ = dynamic_cast<const CmsCoupon *>(&coupon);
        QL_REQUIRE(coupon_, "CMS coupon needed");
        gearing_ = coupon_->gearing();
        spread_ = coupon_->spread();

        fixingDate_ = coupon_->fixingDate();
        paymentDate_ = coupon_->date();
        swapIndex_ = coupon_->swapIndex();

        forwardCurve_ = swapIndex_->forwardingTermStructure();
        if (swapIndex_->exogenousDiscount())
            discountCurve_ = swapIndex_->discountingTermStructure();
        else
            discountCurve_ = forwardCurve_;

        // if no coupon discount curve is given just use the discounting curve
        // from the swap index. for rate calculation this curve cancels out in
        // the computation, so e.g. the discounting swap engine will produce
        // correct results, even if the couponDiscountCurve is not set here.
        // only the price member function in this class will be dependent on the
        // coupon discount curve.

        today_ = QuantLib::Settings::instance().evaluationDate();

        if (paymentDate_ > today_ && !couponDiscountCurve_.empty())
            couponDiscountRatio_ =
                couponDiscountCurve_->discount(paymentDate_) /
                discountCurve_->discount(paymentDate_);
        else
            couponDiscountRatio_ = 1.;

        spreadLegValue_ = spread_ * coupon_->accrualPeriod() *
                          discountCurve_->discount(paymentDate_) *
                          couponDiscountRatio_;

        if (fixingDate_ > today_) {

            swapTenor_ = swapIndex_->tenor();
            swap_ = swapIndex_->underlyingSwap(fixingDate_);

            swapRateValue_ = swap_->fairRate();
            annuity_ = 1.0E4 * std::fabs(swap_->fixedLegBPS());

            boost::shared_ptr<SmileSection> sectionTmp =
                swaptionVolatility()->smileSection(fixingDate_, swapTenor_);

            adjustedLowerBound_ = settings_.lowerRateBound_;
            adjustedUpperBound_ = settings_.upperRateBound_;

            if(sectionTmp->volatilityType() == Normal) {
                // adjust lower bound if it was not set explicitly
                if(settings_.defaultBounds_)
                    adjustedLowerBound_ = std::min(adjustedLowerBound_, -adjustedUpperBound_);
            } else {
                // adjust bounds by section's shift
                adjustedLowerBound_ -= sectionTmp->shift();
                adjustedUpperBound_ -= sectionTmp->shift();
            }

            // if the section does not provide an atm level, we enhance it to
            // have one, no need to exit with an exception ...

            if (sectionTmp->atmLevel() == Null<Real>())
                smileSection_ = boost::make_shared<AtmSmileSection>(
                    sectionTmp, swapRateValue_);
            else
                smileSection_ = sectionTmp;

            // compute linear model's parameters

            Real gx = 0.0, gy = 0.0;
            for (Size i = 0; i < swap_->fixedLeg().size(); i++) {
                boost::shared_ptr<Coupon> c =
                    boost::dynamic_pointer_cast<Coupon>(swap_->fixedLeg()[i]);
                Real yf = c->accrualPeriod();
                Date d = c->date();
                Real pv = yf * discountCurve_->discount(d);
                gx += pv * GsrG(d);
                gy += pv;
            }

            Real gamma = gx / gy;
            Date lastd = swap_->fixedLeg().back()->date();

            a_ = discountCurve_->discount(paymentDate_) *
                 (gamma - GsrG(paymentDate_)) /
                 (discountCurve_->discount(lastd) * GsrG(lastd) +
                  swapRateValue_ * gy * gamma);

            b_ = discountCurve_->discount(paymentDate_) / gy -
                 a_ * swapRateValue_;
        }
    }

    Real LinearTsrPricer::strikeFromVegaRatio(Real ratio,
                                              Option::Type optionType,
                                              Real referenceStrike) const {

        Real a, b, min, max, k;
        if (optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k =
                std::min(smileSection_->maxStrike(), adjustedUpperBound_);
        } else {
            a = min = k =
                std::max(smileSection_->minStrike(), adjustedLowerBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        VegaRatioHelper h(&*smileSection_,
                          smileSection_->vega(swapRateValue_) * ratio);
        Brent solver;

        try {
            k = solver.solve(h, 1.0E-5, (a + b) / 2.0, a, b);
        }
        catch (...) {
            // use default value set above
        }

        return std::min(std::max(k, min), max);
    }

    Real LinearTsrPricer::strikeFromPrice(Real price, Option::Type optionType,
                                          Real referenceStrike) const {

        Real a, b, min, max, k;
        if (optionType == Option::Call) {
            a = swapRateValue_;
            min = referenceStrike;
            b = max = k =
                std::min(smileSection_->maxStrike(), adjustedUpperBound_);
        } else {
            a = min = k =
                std::max(smileSection_->minStrike(), adjustedLowerBound_);
            b = swapRateValue_;
            max = referenceStrike;
        }

        PriceHelper h(&*smileSection_, optionType, price);
        Brent solver;

        try {
            k = solver.solve(h, 1.0E-5, swapRateValue_, a, b);
        }
        catch (...) {
            // use default value set above
        }

        return std::min(std::max(k, min), max);
    }

    Real LinearTsrPricer::optionletPrice(Option::Type optionType,
                                         Real strike) const {

        if (optionType == Option::Call && strike >= adjustedUpperBound_)
            return 0.0;
        if (optionType == Option::Put && strike <= adjustedLowerBound_)
            return 0.0;

        // determine lower or upper integration bound (depending on option type)

        Real lower = strike, upper = strike;

        switch (settings_.strategy_) {

        case Settings::RateBound: {
            if (optionType == Option::Call)
                upper = adjustedUpperBound_;
            else
                lower = adjustedLowerBound_;
            break;
        }

        case Settings::VegaRatio: {
            // strikeFromVegaRatio ensures that returned strike is on the
            // expected side of strike
            Real bound =
                strikeFromVegaRatio(settings_.vegaRatio_, optionType, strike);
            if (optionType == Option::Call)
                upper = std::min(bound, adjustedUpperBound_);
            else
                lower = std::max(bound, adjustedLowerBound_);
            break;
        }

        case Settings::PriceThreshold: {
            // strikeFromPrice ensures that returned strike is on the expected
            // side of strike
            Real bound =
                strikeFromPrice(settings_.vegaRatio_, optionType, strike);
            if (optionType == Option::Call)
                upper = std::min(bound, adjustedUpperBound_);
            else
                lower = std::max(bound, adjustedLowerBound_);
            break;
        }

        case Settings::BSStdDevs : {
            Real atm = smileSection_->atmLevel();
            Real atmVol = smileSection_->volatility(atm);
            Real shift = smileSection_->shift();
            Real lowerTmp, upperTmp;
            if (smileSection_->volatilityType() == ShiftedLognormal) {
                upperTmp = (atm + shift) *
                               std::exp(settings_.stdDevs_ * atmVol -
                                        0.5 * atmVol * atmVol *
                                            smileSection_->exerciseTime()) -
                           shift;
                lowerTmp = (atm + shift) *
                               std::exp(-settings_.stdDevs_ * atmVol -
                                        0.5 * atmVol * atmVol *
                                            smileSection_->exerciseTime()) -
                           shift;
            } else {
                Real tmp = settings_.stdDevs_ * atmVol *
                           std::sqrt(smileSection_->exerciseTime());
                upperTmp = atm + tmp;
                lowerTmp = atm - tmp;
            }
            upper = std::min(upperTmp - shift, adjustedUpperBound_);
            lower = std::max(lowerTmp - shift, adjustedLowerBound_);
            break;
        }

        default:
            QL_FAIL("Unknown strategy (" << settings_.strategy_ << ")");
        }

        // compute the relevant integral

        Real result = 0.0;
        Real tmpBound;
        if (upper > lower) {
            tmpBound = std::min(upper, swapRateValue_);
            if (tmpBound > lower) {
                result += integrator_->operator()(
                    std::bind1st(std::mem_fun(&LinearTsrPricer::integrand),
                                 this),
                    lower, tmpBound);
            }
            tmpBound = std::max(lower, swapRateValue_);
            if (upper > tmpBound) {
                result += integrator_->operator()(
                    std::bind1st(std::mem_fun(&LinearTsrPricer::integrand),
                                 this),
                    tmpBound, upper);
            }
            result *= (optionType == Option::Call ? 1.0 : -1.0);
        }

        result += singularTerms(optionType, strike);

        return annuity_ * result * couponDiscountRatio_ *
               coupon_->accrualPeriod();
    }

    Real LinearTsrPricer::meanReversion() const { return meanReversion_->value(); }

    Rate LinearTsrPricer::swapletRate() const {
        return swapletPrice() /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::capletPrice(Rate effectiveCap) const {
        // caplet is equivalent to call option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = std::max(
                coupon_->swapIndex()->fixing(fixingDate_) - effectiveCap, 0.);
            Rate price =
                (gearing_ * Rs) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real capletPrice = optionletPrice(Option::Call, effectiveCap);
            return gearing_ * capletPrice;
        }
    }

    Rate LinearTsrPricer::capletRate(Rate effectiveCap) const {
        return capletPrice(effectiveCap) /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::floorletPrice(Rate effectiveFloor) const {
        // floorlet is equivalent to put option on fixing
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = std::max(
                effectiveFloor - coupon_->swapIndex()->fixing(fixingDate_), 0.);
            Rate price =
                (gearing_ * Rs) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real floorletPrice = optionletPrice(Option::Put, effectiveFloor);
            return gearing_ * floorletPrice;
        }
    }

    Rate LinearTsrPricer::floorletRate(Rate effectiveFloor) const {
        return floorletPrice(effectiveFloor) /
               (coupon_->accrualPeriod() *
                discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
    }

    Real LinearTsrPricer::swapletPrice() const {
        if (fixingDate_ <= today_) {
            // the fixing is determined
            const Rate Rs = coupon_->swapIndex()->fixing(fixingDate_);
            Rate price =
                (gearing_ * Rs + spread_) *
                (coupon_->accrualPeriod() *
                 discountCurve_->discount(paymentDate_) * couponDiscountRatio_);
            return price;
        } else {
            Real atmCapletPrice = optionletPrice(Option::Call, swapRateValue_);
            Real atmFloorletPrice = optionletPrice(Option::Put, swapRateValue_);
            return gearing_ * (coupon_->accrualPeriod() *
                                   discountCurve_->discount(paymentDate_) *
                                   swapRateValue_ * couponDiscountRatio_ +
                               atmCapletPrice - atmFloorletPrice) +
                   spreadLegValue_;
        }
    }
}


/**
1. Trigger when receive new map/tf (message filter new map and lidar scan)
2. use icp to match scan and merge into a new topic
**/
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <vector>
#include <string>
#include <math.h>
#include <signal.h>
#include <Eigen/Geometry>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Odometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/crop_box.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/tf.h>

using namespace pcl;

//std::string PATH = "/home/argsubt/ros_docker_ws/hw4/map.pcd";
std::string PATH = "/home/argsubt/ros_docker_ws/localization_comp/SDC Localization Competition Data/map/itri_map.pcd";

class IcpMapping{
    public:
        IcpMapping();
        void pcCallback(const sensor_msgs::PointCloud2ConstPtr &pc);
        Eigen::Matrix4f get_transfrom(std::string link_name);
        Eigen::Matrix4f get_initial_guess();

    private:
        ros::NodeHandle nh;
        ros::Subscriber pc_sub;
        ros::Publisher pc_pub;
        ros::Publisher map_filtered_pub;
        ros::Publisher odom_pub;

        Eigen::Matrix4f trans;
        Eigen::Matrix4f init_trans;
        pcl::PointCloud<PointXYZI>::Ptr icp_map;
        pcl::PointCloud<PointXYZI>::Ptr icp_map_voxel;
        pcl::PointCloud<PointXYZI>::Ptr pc_input;
        pcl::PointCloud<PointXYZI>::Ptr pc_input_voxel;
        //pcl::PointCloud<PointXYZI>::Ptr pc_input_voxel;
        //pcl::PointCloud<PointXYZI>::Ptr pc_last_batch;

        sensor_msgs::PointCloud2 ros_cloud_msg;

	    tf::TransformListener listener;
        tf::TransformBroadcaster broadcaster;

        // PointCloud<PointXYZI>::Ptr downsample(PointCloud<PointXYZI>::Ptr &in);
        VoxelGrid<PointXYZI> sor;
};

IcpMapping::IcpMapping(){
    icp_map.reset(new PointCloud<PointXYZI>());
    pc_input.reset(new PointCloud<PointXYZI>());
    pc_input_voxel.reset(new PointCloud<PointXYZI>());
    icp_map_voxel.reset(new PointCloud<PointXYZI>());

    pc_pub = nh.advertise<sensor_msgs::PointCloud2> ("/cloud_trans", 1);
    map_filtered_pub = nh.advertise<sensor_msgs::PointCloud2> ("/map_filtered", 1);
    odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

    pcl::io::loadPCDFile<pcl::PointXYZI>(PATH, *icp_map);
    std::cout << "Loaded" << endl;

    std::cerr << "PointCloud before filtering: " << icp_map->width * icp_map->height
        << " data points (" << pcl::getFieldsList (*icp_map) << ")." << std::endl;
    sor.setInputCloud(icp_map);
    sor.setLeafSize(0.5f, 0.5f, 0.5f);
    sor.filter(*icp_map_voxel);
    std::cerr << "PointCloud after filtering: " << icp_map_voxel->width * icp_map_voxel->height
        << " data points (" << pcl::getFieldsList (*icp_map_voxel) << ")." << std::endl;

    toROSMsg(*icp_map_voxel, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    map_filtered_pub.publish(ros_cloud_msg);

    //ros::Duration(1).sleep();

    //pc_last_batch.reset(new PointCloud<PointXYZI>());

    //wait for gps
    std::cout << "waiting for gps" << std::endl;
    init_trans = get_initial_guess();
    std::cout << "initial guess get" << std::endl;
    std::cout << init_trans << std::endl;

    pc_sub = nh.subscribe("/lidar_points", 1, &IcpMapping::pcCallback, this);
}

// PointCloud<PointXYZI> IcpMapping::downsample(PointCloud<PointXYZI>::Ptr &in){
//     PointCloud<PointXYZI>::Ptr pc_filtered;

//     sor.setInputCloud(in);
//     sor.setLeafSize(0.01f, 0.01f, 0.01f);
//     sor.filter(*pc_filtered);

//     return *pc_filtered;
// }

Eigen::Matrix4f IcpMapping::get_initial_guess(){
    Eigen::Matrix4f trans;
    tf::Transform transform;

    geometry_msgs::PointStampedConstPtr gps_point;
    gps_point = ros::topic::waitForMessage<geometry_msgs::PointStamped>("/gps", nh);

    tf::Quaternion q;
    q.setRPY(0, 0, -3.8078303612);
    Eigen::Quaternionf eq(q.w(), q.x(), q.y(), q.z());
	Eigen::Matrix3f mat = eq.toRotationMatrix();

    trans << mat(0,0), mat(0,1), mat(0,2), (*gps_point).point.x,
			mat(1,0), mat(1,1), mat(1,2), (*gps_point).point.y,
			mat(2,0), mat(2,1), mat(2,2), (*gps_point).point.z,
			0, 0, 0, 1;
	return trans;
}

Eigen::Matrix4f IcpMapping::get_transfrom(std::string link_name){
	tf::StampedTransform transform;
	Eigen::Matrix4f trans;

	try{
		ros::Duration five_seconds(5.0);
		listener.waitForTransform("base_link", link_name, ros::Time(0), five_seconds);
		listener.lookupTransform("base_link", link_name, ros::Time(0), transform);
	}
	catch (tf::TransformException ex){
		ROS_ERROR("%s",ex.what());
		return trans;
	}
	Eigen::Quaternionf q(transform.getRotation().getW(), \
		transform.getRotation().getX(), transform.getRotation().getY(), transform.getRotation().getZ());
	Eigen::Matrix3f mat = q.toRotationMatrix();
	trans << mat(0,0), mat(0,1), mat(0,2), transform.getOrigin().getX(),
			mat(1,0), mat(1,1), mat(1,2), transform.getOrigin().getY(),
			mat(2,0), mat(2,1), mat(2,2), transform.getOrigin().getZ(),
			0, 0, 0, 1;
	return trans;
}

void IcpMapping::pcCallback(const sensor_msgs::PointCloud2ConstPtr &pc){
	ROS_INFO("localizing");
	fromROSMsg(*pc, *pc_input);

    //=======cropbox filter=========================
    CropBox<PointXYZI> crop;

    crop.setMin(Eigen::Vector4f(-30,-15,-10,1.0)); //给定立体空间
    crop.setMax(Eigen::Vector4f(30,15,10,1.0));  //数据随意给的，具体情况分析
    crop.setInputCloud(pc_input);
    crop.setKeepOrganized(true);
    crop.setUserFilterValue(0.1f);
    crop.filter(*pc_input);

    trans = get_transfrom("velodyne");
    std::cout << init_trans << std::endl;
	transformPointCloud (*pc_input, *pc_input, trans);
    transformPointCloud (*pc_input, *pc_input, init_trans);

    ROS_INFO("transformed to world");

    //=======cropbox filter=========================
    // CropBox

    //=======passthrough filter=====================
    // PassThrough<PointXYZI> pass;
    // pass.setInputCloud (pc_input);
    // pass.setFilterFieldName ("z");
    // pass.setFilterLimits (-0.5, 1.5);
    // pass.filter (*pc_input);

    sor.setInputCloud(pc_input);
    sor.setLeafSize(0.7f, 0.7f, 0.7f);
    sor.filter(*pc_input_voxel);

    // toROSMsg(*pc_input_voxel, ros_cloud_msg);
	// ros_cloud_msg.header.frame_id = "world";
    // pc_pub.publish(ros_cloud_msg);

    if (icp_map_voxel->empty()){
        ROS_INFO("Empty cloud");
    }
    else{
        ROS_INFO("Aligning");
        IterativeClosestPoint<PointXYZI, PointXYZI> icp;
        icp.setInputSource(pc_input_voxel);
        icp.setInputTarget(icp_map_voxel);
        icp.setMaximumIterations (800);
        icp.setTransformationEpsilon (1e-9);
        icp.setRANSACOutlierRejectionThreshold (0.05);
        icp.setMaxCorrespondenceDistance (5);
        //icp.setEuclideanFitnessEpsilon (0.01);
        icp.align(*pc_input_voxel);
        //transformPointCloud (*pc_input, *pc_input, (icp.getFinalTransformation()));
        // *icp_map += *pc_input;
        // sor.setInputCloud(icp_map_voxel);
        // sor.setLeafSize(0.2f, 0.2f, 0.2f);
        // sor.filter(*icp_map_voxel);

        //copyPointCloud(*pc_input, *pc_last);
        ROS_INFO("Score: %lf", icp.getFitnessScore());
        init_trans = (icp.getFinalTransformation()) * init_trans ;
        std::cout << init_trans << std::endl;
    }

    ROS_INFO("Publishing");

    toROSMsg(*pc_input_voxel, ros_cloud_msg);
	ros_cloud_msg.header.frame_id = "world";
    pc_pub.publish(ros_cloud_msg);

    tf::Transform world_trans;
    Eigen::Affine3f affine_trans;
    affine_trans.matrix() = init_trans;
    tf::transformEigenToTF(affine_trans.cast<double>(), world_trans);
    broadcaster.sendTransform(tf::StampedTransform(world_trans, ros::Time::now(), "world", "base_link"));

    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();
    odom.header.frame_id = "world";

    //set the position
    odom.pose.pose.position.x = world_trans.getOrigin().getX();
    odom.pose.pose.position.y = world_trans.getOrigin().getY();
    odom.pose.pose.position.z = world_trans.getOrigin().getZ();

    geometry_msgs::Quaternion quat_msg;
    quaternionTFToMsg(world_trans.getRotation() , quat_msg);
    odom.pose.pose.orientation = quat_msg;

    //publish the message
    odom_pub.publish(odom);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "icp_mapping");

    IcpMapping IM;

    ROS_INFO("\033[1;32m---->\033[0m Icp Mapping Started.");

    ros::spin();
    return 0;
}


#include <cmath>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#include <Eigen/Geometry>

#include <gtest/gtest.h>

#include <nlohmann/json/json.hpp>

#include "geometry/layout.hpp"
#include "geometry/polygon.hpp"
#include "quadtree/tree.hpp"
#include "terrain.hpp"
#include "io/readers.hpp"
#include "io/writers.hpp"

using std::cerr;
using std::endl;
using std::string;
using std::vector;

using Eigen::Vector2d;

using nlohmann::json;

using terrain::geometry::Layout;

namespace terrain::quadtree {

static const Layout default_layout; // i.e. create with default constructor

void assert_layouts_match( const Layout& first, const Layout& second ){
    EXPECT_DOUBLE_EQ( first.get_precision(), second.get_precision());
    EXPECT_DOUBLE_EQ( first.get_x(),         second.get_x());
    EXPECT_DOUBLE_EQ( first.get_y(),         second.get_y());
    EXPECT_DOUBLE_EQ( first.get_width(),     second.get_width());
    EXPECT_EQ(        first.get_dimension(), second.get_dimension());
    EXPECT_EQ(        first.get_size(),      second.get_size());
}

TEST( QuadTreeTest, CalculateMemoryUsage){
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    EXPECT_EQ(sizeof(Terrain<Tree>), 32);
    EXPECT_EQ(sizeof(Layout), 64);
    EXPECT_EQ(sizeof(Tree), 72);     // composed of: Layout, root-pointer
    EXPECT_EQ(sizeof(Vector2d), 16);
    EXPECT_EQ(sizeof(Node), 40);
}

TEST(QuadTreeTest, ConstructDefault) {
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    EXPECT_TRUE( tree.root );
    EXPECT_TRUE( tree.root->is_leaf() );
}

TEST(QuadTreeTest, ConstructByCenterAndSize) {
    Tree tree({1., 1, 1, 256});
    Terrain terrain(tree);

    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(), 1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),         1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),         1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),   256.);
    EXPECT_EQ( terrain.get_layout().get_dimension(),      256);
    EXPECT_EQ( terrain.get_layout().get_size(),         65536);

    // functional tests:
    // (1) in bounds
    ASSERT_TRUE(tree.contains({0,0}));
    // (2) out of bounds in x
    ASSERT_FALSE(tree.contains({200,0}));
    // (3) out of bounds in y
    ASSERT_FALSE(tree.contains({0,200}));
    // (4) out of bounds in both x AND y
    ASSERT_FALSE(tree.contains({130,130}));
}

TEST( QuadTreeTest, ConstructAndSetBounds) {
    Tree tree({1., 5, 3, 17});
    Terrain terrain(tree);

    // pre-conditions
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(), 1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),         5.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),         3.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),    32.);
    EXPECT_EQ( terrain.get_layout().get_dimension(),       32);
    EXPECT_EQ( terrain.get_layout().get_size(),          1024);

    const Layout new_layout = {32, 1.,1, 256};

    // test target
    tree.reset(new_layout);
    // test target

    // post-conditions
    assert_layouts_match( terrain.get_layout(), new_layout);
}

TEST( QuadTreeTest, LoadMalformedSource){
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    // this is simply a malformed document.  It should not parse.
    std::istringstream source(R"({"bounds": {"x": 100, "y": 100, "width": )");

    // this should fail. Gracefully.
    EXPECT_FALSE( terrain::io::load_from_json_stream(terrain, source));

    // these tests should be *exactly* the same as before the 'load' call
    assert_layouts_match( terrain.get_layout(), default_layout);
}

TEST( QuadTreeTest, LoadBoundsOnly){
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    // construct a valid document, with correct fields, but simply .. missing things:
    std::stringstream stream(R"({"bounds": {"x": 100, "y": 100, "width": 64}} )");

    // this should fail. (gracefully)
    EXPECT_FALSE( terrain::io::load_from_json_stream(terrain, stream));

    // these tests should be *exactly* the same as before the 'load' call
    assert_layouts_match( terrain.get_layout(), default_layout);
}

TEST( QuadTreeTest, LoadValidTree){
    Tree tree;
    Terrain terrain(tree);
    assert_layouts_match( terrain.get_layout(), default_layout);

    const json source = {
            {"NE", {
                {"NE", 0.0},
                {"NW", 0.0},
                {"SE", 0.0},
                {"SW", 0.0}}},
            {"NW", 0.0},
            {"SE", 0.0},
            {"SW", 0.0}};

    tree.load_tree(source);

    // // debug
    // tree.debug_tree();

    // test shape
    ASSERT_FALSE( tree.root->is_leaf());
    ASSERT_FALSE( tree.root->get_northeast()->is_leaf());
    ASSERT_TRUE( tree.root->get_southwest()->is_leaf());
}

TEST( QuadTreeTest, CalculateFullLoading){
    EXPECT_EQ( Tree::calculate_complete_tree(0), 1);
    EXPECT_EQ( Tree::calculate_complete_tree(1), 5);
    EXPECT_EQ( Tree::calculate_complete_tree(2), 21);
    ASSERT_EQ( Tree::calculate_complete_tree(3), 85);
    ASSERT_EQ( Tree::calculate_complete_tree(4), 341);
    ASSERT_EQ( Tree::calculate_complete_tree(5), 1365);
}

TEST( QuadTreeTest, CalculateLoadFactor){
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    const json source = {
        {"NE", {
            {"NE", 0.0},
            {"NW", 0.0},
            {"SE", {
                {"NE", 0.0},
                {"NW", 0.0},
                {"SE", 0.0},
                {"SW", 0.0}}},
            {"SW", 0.0}}},
        {"NW", 0.0},
        {"SE", 0.0},
        {"SW", 0.0}};

    tree.load_tree(source);

    // // debug
    // tree.debug_tree();

    // test shape
    ASSERT_FALSE( tree.root->is_leaf());
    ASSERT_FALSE( tree.root->get_northeast()->is_leaf());
    ASSERT_TRUE( tree.root->get_northeast()->get_southeast()->get_southeast()->is_leaf());
    ASSERT_TRUE( tree.root->get_southwest()->is_leaf());

    ASSERT_EQ( tree.get_height(), 3);
    ASSERT_EQ( tree.size(), 13);
    ASSERT_NEAR( tree.get_load_factor(),  0.03812, 1e-4);
}

TEST(QuadTreeTest, LoadGridFromJSON) {
    Tree tree;
    Terrain terrain(tree);

    assert_layouts_match( terrain.get_layout(), default_layout);

    std::istringstream stream(R"(
        {"layout": {"precision": 32.0, "x": 1, "y": 1, "width": 256},
         "grid":[[88, 88, 88, 88,  0, 88, 88, 88],
                 [88, 88, 88,  0,  0,  0, 88, 88],
                 [88, 88,  0,  0,  0,  0,  0, 88],
                 [88,  0,  0,  0,  0,  0,  0,  0],
                 [ 0,  0,  0,  0, 88, 88, 88, 88],
                 [88,  0,  0,  0, 88, 88, 88, 88],
                 [88, 88,  0,  0, 88, 88, 88, 88],
                 [88, 88, 88,  0, 88, 88, 88, 88]]} )");

    // test target
    ASSERT_TRUE( terrain::io::load_from_json_stream(terrain, stream));
    // test target

    // // DEBUG
    // terrain.debug();

    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(), 32.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),          1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),          1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),    256.);
    EXPECT_EQ( terrain.get_layout().get_dimension(),         8);
    EXPECT_EQ( terrain.get_layout().get_size(),             64);

    {// test tree shape
        EXPECT_EQ(tree.get_height(), 3);
        EXPECT_EQ(tree.size(), 41);
        EXPECT_FALSE(tree.root->is_leaf());

        // spot check #1: RT-NE-SW-quadrant
        const auto* r_ne_sw = tree.root->get_northeast()->get_southwest();
        ASSERT_TRUE(r_ne_sw->is_leaf());
        ASSERT_DOUBLE_EQ(r_ne_sw->get_value(), 0);

        // spot check #2: RT-NW-NE-NW quadrant
        const auto* r_ne_nw = tree.root->get_northwest()->get_northeast()->get_northwest();
        ASSERT_TRUE(r_ne_nw->is_leaf());
        ASSERT_DOUBLE_EQ(r_ne_nw->get_value(), 88);
    }

    const Tree& frozen = tree;
    EXPECT_EQ( frozen.classify({  16,   16}),   0);
    EXPECT_EQ( frozen.classify({  48,   48}),   0);
    EXPECT_EQ( frozen.classify({  72,   72}),  88);
    EXPECT_EQ( frozen.classify({ 104,  104}),  88);

    // [-127, -95, -63, -31, 1, 33, 65, 97, 129]
    EXPECT_EQ( frozen.classify({ -70,  130}),  88);
    EXPECT_EQ( frozen.classify({ -70,  129}),  88);
    EXPECT_EQ( frozen.classify({ -70,   97}),  88);
    EXPECT_EQ( frozen.classify({ -70,   65}),  88);
    EXPECT_EQ( frozen.classify({ -70,   33}),   0);
    EXPECT_EQ( frozen.classify({ -70,    1}),   0);
    EXPECT_EQ( frozen.classify({ -70,  -31}),   0);
    EXPECT_EQ( frozen.classify({ -70,  -63}),  88);
    EXPECT_EQ( frozen.classify({ -70,  -95}),  88);
    EXPECT_EQ( frozen.classify({ -70, -127}),  88);
    EXPECT_EQ( frozen.classify({ -70, -130}),  88);

    EXPECT_EQ( frozen.classify({  15,  130}),   0);
    EXPECT_EQ( frozen.classify({  15,  129}),   0);
    EXPECT_EQ( frozen.classify({  15,   97}),   0);
    EXPECT_EQ( frozen.classify({  15,   65}),   0);
    EXPECT_EQ( frozen.classify({  15,   33}),   0);
    EXPECT_EQ( frozen.classify({  15,    1}),  88);
    EXPECT_EQ( frozen.classify({  15,  -31}),  88);
    EXPECT_EQ( frozen.classify({  15,  -63}),  88);
    EXPECT_EQ( frozen.classify({  15,  -95}),  88);
    EXPECT_EQ( frozen.classify({  15, -127}),  88);
    EXPECT_EQ( frozen.classify({  15, -130}),  88);
}

TEST(QuadTreeTest, LoadPolygonFromJSON) {
    quadtree::Tree tree;
    Terrain terrain(tree);

    constexpr double desired_width = 16.;   // overall boundary
    constexpr double desired_precision = 1.0;
    // =====
    json source = generate_diamond( desired_width, desired_precision);

    std::istringstream stream(source.dump());

    // // DEBUG
    // cerr << "======\n" << source.dump(4) << "\n======\n" << endl;

    ASSERT_TRUE( terrain::io::load_from_json_stream(terrain, stream));

    // // DEBUG
    // terrain.debug();

    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(),  1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),         desired_width/2);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),         desired_width/2);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),     desired_width);
    EXPECT_EQ( terrain.get_layout().get_dimension(),        16);
    EXPECT_EQ( terrain.get_layout().get_size(),             256);

    ASSERT_EQ( tree.classify({ 4.5, 15.5}), 0x99);
    ASSERT_EQ( tree.classify({ 4.5, 14.5}), 0x99);
    ASSERT_EQ( tree.classify({ 4.5, 13.5}), 0x99);
    ASSERT_EQ( tree.classify({ 4.5, 12.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5, 11.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5, 10.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  9.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  8.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  7.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  6.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  5.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  4.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  3.5}),    0);
    ASSERT_EQ( tree.classify({ 4.5,  2.5}), 0x99);
    ASSERT_EQ( tree.classify({ 4.5,  1.5}), 0x99);
    ASSERT_EQ( tree.classify({ 4.5,  0.5}), 0x99);

    ASSERT_EQ( tree.classify({  0.5, 5.5}), 0x99);
    ASSERT_EQ( tree.classify({  1.5, 5.5}), 0x99);
    ASSERT_EQ( tree.classify({  2.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  3.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  4.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  5.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  6.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  7.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  8.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({  9.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({ 10.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({ 11.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({ 12.5, 5.5}),    0);
    ASSERT_EQ( tree.classify({ 13.5, 5.5}), 0x99);
    ASSERT_EQ( tree.classify({ 14.5, 5.5}), 0x99);
    ASSERT_EQ( tree.classify({ 15.5, 5.5}), 0x99);
}

TEST( QuadTreeTest, SearchExplicitTree) {
    Tree tree({50, 0, 0, 100});
    Terrain terrain(tree);
    tree.root->split(50, 100);

    ASSERT_FALSE(tree.root->is_leaf());

    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(), 32.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),          0.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),          0.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),    128);
    EXPECT_EQ( terrain.get_layout().get_dimension(),         4);
    EXPECT_EQ( terrain.get_layout().get_size(),             16);

    cell_value_t false_value = 5;
    cell_value_t true_value = 14;

    // Set Quadrant I:
    tree.root->get_northeast()->set_value(true_value);
    // Set Quadrdant II:
    tree.root->get_northwest()->set_value(false_value);
    // Set Quadrant III:
    tree.root->get_southwest()->set_value(true_value);
    // Set Quadrant IV:
    tree.root->get_southeast()->set_value(false_value);

    // // DEBUG
    // tree.debug();

    // .... Out Of Bounds: snaps to nearest node...
    ASSERT_EQ(tree.classify({110, 110}), true_value);

    // functional tests:
    // .... Quadrant I:
    EXPECT_EQ(tree.classify({ 25,  25}), true_value);
    // .... Quadrant II:
    EXPECT_EQ(tree.classify({-25,  25}), false_value);
    // .... Quadrant III:
    EXPECT_EQ(tree.classify({-25, -25}), true_value);
    // .... Quadrant IV:
    EXPECT_EQ(tree.classify({ 25, -25}), false_value);
}

TEST( QuadTreeTest, SampleTree ){
    quadtree::Tree tree;
    Terrain terrain(tree);

    std::istringstream stream(R"(
        {"layout": {"precision": 1, "x": 2, "y": 2, "width": 4},
         "grid":[[  1,  2,  3,  4],
                 [  5,  6,  7,  8],
                 [  9, 10, 11, 12],
                 [ 13, 14, 15, 16]]} )");

    EXPECT_TRUE( terrain::io::load_from_json_stream(terrain, stream));

    // // DEBUG
    // tree.debug_tree(true);
    // terrain.debug();

    // Out-Of-Bounds call -- returns nearest node.
    const Sample s0 = tree.sample({  5,  5});
    ASSERT_TRUE( Vector2d(3.5, 3.5) == s0.at );
    ASSERT_EQ( s0.is, 4);

    const Sample s1 = tree.sample({  0,  0});
    ASSERT_TRUE( Vector2d(0.5, 0.5) == s1.at );
    ASSERT_EQ( s1.is,  13);

    const Sample s2 = tree.sample({  0.9,  0.9});
    ASSERT_TRUE( Vector2d(0.5, 0.5) == s2.at );
    ASSERT_EQ( s2.is,  13);

    const Sample s3 = tree.sample({  2.4,  2.7});
    ASSERT_TRUE( Vector2d(2.5, 2.5) == s3.at );
    ASSERT_EQ( s3.is,   7);

    const Sample s4 = tree.sample({  1.7,  3.3});
    ASSERT_TRUE( Vector2d(1.5, 3.5) == s4.at );
    ASSERT_EQ( s4.is,   2);
}

// TEST( QuadTreeTest, InterpolateTree){
//     Tree tree({{1,1}, 64}, 1.0);
//     Terrain terrain(tree);
//     tree.root->split();

//     // Set Quadrant I:
//     tree.root->get_northeast()->set_value(0);
//     // Set Quadrdant II:
//     tree.root->get_northwest()->set_value(50);
//     // Set Quadrant III:
//     tree.root->get_southwest()->set_value(100);
//     // Set Quadrant IV:
//     tree.root->get_southeast()->set_value(50);

//     vector<TestPoint> test_cases;
//     test_cases.emplace_back( -35,    4, cell_default_value);  // Start out of bounds
//     test_cases.emplace_back( -33,    4, cell_default_value);
//     test_cases.emplace_back( -32,    4, cell_default_value);
//     test_cases.emplace_back( -31,    4,   70);  // border of tree
//     test_cases.emplace_back( -30.9,  4,   70);
//     test_cases.emplace_back( -30,    4,   70);
//     test_cases.emplace_back( -20,    4,   70);
//     test_cases.emplace_back( -17,    4,   70);
//     test_cases.emplace_back( -16,    4,   70);
//     test_cases.emplace_back( -15.1,  4,   70);
//     test_cases.emplace_back( -15.0,  4,   70);  // breakpoint - center of outer cell
//     test_cases.emplace_back( -14.9,  4,   70);
//     test_cases.emplace_back( -10,    4,   62);
//     test_cases.emplace_back( - 5,    4,   54);
//     test_cases.emplace_back(   0,    4,   47);
//     test_cases.emplace_back(   1,    4,   45);  // midpoint
//     test_cases.emplace_back(   2,    4,   43);
//     test_cases.emplace_back(  10,    4,   31);
//     test_cases.emplace_back(  14,    4,   25);
//     test_cases.emplace_back(  15,    4,   23);
//     test_cases.emplace_back(  16,    4,   22);  // breakpoint - center of outer cell
//     test_cases.emplace_back(  17,    4,   20);
//     test_cases.emplace_back(  20,    4,   20);
//     test_cases.emplace_back(  30,    4,   20);
//     test_cases.emplace_back(  31,    4,   20);
//     test_cases.emplace_back(  32,    4,   20);
//     test_cases.emplace_back(  33,    4,   20);  // border of tree
//     test_cases.emplace_back(  34,    4, cell_default_value);
//     test_cases.emplace_back(  35,    4, cell_default_value);

//     // =====================================
//     for( const TestPoint& expect : test_cases){
//         const auto& actual_value = tree.interp({expect.x, expect.y});

//         std::ostringstream buf;
//         buf << "@@  x=" << expect.x << "  y=" << expect.y << "  v=" << expect.value << endl;

//         ASSERT_EQ(actual_value, expect.value) << buf.str();
//     }
// }

TEST( QuadTreeTest, SavePNG) {
    Terrain<Tree> terrain;
    const json source = generate_diamond(  16.,   // boundary_width
                                            0.4);  // desired_precision);

    std::istringstream stream(source.dump());

    ASSERT_TRUE( terrain::io::load_from_json_stream(terrain, stream));

    // // DEBUG
    // terrain.debug();

    EXPECT_DOUBLE_EQ( terrain.get_layout().get_precision(),  1.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_x(),          8.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_y(),          8.);
    EXPECT_DOUBLE_EQ( terrain.get_layout().get_width(),     16);
    EXPECT_EQ( terrain.get_layout().get_dimension(),        16);
    EXPECT_EQ( terrain.get_layout().get_size(),            256);

    // // Because this manually tested, comment this block until needed:
    // const string filename("tree.test.png");
    // terrain.to_png(filename);
}

TEST( QuadTreeTest, WriteLoadCycle){
    Tree source_tree({32, 11,11, 128});
    Terrain source_terrain(source_tree);

    EXPECT_DOUBLE_EQ( source_terrain.get_layout().get_precision(), 32.);
    EXPECT_DOUBLE_EQ( source_terrain.get_layout().get_x(),         11.);
    EXPECT_DOUBLE_EQ( source_terrain.get_layout().get_y(),         11.);
    EXPECT_DOUBLE_EQ( source_terrain.get_layout().get_width(),    128);
    EXPECT_EQ( source_terrain.get_layout().get_dimension(),         4);
    EXPECT_EQ( source_terrain.get_layout().get_size(),             16);

    source_tree.root->split(32, 128);

    // Set interesting values
    source_tree.root->get_northeast()->get_northeast()->set_value(21);
    source_tree.root->get_northeast()->get_northwest()->set_value(22);
    source_tree.root->get_northeast()->get_southeast()->set_value(23);
    source_tree.root->get_northeast()->get_southwest()->set_value(24);

    source_tree.root->get_northwest()->get_northeast()->set_value(11);
    source_tree.root->get_northwest()->get_northwest()->set_value(11);
    source_tree.root->get_northwest()->get_southeast()->set_value(11);
    source_tree.root->get_northwest()->get_southwest()->set_value(11);

    source_tree.root->get_southwest()->get_northeast()->set_value(31);
    source_tree.root->get_southwest()->get_northwest()->set_value(32);
    source_tree.root->get_southwest()->get_southeast()->set_value(33);
    source_tree.root->get_southwest()->get_southwest()->set_value(34);

    source_tree.root->get_southeast()->get_northeast()->set_value(55);
    source_tree.root->get_southeast()->get_northwest()->set_value(55);
    source_tree.root->get_southeast()->get_southeast()->set_value(55);
    source_tree.root->get_southeast()->get_southwest()->set_value(55);

    // // DEBUG
    // source_tree.debug_tree();

    source_tree.root->prune();

    // // DEBUG
    // source_tree.debug_tree();

    // write tree #1 to the serialization buffer
    std::stringstream buffer;
    ASSERT_TRUE( terrain::io::to_json(source_terrain, buffer));

    // // DEBUG
    // cerr << buffer.str() << endl;

    // load contents into another tree
    Tree load_tree;
    Terrain load_terrain(load_tree);

    buffer.seekg(0);
    ASSERT_TRUE( terrain::io::load_from_json_stream(load_terrain, buffer));

    // // DEBUG
    // load_tree.debug_tree();

    // test contents of test_tree
    { // test layout:
        EXPECT_DOUBLE_EQ( load_terrain.get_layout().get_precision(), 32.);
        EXPECT_DOUBLE_EQ( load_terrain.get_layout().get_x(),         11.);
        EXPECT_DOUBLE_EQ( load_terrain.get_layout().get_y(),         11.);
        EXPECT_DOUBLE_EQ( load_terrain.get_layout().get_width(),    128);
        EXPECT_EQ( load_terrain.get_layout().get_dimension(),         4);
        EXPECT_EQ( load_terrain.get_layout().get_size(),             16);
    }
    { // test tree shape
        auto & root = load_tree.root;
        ASSERT_FALSE(root->is_leaf());
        {
            auto* ne_quad = root->get_northeast();
            ASSERT_FALSE(ne_quad->is_leaf());
            ASSERT_TRUE(ne_quad->get_northeast()->is_leaf());
            ASSERT_TRUE(ne_quad->get_northwest()->is_leaf());
            ASSERT_TRUE(ne_quad->get_southwest()->is_leaf());
            ASSERT_TRUE(ne_quad->get_southeast()->is_leaf());
        }
        ASSERT_TRUE(root->get_northwest()->is_leaf());
        ASSERT_TRUE(root->get_southeast()->is_leaf());
        {
            auto* sw_quad = root->get_northeast();
            ASSERT_FALSE(sw_quad->is_leaf());
            ASSERT_TRUE(sw_quad->get_northeast()->is_leaf());
            ASSERT_TRUE(sw_quad->get_northwest()->is_leaf());
            ASSERT_TRUE(sw_quad->get_southwest()->is_leaf());
            ASSERT_TRUE(sw_quad->get_southeast()->is_leaf());
        }
    }
}

} // namespace quadtree


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_FACT_4_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_FACT_4_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Fact_4 generic tag

     Represents the Fact_4 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Fact_4,double
                                , 24, 0x41c00000, 0x4038000000000000ll
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Fact_4, Site> dispatching_Fact_4(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Fact_4, Site>();
   }
   template<class... Args>
   struct impl_Fact_4;
  }
  /*!
    Generates 4! that is 24

    @par Semantic:

    @code
    T r = Fact_4<T>();
    @endcode

    is similar to:

    @code
    T r = T(24);
    @endcode

    @par Alias

    Twentyfour

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Fact_4, Fact_4)
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Fact_4, Twentyfour)
} }

#include <boost/simd/constant/common.hpp>

#endif


#pragma once
#include <memory>
#include <Eigen/Dense>
#include "SdfObject.hpp"
#include "../Ray.hpp"
#include "../accelerate/Bound3.hpp"

class SdfSphere : public SdfObject
{
public:
    SdfSphere(Eigen::Vector3f position, float radis) : SdfObject(position), radis(radis){};

    float sdf(const Eigen::Vector3f &position) const override
    {
        return (position - this->position).norm() - radis;
    };

    std::unique_ptr<Bound3> build_bound3() const override
    {
        auto min = position - Eigen::Vector3f::Ones(radis);
        auto max = position + Eigen::Vector3f::Ones(radis);
        return std::make_unique<Bound3>(min, max);
    };

private:
    float radis;
};


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Point2.cpp
 * @brief   2D Point
 * @author  Frank Dellaert
 */

#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <cmath>

using namespace std;

namespace gtsam {

/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2);

static const Matrix oneOne = Matrix_(1, 2, 1.0, 1.0);

/* ************************************************************************* */
void Point2::print(const string& s) const {
  cout << s << *this << endl;
}

/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
  return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}

/* ************************************************************************* */
double Point2::norm(boost::optional<Matrix&> H) const {
  double r = sqrt(x_ * x_ + y_ * y_);
  if (H) {
    Matrix D_result_d;
    if (fabs(r) > 1e-10)
      D_result_d = Matrix_(1, 2, x_ / r, y_ / r);
    else
      D_result_d = oneOne; // TODO: really infinity, why 1 here??
    *H = D_result_d;
  }
  return r;
}

/* ************************************************************************* */
static const Matrix I2 = eye(2);
double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  Point2 d = point - *this;
  if (H1 || H2) {
    Matrix H;
    double r = d.norm(H);
    if (H1) *H1 = -H;
    if (H2) *H2 =  H;
    return r;
  } else
    return d.norm();
}

/*
 * Calculate f and h, respectively the parallel and perpendicular distance of
 * the intersections of two circles along and from the line connecting the centers.
 * Both are dimensionless fractions of the distance d between the circle centers.
 * If the circles do not intersect or they are identical, returns boost::none.
 * If one solution (touching circles, as determined by tol), h will be exactly zero.
 * h is a good measure for how accurate the intersection will be, as when circles touch
 * or nearly touch, the intersection is ill-defined with noisy radius measurements.
 * @param R_d : R/d, ratio of radius of first circle to distance between centers
 * @param r_d : r/d, ratio of radius of second circle to distance between centers
 * @param tol: absolute tolerance below which we consider touching circles
 */
/* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/
boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
    double tol) {

  double R2_d2 = R_d*R_d; // Yes, RD-D2 !
  double f = 0.5 + 0.5*(R2_d2 - r_d*r_d);
  double h2 = R2_d2 - f*f; // just right triangle rule

  // h^2<0 is equivalent to (d > (R + r) || d < (R - r))
  // Hence, there are only solutions if >=0
  if (h2<-tol) return boost::none; // allow *slightly* negative
  else if (h2<tol) return Point2(f,0.0); // one solution
  else return Point2(f,sqrt(h2)); // two solutions
}

/* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
    boost::optional<Point2> fh) {

  list<Point2> solutions;
  // If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r)
  if (fh) {
    // vector between circle centers
    Point2 c12 = c2-c1;

    // Determine p2, the point where the line through the circle
    // intersection points crosses the line between the circle centers.
    Point2 p2 = c1 + fh->x() * c12;

    // If h == 0, the circles are touching, so just return one point
    if (fh->y()==0.0)
      solutions.push_back(p2);
    else {
      // determine the offsets of the intersection points from p
      Point2 offset = fh->y() * Point2(-c12.y(), c12.x());

      // Determine the absolute intersection points.
      solutions.push_back(p2 + offset);
      solutions.push_back(p2 - offset);
    }
  }
  return solutions;
}

/* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
    double r2, double tol) {

  // distance between circle centers.
  double d = c1.dist(c2);

  // centers coincide, either no solution or infinite number of solutions.
  if (d<1e-9) return list<Point2>();

  // Calculate f and h given normalized radii
  double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
  boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d);

  // Call version that takes fh
  return CircleCircleIntersection(c1, c2, fh);
}

/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
  os << '(' << p.x() << ", " << p.y() << ')';
  return os;
}

} // namespace gtsam


#include <vector>
#include <iostream>
#include <algorithm>
#include <chrono>
#include "randutils.hpp" // see https://gist.github.com/imneme/540829265469e673d045
#include <boost/sort/spreadsort/spreadsort.hpp>
#include "drop_merge_sort.hpp"

using namespace std::chrono;

randutils::default_rng rng;

template<typename T>
std::vector<T> randomize(float factor) {
    int max = 1000000;
    std::vector<T> ret(max);
    for (int i = 0; i < max; ++i) {
        if (rng.uniform(0.0, 1.0) < factor)
            ret[i] = rng.uniform(0, max-1);
        else
            ret[i] = i;
    }
    return ret;
}
template<>
std::vector<std::string> randomize<std::string>(float factor) {
    int max = 100000;
    std::vector<std::string> ret(max);
    for (int i = 0; i < max; ++i) {
        std::string s;
        if (rng.uniform(0.0, 1.0) < factor)
            s = std::to_string(rng.uniform(0, max-1));
        else
            s = std::to_string(i);
        ret[i] = std::string(100 - s.size(), '0') + s;
    }
    return ret;
}

template<typename T, typename Sort>
float measure(float factor, Sort sort) {
    size_t total = 0;
    constexpr size_t sz = 5;
    for (int i = 0; i < sz; ++i) {
        auto tab = randomize<T>(factor);
        //auto copy = tab;
        auto t1 = high_resolution_clock::now();
        sort(tab);
        auto t2 = high_resolution_clock::now();
        size_t time = duration_cast<microseconds>(t2-t1).count();
        total += time;

        // sanity check
        //std::sort(copy.begin(), copy.end());
        //if(copy != tab)
        //    throw "asdf";
    }
    return total / (1000.0 * sz);
}

template<typename T>
void benchmark() {
    for (int i = 0; i <= 100; ++i) {
        float factor = i * 0.01;
        auto dt1 = measure<T>(factor, [](auto &tab){dmsort(std::begin(tab), std::end(tab));});
        auto dt2 = measure<T>(factor, [](auto &tab){std::sort(std::begin(tab), std::end(tab));});
        auto dt3 = measure<T>(factor, [](auto &tab){boost::sort::spreadsort::spreadsort(std::begin(tab), std::end(tab));});
        std::cout << (factor) << "\t" << dt1 << "\t" << dt2 << "\t" << dt3 << "\n";
    }
    std::cout << "\n";
}

int main(){
    benchmark<int>();
    benchmark<std::string>();
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file HuberLossFunc.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_HUBER_LOSS_FUNCTION_HPP
#define STEAM_HUBER_LOSS_FUNCTION_HPP

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>

#include <steam/problem/lossfunc/LossFunctionBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Huber loss function class
//////////////////////////////////////////////////////////////////////////////////////////////
class HuberLossFunc : public LossFunctionBase
{
 public:

  /// Convenience typedefs
  typedef boost::shared_ptr<HuberLossFunc> Ptr;
  typedef boost::shared_ptr<const HuberLossFunc> ConstPtr;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor -- k is the `threshold' based on number of std devs (1-3 is typical)
  //////////////////////////////////////////////////////////////////////////////////////////////
  HuberLossFunc(double k) : k_(k) {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Cost function (basic evaluation of the loss function)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double cost(double whitened_error_norm) const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Weight for iteratively reweighted least-squares (influence function div. by error)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual double weight(double whitened_error_norm) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Huber constant
  //////////////////////////////////////////////////////////////////////////////////////////////
  double k_;
};

} // steam

#endif // STEAM_HUBER_LOSS_FUNCTION_HPP


#pragma once
#include <cmath>
#include <vector>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/access.hpp>
#include "BaseNeuron.hpp"
#include "NeuronModel.hpp"
#include "../../optimizer/StochasticGradientDescent.hpp"
#include "activation_function/ActivationFunction.hpp"
#include "../../../tools/Tools.hpp"

namespace snn::internal
{
    template <class Derived>
    class  Neuron : public BaseNeuron<Derived>
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

    protected:

        int numberOfInputs;
        std::vector<float> weights;
        float bias;

        std::vector<float> previousDeltaWeights;
        std::vector<float> lastInputs;
        std::vector<float> errors;

        float sum = 0;

        activation activationFunction;
        std::shared_ptr<ActivationFunction> outputFunction;

        static float randomInitializeWeight(int numberOfInputs);

    public:
        Neuron() = default; // use restricted to Boost library only
        Neuron(NeuronModel model, std::shared_ptr<NeuralNetworkOptimizer> optimizer);
        Neuron(const Neuron& neuron) = default;
        ~Neuron() = default;

        [[nodiscard]] int isValid() const;

        [[nodiscard]] std::vector<float> getWeights() const;
        [[nodiscard]] int getNumberOfParameters() const;
        [[nodiscard]] int getNumberOfInputs() const;

        bool operator==(const Neuron& neuron) const;
        bool operator!=(const Neuron& neuron) const;
    };

    template <class Derived>
    template <class Archive>
    void Neuron<Derived>::serialize(Archive& ar, unsigned version)
    {
        boost::serialization::void_cast_register<Neuron, BaseNeuron<Derived>>();
        ar & boost::serialization::base_object<BaseNeuron<Derived>>(*this);
        ar & this->numberOfInputs;
        ar & this->weights;
        ar & this->bias;
        ar & this->previousDeltaWeights;
        ar & this->lastInputs;
        ar & this->errors;
        ar & this->sum;
        ar & this->activationFunction;
        this->outputFunction = ActivationFunction::get(activationFunction);
    }

    #include "Neuron.tpp"
}