//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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 NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_IMPL_TRIGO_TRIG_REDUCTION_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_IMPL_TRIGO_TRIG_REDUCTION_HPP_INCLUDED

#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/rem_pio2_straight.hpp>
#include <nt2/include/functions/simd/rem_pio2.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/split.hpp>
#include <nt2/include/functions/simd/group.hpp>
#include <nt2/include/functions/simd/fast_toint.hpp>
#include <nt2/include/functions/simd/round2even.hpp>
#include <nt2/include/functions/simd/if_else_allbits.hpp>
#include <nt2/include/functions/simd/is_not_greater.hpp>
#include <nt2/include/functions/simd/is_nez.hpp>
#include <nt2/include/functions/simd/is_flint.hpp>
#include <nt2/include/functions/simd/all.hpp>
#include <nt2/include/functions/simd/inrad.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/pio_4.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/_20_pi.hpp>
#include <nt2/include/constants/oneo_90.hpp>
#include <nt2/include/constants/_180.hpp>
#include <nt2/include/constants/oneo_180.hpp>
#include <nt2/include/constants/medium_pi.hpp>
#include <nt2/include/constants/false.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/simd/sdk/config/enforce_precision.hpp>
#include <boost/mpl/int.hpp>

#include <boost/utility/enable_if.hpp>

namespace nt2 { namespace details
{
  template< class A0
            , class unit_tag
            , class style
            , class mode
            , class base_A0 = typename meta::scalar_of<A0>::type
  >
  struct trig_reduction;

  // This class exposes the public static member:
  // reduce:                to provide range reduction
  //
  // unit_tag allows to choose statically the scaling  among radian_tag, pi_tag, degree_tag
  // meaning that the cosa function will (for example) define respectively
  // x-->cos(x)          (radian_tag),
  // x-->cos(p*x)        (pi_tag)
  // x-->cos((pi/180)*x) (degree_tag)
  //

  // trigonometric reduction strategies in the [-pi/4, pi/4] range.
  // these reductions are used in the accurate and fast
  // trigonometric functions with different policies

  template<class A0, class style, class mode>
  struct trig_reduction < A0, radian_tag, style, mode>
  {
    typedef typename meta::scalar_of<A0>::type                         base_A0;
    typedef typename meta::as_logical<A0>::type                        bA0;
    typedef typename meta::as_integer<A0, signed>::type                int_type;
    typedef typename boost::simd::meta::is_upgradable_on_ext<A0>::type conversion_allowed;

    static BOOST_FORCEINLINE bA0 is_0_pio4_reduced(const A0&a0) { return boost::simd::is_ngt(a0, nt2::Pio_4<A0>()); }
    static BOOST_FORCEINLINE bA0 is_0_pio2_reduced(const A0&a0) { return boost::simd::is_ngt(a0, nt2::Pio_2<A0>()); }
    static BOOST_FORCEINLINE bA0 is_0_20pi_reduced(const A0&a0) { return boost::simd::is_ngt(a0, _20_pi<A0>()); }
    static BOOST_FORCEINLINE bA0 is_0_mpi_reduced (const A0&a0) { return boost::simd::is_ngt(a0, Medium_pi<A0>()); }  //2^6pi
    static BOOST_FORCEINLINE bA0 is_0_dmpi_reduced(const A0&a0) { return boost::simd::is_ngt(a0, single_constant<A0,0x49490fdb>()); }  //2^18pi

    static BOOST_FORCEINLINE bA0 cot_invalid(const A0& ) { return False<bA0>(); }
    static BOOST_FORCEINLINE bA0 tan_invalid(const A0& ) { return False<bA0>(); }

    static BOOST_FORCEINLINE int_type reduce(const A0& x, A0& xr) { return inner_reduce(x, xr); }
  private:
    static BOOST_FORCEINLINE int_type inner_reduce(const A0& x, A0& xr)
    {
      A0 xx =  preliminary<mode>::clip(x);
      return select_mode(xx, xr, boost::mpl::int_<mode::start>());
    }

    template < class Mode, bool clipped = Mode::clipped>
    struct preliminary
    {
      static BOOST_FORCEINLINE A0 const& clip(const A0& x) { return x; }
    };

    template < class Mode>
    struct preliminary<Mode, true>
    {
      static BOOST_FORCEINLINE A0 clip(const A0& x)
      {
        return clipto(x, boost::mpl::int_<Mode::range>());
      }
    private :
      static BOOST_FORCEINLINE A0 clipto(const A0& x, boost::mpl::int_<r_0_pio4> const&)
      {
        return if_else_nan(is_0_pio4_reduced(x), x);
      }
      static BOOST_FORCEINLINE A0 clipto(const A0& x, boost::mpl::int_<r_0_20pi> const&)
      {
        return if_else_nan(is_0_20pi_reduced(x), x);
      }
      static BOOST_FORCEINLINE A0 clipto(const A0& x, boost::mpl::int_<r_0_mpi> const&)
      {
        return if_else_nan(is_0_mpi_reduced(x), x);
      }
      static BOOST_FORCEINLINE A0 clipto(const A0& x, boost::mpl::int_<r_0_dmpi> const&)
      {
        return if_else_nan(is_0_dmpi_reduced(x), x);
      }
    };

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::true_ const&
                , boost::mpl::int_<r_0_pio4> const&
                )
    {
      xr = xx;
      return Zero<int_type>();
    }

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::false_ const&
                , boost::mpl::int_<r_0_pio4> const& r
                )
    {
      if(nt2::all(is_0_pio4_reduced(xx)))
        return select_range(xx,xr,boost::mpl::true_(), r);

      return select_mode(xx,xr,boost::mpl::int_<r_0_pio2>());
    }

    static BOOST_FORCEINLINE int_type
    select_mode(const A0& xx, A0& xr, boost::mpl::int_<r_0_pio4> const& r)
    {
      return select_range(xx,xr,boost::mpl::bool_<mode::range == r_0_pio4>(),r);
    }

    static BOOST_FORCEINLINE int_type select_mode(const A0& xx, A0& xr, boost::mpl::int_<r_0_pio2> const&)
    {
      if(nt2::all(is_0_pio2_reduced(xx)))
        return rem_pio2_straight(xx, xr);
      return select_mode(xx,xr,boost::mpl::int_<r_0_20pi>());
    }

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::true_ const&
                , boost::mpl::int_<r_0_20pi> const&
                )
    {
      return rem_pio2_cephes(xx, xr);
    }

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::false_ const&
                , boost::mpl::int_<r_0_20pi> const& r
                )
    {
      if(nt2::all(is_0_20pi_reduced(xx)))
        return select_range(xx,xr,boost::mpl::true_(), r);

      return select_mode(xx,xr,boost::mpl::int_<r_0_mpi>());
    }

    static BOOST_FORCEINLINE int_type
    select_mode(const A0& xx, A0& xr, boost::mpl::int_< r_0_20pi> const& r)
    {
      return select_range(xx,xr,boost::mpl::bool_<mode::range == r_0_20pi>(),r);
    }

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::true_ const&
                , boost::mpl::int_<r_0_mpi> const&
                )
    {
      return rem_pio2_medium(xx, xr);
    }

    static BOOST_FORCEINLINE int_type
    select_range( const A0& xx, A0& xr
                , boost::mpl::false_ const&
                , boost::mpl::int_<r_0_mpi> const& r
                )
    {
      if(nt2::all(is_0_mpi_reduced(xx)))
        return select_range(xx,xr,boost::mpl::true_(), r);

      return select_mode(xx,xr,boost::mpl::int_<r_0_dmpi>());
    }

    static BOOST_FORCEINLINE int_type select_mode(const A0& xx, A0& xr, boost::mpl::int_< r_0_mpi> const& r)
    {
      return select_range(xx,xr,boost::mpl::bool_<mode::range == r_0_mpi>(),r);
    }

    static BOOST_FORCEINLINE int_type select_mode(const A0& xx, A0& xr, boost::mpl::int_< r_0_dmpi> const&)
    {
      if(nt2::all(is_0_dmpi_reduced(xx)))
        return use_conversion(xx, xr, style(), conversion_allowed());
      return rem_pio2(xx, xr);
    }

    static BOOST_FORCEINLINE int_type use_conversion(const A0 & xx,  A0& xr,  const style &, boost::mpl::false_)
    {
      return rem_pio2(xx, xr);
    }

    static BOOST_FORCEINLINE int_type use_conversion(const A0 & xx,  A0& xr,  const tag::not_simd_type &, boost::mpl::true_)
    {
      // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
      typedef typename meta::upgrade<A0>::type uA0;
      typedef trig_reduction< uA0, radian_tag,  tag::not_simd_type, mode, double> aux_reduction;
      uA0 ux = xx, uxr;
      int_type n = static_cast<int_type>(aux_reduction::reduce(ux, uxr));
      xr = static_cast<A0>(uxr);
      return n;
    }

    static BOOST_FORCEINLINE int_type use_conversion(const A0 & x,  A0& xr,  const tag::simd_type &, boost::mpl::true_)
    {
      // all of x are in [0, 2^18*pi],  conversion to double is used to reduce
      typedef typename meta::upgrade<A0>::type uA0;
      typedef typename meta::upgrade<int_type>::type uint_type;
      typedef trig_reduction< uA0, radian_tag,  tag::simd_type, mode, double> aux_reduction;
      uA0 ux1, ux2, uxr1, uxr2;
      nt2::split(x, ux1, ux2);
      uint_type n1 = aux_reduction::reduce(ux1, uxr1);
      uint_type n2 = aux_reduction::reduce(ux2, uxr2);
      xr = nt2::group(uxr1, uxr2);
      nt2::split(xr, ux1, ux2);
      return nt2::group(n1, n2);
    }
  };

  template<class A0, class style>
  struct trig_reduction<A0,degree_tag, style, big_>
  {
    typedef typename meta::as_logical<A0>::type              bA0;
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static BOOST_FORCEINLINE bA0 cot_invalid(const A0& x) { return logical_and(nt2::is_nez(x), is_flint(x*nt2::Oneo_180<A0>())); }
    static BOOST_FORCEINLINE bA0 tan_invalid(const A0& x) { return nt2::is_flint((x-nt2::_90<A0>())*nt2::Oneo_180<A0>()); }

    static BOOST_FORCEINLINE int_type reduce(const A0& x, A0& xr)
    {
      A0 xi = nt2::round2even(x*nt2::Oneo_90<A0>());
      A0 x2 = x - xi * nt2::_90<A0>();

      xr =  nt2::inrad(x2);
      return nt2::fast_toint(xi);
    }
  };

  #ifdef BOOST_SIMD_HAS_X87
  template<class A0>
  struct trig_reduction<A0,degree_tag, tag::not_simd_type, big_>
  {
    typedef typename meta::as_logical<A0>::type              bA0;
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static BOOST_FORCEINLINE bA0 cot_invalid(const A0& x) { return logical_and(nt2::is_nez(x), is_flint(x/nt2::_180<A0>())); }
    static BOOST_FORCEINLINE bA0 tan_invalid(const A0& x) { return nt2::is_flint((x-nt2::_90<A0>())/nt2::_180<A0>()); }

    static BOOST_FORCEINLINE int_type reduce(const A0& x, A0& xr)
    {
      A0 xi = nt2::round2even(x*nt2::Oneo_90<A0>());
      A0 x2 = x - xi * nt2::_90<A0>();

      xr =  nt2::inrad(x2);
      return nt2::fast_toint(xi);
    }
  };
  #endif

  template < class A0, class style>
  struct trig_reduction < A0, pi_tag,  style, big_>
  {
    typedef typename meta::as_logical<A0>::type              bA0;
    typedef typename meta::as_integer<A0, signed>::type int_type;

    static BOOST_FORCEINLINE bA0 cot_invalid(const A0& x) { return logical_and(nt2::is_nez(x), nt2::is_flint(x)); }
    static BOOST_FORCEINLINE bA0 tan_invalid(const A0& x) { return nt2::is_flint(x-nt2::Half<A0>()) ; }

    static BOOST_FORCEINLINE int_type reduce(const A0& x,  A0& xr)
    {
      A0 xi = nt2::round2even(x*nt2::Two<A0>());
      A0 x2 = x - xi * nt2::Half<A0>();
      xr = x2*nt2::Pi<A0>();
      return nt2::fast_toint(xi);
    }
  };
} }

#endif


#include "pch.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <numeric>
#include <string>
#include <set>
#include <vector>
#include <map>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>

using namespace std;

class tfidf {
private:
	std::vector<std::string> patentsList; // all patents in order
	std::vector<std::vector<double>> dataMat; // converted bag of words matrix
	unsigned int nrow; // matrix row number
	unsigned int ncol; // matrix column number
	std::vector<std::vector<double>> weightMat; //tfidf weight matrix
	std::vector<std::vector<std::string>> rawDataSet; // raw data
	std::vector<std::string> vocabList; // all terms
	std::map<std::string, int> h_hot; // hot num
	std::vector<int> numOfTerms; // used in tf calculation
	std::vector<std::string> stopWords; // list of common words to ignore

	std::string readFileText(std::string & filename)
	{
		std::ifstream in(filename);
		std::string str((std::istreambuf_iterator<char>(in)),
			std::istreambuf_iterator<char>());
		return str;
	}

	void createVocabList()
	{
		std::set<std::string> vocabListSet;
		for (std::vector<std::string> document : rawDataSet)
		{
			for (std::string word : document)
				vocabListSet.insert(word);
		}
		std::copy(vocabListSet.begin(), vocabListSet.end(), std::back_inserter(vocabList));
	}

	std::vector<double> bagOfWords2VecMN(std::vector<std::string> & inputSet)
	{
		std::vector<double> returnVec(vocabList.size(), 0);
		for (std::string word : inputSet)
		{
			size_t idx = std::find(vocabList.begin(), vocabList.end(), word) - vocabList.begin();
			if (idx == vocabList.size())
				cout << "word: " << word << "not found" << endl;
			else
				returnVec.at(idx) += 1;
		}
		return returnVec;
	}

	void vec2mat()
	{
		cout << "Converting text to vector..." << endl;
		int cnt(0);
		for (auto it = rawDataSet.begin(); it != rawDataSet.end(); ++it)
		{
			cnt++;
			cout << cnt << "\r";
			std::cout.flush();
			dataMat.push_back(bagOfWords2VecMN(*it));
			numOfTerms.push_back(it->size());
			it->clear();
		}
		cout << endl;
		ncol = dataMat[0].size();
		nrow = dataMat.size();
		rawDataSet.clear(); // release memory
	}

	std::vector<std::string> textParse(std::string & bigString)
	{
		std::vector<std::string> vec;
		boost::tokenizer<> tok(bigString);
		for (boost::tokenizer<>::iterator beg = tok.begin(); beg != tok.end(); ++beg)
		{
			if (!(std::binary_search(stopWords.begin(), stopWords.end(), *beg)))
				vec.push_back(*beg);
		}
		return vec;
	}

	std::vector<double> vec_sum(const std::vector<double>& a, const std::vector<double>& b)
	{
		assert(a.size() == b.size());
		std::vector<double> result;
		result.reserve(a.size());
		std::transform(a.begin(), a.end(), b.begin(),
			std::back_inserter(result), std::plus<double>());
		return result;
	}

	// larger -> better
	double cosine_similarity(std::vector<double> &v1, std::vector<double> &v2)
	{
		unsigned int len = v1.size();
		double d1 = 0;
		double d2 = 0;
		double d3 = 0;
		for (unsigned int i = 0; i < len; ++i)
		{
			d1 += (v1[i] * v2[i]);
			d2 += (v1[i] * v1[i]);
			d3 += (v2[i] * v2[i]);
		}
		return d1 / (sqrt(d2) * sqrt(d3));
	}

	void orderByHot(std::vector<std::string> *vec)
	{
		std::map<int, std::vector<std::string>> temp;
		for (std::string e : *vec)
		{
			temp[h_hot[e]].push_back(e);
		}
		vec->clear();
		for (auto it = temp.rbegin(); it != temp.rend(); ++it)
		{
			for (std::string e : it->second)
				vec->push_back(e);
		}
	}

public:
	unsigned int recAmount;
	unsigned int finishCount;

	void loadData()
	{
		cout << "Loading data..." << endl;
		ifstream in("patentList.csv");
		string tmp;
		std::vector<std::string> vec_str;
		int cnt = 0;
		while (!in.eof()) {
			cnt++;
			if (cnt > 500) // FOR TEST ONLY
				break;
			getline(in, tmp, '\n');
			if (tmp == "") break;
			boost::split(vec_str, tmp, boost::is_any_of(","));
			std::vector<std::string> wordList = textParse(vec_str[1]);
			rawDataSet.push_back(wordList);
			patentsList.push_back(vec_str[0]);
			tmp.clear();
			vec_str.clear();
		}

		std::ifstream in2("song_hot_num.csv");
		while (!in2.eof())
		{
			getline(in2, tmp, '\n');
			if (tmp == "") break;
			boost::split(vec_str, tmp, boost::is_any_of(","));
			h_hot[vec_str[0]] = atoi(vec_str[1].c_str());
			tmp.clear();
			vec_str.clear();
		}
	}

	void loadStopWords()
	{
		ifstream in("stop_words.txt");
		string tmp;
		while (!in.eof()) {
			getline(in, tmp, '\n');
			stopWords.push_back(tmp);
			tmp.clear();
		}
		std::sort(stopWords.begin(), stopWords.end());
	}

	void getMat()
	{
		cout << "Total " << rawDataSet.size() << " patents." << endl;
		cout << "Processing..." << endl;
		createVocabList();
		vec2mat();
		cout << "Calculating TF-IDF weight matrix..." << endl;
		std::vector<std::vector<double>> dataMat2(dataMat);
		std::vector<double> termCount;
		termCount.resize(ncol);

		for (unsigned int i = 0; i != nrow; ++i)
		{
			for (unsigned int j = 0; j != ncol; ++j)
			{
				if (dataMat2[i][j] > 1) // only keep 1 and 0
					dataMat2[i][j] = 1;
			}
			termCount = vec_sum(termCount, dataMat2[i]); // no. of doc. each term appears
		}
		dataMat2.clear(); //release

		std::vector<double> row_vec;
		for (unsigned int i = 0; i != nrow; ++i)
		{
			cout << "\r" << (i + 1);
			std::cout.flush();
			for (unsigned int j = 0; j != ncol; ++j)
			{
				double tf = dataMat[i][j] / numOfTerms[i];
				double idf = log((double)nrow / (termCount[j]));
				row_vec.push_back(tf * idf); // TF-IDF equation
			}
			weightMat.push_back(row_vec);
			row_vec.clear();
		}
		nrow = weightMat.size();
		cout << endl;
	}

	void saveMat(std::string filename)
	{
		cout << "Saving weight matrix to " << filename << "..." << endl;
		std::ofstream outfile;
		outfile.open(filename, std::ios_base::app);
		for (auto it = weightMat.begin(); it != weightMat.end(); ++it)
		{
			std::ostringstream ss;
			for (auto it2 = it->begin(); it2 != it->end(); ++it2)
			{
				ss << *it2 << ",";
			}
			outfile << ss.str().substr(0, ss.str().size() - 1) << endl;
			ss.clear();
		}
	}

	void calSimi(unsigned int limit1, unsigned int limit2)
	{
		cout << "Calculating..." << endl;
		double similarity;
		std::map<double, std::vector<std::string>> similarPatent;
		std::vector<std::string> patent;
		for (std::size_t i = limit1; i != limit2; ++i) // for each patent
		{
			finishCount++;
			for (std::size_t j = 0; j != nrow; ++j) // compare with each patent
			{
				if (j != i) // exclude patent itself
				{
					similarity = cosine_similarity(weightMat[i], weightMat[j]);
					similarPatent[similarity].push_back(patentsList[j]);
				}
			}

			if (0 < similarPatent.rbegin()->first)
			{
				for (auto it2 = similarPatent.rbegin(); it2 != similarPatent.rend(); ++it2)
				{
					if (patent.size() >= recAmount)
						break;
					std::vector<std::string> temp(it2->second);
					orderByHot(&temp);
					for (std::string e : temp)
						patent.push_back(e);
				}
				if (patent.size() >= recAmount)
				{
					std::vector<std::string> patent2(patent.begin(), patent.begin() + recAmount);
					std::ofstream outfile;
					outfile.open("similar_patents.txt", std::ios_base::app);
					outfile << patentsList[i] << "," << boost::join(patent2, ",") << endl;
					outfile.close();
					patent2.clear();
				}
				else
				{
					cout << "Unexpected small size recommendation list." << endl;
				}
			}
			patent.clear();
			similarPatent.clear();
		}
		cout << endl;
	}
};

int main()
{
	tfidf patents;
	patents.loadStopWords();
	patents.loadData();
	patents.recAmount = 4;
	patents.getMat();
	patents.saveMat("tfidf_matrix.txt");
	patents.calSimi(0, 15);
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, CNRS
 *  Author: Florent Lamiraux
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CNRS nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#define BOOST_TEST_MODULE "FCL_DISTANCE_OBB"
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))

#include <boost/test/included/unit_test.hpp>
#include <hpp/fcl/narrowphase/narrowphase.h>

#include "../src/BV/OBB.h"
#include "../src/distance_func_matrix.h"

using fcl::FCL_REAL;

struct Sample
{
  fcl::Matrix3f R;
  fcl::Vec3f T;
  fcl::Vec3f extent1;
  fcl::Vec3f extent2;
};

struct Result
{
  bool overlap;
  FCL_REAL distance;
};

BOOST_AUTO_TEST_CASE(obb_overlap)
{
  static const size_t nbSamples = 10000;
  Sample sample [nbSamples];
  FCL_REAL range = 2;
  FCL_REAL sumDistance = 0, sumDistanceLowerBound = 0;
  for (std::size_t i=0; i<nbSamples; ++i) {
    // sample unit quaternion
    FCL_REAL u1 = (FCL_REAL) rand() / RAND_MAX;
    FCL_REAL u2 = (FCL_REAL) rand() / RAND_MAX;
    FCL_REAL u3 = (FCL_REAL) rand() / RAND_MAX;

    FCL_REAL q1 = sqrt (1-u1)*sin(2*M_PI*u2);
    FCL_REAL q2 = sqrt (1-u1)*cos(2*M_PI*u2);
    FCL_REAL q3 = sqrt (u1) * sin(2*M_PI*u3);
    FCL_REAL q4 = sqrt (u1) * cos(2*M_PI*u3);
    fcl::Quaternion3f (q1, q2, q3, q4).toRotation (sample [i].R);

    // sample translation
    sample [i].T = fcl::Vec3f ((FCL_REAL) range * rand() / RAND_MAX,
			       (FCL_REAL) range * rand() / RAND_MAX,
			       (FCL_REAL) range * rand() / RAND_MAX);

    // sample extents
    sample [i].extent1 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
				     (FCL_REAL) rand() / RAND_MAX,
				     (FCL_REAL) rand() / RAND_MAX);

    sample [i].extent2 = fcl::Vec3f ((FCL_REAL) rand() / RAND_MAX,
				     (FCL_REAL) rand() / RAND_MAX,
				     (FCL_REAL) rand() / RAND_MAX);
  }

  // Compute result for each function
  Result resultDistance [nbSamples];
  Result resultDisjoint [nbSamples];
  Result resultDisjointAndLowerBound [nbSamples];

  fcl::Transform3f tf1;
  fcl::Transform3f tf2;
  fcl::Box box1 (0, 0, 0);
  fcl::Box box2 (0, 0, 0);
  fcl::DistanceRequest request (false, 0, 0, fcl::GST_INDEP);
  fcl::DistanceResult result;
  FCL_REAL distance;
  FCL_REAL squaredDistance;
  timeval t0, t1;
  fcl::GJKSolver_indep gjkSolver;

  // ShapeShapeDistance
  gettimeofday (&t0, NULL);
  for (std::size_t i=0; i<nbSamples; ++i) {
    box1.side = 2*sample [i].extent1;
    box2.side = 2*sample [i].extent2;
    tf2.setTransform (sample [i].R, sample [i].T);
    resultDistance [i].distance =
      fcl::ShapeShapeDistance<fcl::Box, fcl::Box, fcl::GJKSolver_indep>
      (&box1, tf1, &box2, tf2, &gjkSolver, request, result);
    resultDistance [i].overlap = (resultDistance [i].distance < 0);
    if (resultDistance [i].distance < 0) {
      resultDistance [i].distance = 0;
    }
    result.clear ();
  }
  gettimeofday (&t1, NULL);
  double t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
  std::cout << "Time for " << nbSamples
	    << " calls to ShapeShapeDistance (in seconds): "
	    << t << std::endl;

  // obbDisjointAndLowerBoundDistance
  gettimeofday (&t0, NULL);
  for (std::size_t i=0; i<nbSamples; ++i) {
    resultDisjointAndLowerBound [i].overlap =
      !obbDisjointAndLowerBoundDistance (sample [i].R, sample [i].T,
					 sample [i].extent1,
					 sample [i].extent2,
					 squaredDistance);
    resultDisjointAndLowerBound [i].distance = sqrt (squaredDistance);
  }
  gettimeofday (&t1, NULL);
  t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
  std::cout << "Time for " << nbSamples
	    << " calls to obbDisjointAndLowerBoundDistance"
	    << " (in seconds): "
	    << t << std::endl;

  gettimeofday (&t0, NULL);
  for (std::size_t i=0; i<nbSamples; ++i) {
    resultDisjoint [i].overlap =
      !obbDisjoint (sample [i].R, sample [i].T,
		    sample [i].extent1,
		    sample [i].extent2);
    resultDisjoint [i].distance = 0;
  }
  gettimeofday (&t1, NULL);
  t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
  std::cout << "Time for " << nbSamples << " calls to obbDisjoint"
	    << " (in seconds): "
	    << t << std::endl;

  // Test correctness of results
  bool res1, res2, res3;
  for (std::size_t i=0; i<nbSamples; ++i) {
    res1 = resultDisjoint [i].overlap;
    res2 = resultDisjointAndLowerBound [i].overlap;
    res3 = resultDistance [i].overlap;
    sumDistanceLowerBound += resultDisjointAndLowerBound [i].distance;
    sumDistance += resultDistance [i].distance;

#if 0
    std::cout << "ShapeShapeDistance:    overlap: "
	      << resultDistance [i].overlap
	      << ", distance: " << resultDistance [i].distance << std::endl;
    std::cout << "disjoint:              overlap: "
	      << resultDisjoint [i].overlap
	      << ", distance: " << resultDisjoint [i].distance << std::endl;
    std::cout << "disjointAndLowerBound: overlap: "
	      << resultDisjointAndLowerBound [i].overlap
	      << ", distance: " << resultDisjointAndLowerBound [i].distance
	      << std::endl << std::endl;
#endif
    BOOST_CHECK (res1 == res2);
    BOOST_CHECK (res1 == res3);
    BOOST_CHECK ((!res1 && resultDisjointAndLowerBound [i].distance > 0) ||
		 (res1 && resultDisjointAndLowerBound [i].distance == 0));
    BOOST_CHECK (resultDisjointAndLowerBound [i].distance <=
		 resultDistance [i].distance);
  }
  std::cout << "Sum distances ShapeShapeDistance:               "
	    << sumDistance << std::endl;
  std::cout << "Sum distances obbDisjointAndLowerBoundDistance: "
	    << sumDistanceLowerBound << std::endl;
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <numeric>
#include <unordered_map>
#include <boost/algorithm/string/find.hpp>
#include <cmath>
#include <boost/range/algorithm/count.hpp>

class Tile
{
    public:
        Tile() = default;
        Tile(int _id) : id(_id) {}
        int id;
        // up, down, left, right
        std::vector<std::string> borders = { "", "", "", ""};
        std::vector<int> adjecent;
        int count = 0;

        std::pair<int, int> match(Tile& other_tile)
        {
            std::pair<int, int> match_indices = std::make_pair(-1, -1);
            for(int i = 0; i < borders.size(); i++)
            {
                for(int j = 0; j < other_tile.borders.size(); j++)
                {
                    if(borders[i] == other_tile.borders[j] &&
                    std::find(adjecent.begin(), adjecent.end(), other_tile.id) == adjecent.end())
                    {
                        adjecent.push_back(other_tile.id);
                        other_tile.adjecent.push_back(id);
                        match_indices = std::make_pair(id, other_tile.id);
                        count++;
                        other_tile.count++;
                        return match_indices;
                    }
                }
            }
            return match_indices;
        }

        void add_reversed_borders()
        {
            std::vector<std::string> reversed;
            for(std::string border : borders)
            {
                std::reverse(border.begin(), border.end());
                reversed.push_back(border);
            }
            borders.insert(borders.begin(), reversed.begin(), reversed.end());
        }
};

class Decoder
{
    public:
        Decoder() = default;

        std::vector<Tile> tiles;

        void add_reversing_combinations()
        {
            for(Tile& tile : tiles)
            {
                tile.add_reversed_borders();
            }
        }

        // key - id, value - lines
        std::unordered_map<int, std::vector<std::string>> tiles_map;

        Decoder(std::string file_name, bool monster = false)
        {
            std::ifstream file(file_name);
            std::string line;
            Tile tile;
            int row = 0;
            std::vector<std::string> tile_map;
            if(file.is_open())
            {
                while(std::getline(file, line))
                {
                    if(line.find("Tile") < SIZE_MAX)
                    {
                        tile = Tile(std::stoi(line.substr(line.find(" ") + 1, line.find(":") - line.find(" ") - 1)));
                    }
                    else if(line.empty())
                    {
                        tiles.push_back(tile);
                        tiles_map[tile.id] = tile_map;
                        tile_map.clear();
                        row = 0;
                    }
                    else
                    {
                        tile_map.push_back(line);
                        if(row == 0) tile.borders[0] = line;
                        if(row == 9) tile.borders[1] = line;
                        tile.borders[2] += line[0];
                        tile.borders[3] += line[9];
                        row++;
                    }
                }
                tiles.push_back(tile); // add last tile
            }
            add_reversing_combinations();
            connect_tiles();
            if(monster) build_image();
        }

        void connect_tiles()
        {
            for(Tile& tile : tiles)
            {
                for(Tile& other_tile : tiles)
                {
                    if(other_tile.id == tile.id) continue;
                    std::pair<int, int> match_ids = tile.match(other_tile);
                }
            }
        }

        void remove_borders()
        {
            // key - id, value - lines
            std::unordered_map<int, std::vector<std::string>> new_tiles_map;
            for(auto pair : tiles_map)
            {
                int tile_id = pair.first;
                std::vector<std::string> tile_lines = pair.second;
                std::vector<std::string> lines_removed_border;
                for(int i = 1; i < tile_lines.size() - 1; i++)
                {
                    lines_removed_border.push_back(tile_lines[i].substr(1, tile_lines[i].size() - 2));
                }
                new_tiles_map[tile_id] = lines_removed_border;
            }
            tiles_map = new_tiles_map;
        }

        std::vector<std::string> image;

        std::unordered_map<int, std::vector<int>> get_adjecency_map()
        {
            std::unordered_map<int, std::vector<int>> adjecency_map;
            for(Tile tile : tiles)
            {
                std::vector<int> adjecent;
                std::cout << "tile id = " << tile.id << '\n';
                for(int adj : tile.adjecent)
                {
                    std::cout << "adj = " << adj << '\n';
                    adjecent.push_back(adj);
                }
                adjecency_map[tile.id] = adjecent;
                std::cout << '\n';
            }
            return adjecency_map;
        }

        void build_image()
        {

        }

        std::vector<int> get_corner_ids()
        {
            std::vector<int> corner_ids;
            for(Tile tile : tiles)
            {
                if(tile.count == 2) corner_ids.push_back(tile.id);
            }
            return corner_ids;
        }

        long long multiply_corner_tiles_id()
        {
            std::vector<int> corner_ids = get_corner_ids();
            long long result = std::accumulate(std::begin(corner_ids), std::end(corner_ids),
            1LL, [&](const long long previous, int id) { return previous * id; });
            return result;
        }

        int count_sea_mosters()
        {
            // todo
            return 0;
        }

        int count_hashtag()
        {
            int count = 0;
            for(std::string image_row : image)
            {
                count += boost::count(image_row, '#');
            }
            return count;
        }
};

void part1(const std::string& file_name)
{
    std::cout << "======\nPart 1\n======\n";
    Decoder decoder(file_name);
    std::cout << "Multiplied IDs of the four corner = " << decoder.multiply_corner_tiles_id() << '\n';
}

void part2(const std::string& file_name)
{
    std::cout << "======\nPart 2\n======\n";
    Decoder decoder(file_name, true);
    int sea_mosters = decoder.count_sea_mosters();
    constexpr int count_monster_hashtag = 15;
    std::cout << decoder.count_hashtag() - count_monster_hashtag * sea_mosters << " # are not part of a sea monster\n";
}

int main()
{
    const std::string file_name = "/home/daria/Documents/AoC2020/input/day20.txt";
    part1(file_name);
    std::cout << '\n';
    part2(file_name);
    std::cout << '\n';
}


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// This file was originally part of PyJuggler.

// PyJuggler is (C) Copyright 2002, 2003 by Patrick Hartling
// Distributed under the GNU Lesser General Public License 2.1.  (See
// accompanying file COPYING.txt or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl/Coord.h>
#include <gmtl/EulerAngleOps.h>
#include <gmtl/CoordOps.h>
#include <gmtl/Output.h>
#include <gmtl-pickle.h>

// Using =======================================================================
using namespace boost::python;

// Module ======================================================================
void _Export_Coord_gmtl_Vec_double_4_gmtl_EulerAngle_double_gmtl_ZXY()
{
    scope* gmtl_Coord_gmtl_Vec_double_4_gmtl_EulerAngle_double_gmtl_ZXY_scope = new scope(
    class_< gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> > >("Coord4dZXY", init<  >())
        .def(init< const gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> > & >())
        .def(init< const gmtl::Vec<double,4> &, const gmtl::EulerAngle<double,gmtl::ZXY> & >())
        .def_readwrite("pos", &gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::mPos)
        .def_readwrite("rot", &gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::mRot)
        .def("getPos", &gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::getPos, return_internal_reference< 1 >())
        .def("getRot", &gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::getRot, return_internal_reference< 1 >())
        .def("set",
             (gmtl::Coord<gmtl::Vec<double, 4>, gmtl::EulerAngle<double, gmtl::ZXY> >& (gmtl::Coord<gmtl::Vec<double, 4>, gmtl::EulerAngle<double, gmtl::ZXY> >::*)(const gmtl::Coord<gmtl::Vec<double, 4>, gmtl::EulerAngle<double, gmtl::ZXY> >&)) &gmtl::Coord<gmtl::Vec<double, 4>, gmtl::EulerAngle<double, gmtl::ZXY> >::operator=,
             return_internal_reference<1>())
        .def_pickle(gmtlPickle::Coord_pickle< gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >())
        .def(self == self)
        .def(self != self)
        .def(self_ns::str(self))
    );

    enum_< gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::Params >("Params")
        .value("RotSize", gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::RotSize)
        .value("PosSize", gmtl::Coord<gmtl::Vec<double, 4>,gmtl::EulerAngle<double, gmtl::ZXY> >::PosSize)
    ;

    delete gmtl_Coord_gmtl_Vec_double_4_gmtl_EulerAngle_double_gmtl_ZXY_scope;

}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_gp
#define protected public

#include <boost/test/unit_test.hpp>

#include <limbo/acqui/ucb.hpp>
#include <limbo/kernel/exp.hpp>
#include <limbo/kernel/matern_five_halves.hpp>
#include <limbo/kernel/matern_three_halves.hpp>
#include <limbo/kernel/squared_exp_ard.hpp>
#include <limbo/mean/constant.hpp>
#include <limbo/mean/function_ard.hpp>
#include <limbo/model/gp.hpp>
#include <limbo/model/gp/kernel_lf_opt.hpp>
#include <limbo/model/gp/kernel_loo_opt.hpp>
#include <limbo/model/gp/kernel_mean_lf_opt.hpp>
#include <limbo/model/gp/mean_lf_opt.hpp>
#include <limbo/model/multi_gp.hpp>
#include <limbo/model/multi_gp/parallel_lf_opt.hpp>
#include <limbo/model/sparsified_gp.hpp>
#include <limbo/opt/grid_search.hpp>
#include <limbo/tools/macros.hpp>

using namespace limbo;

// Check gradient via finite differences method
template <typename F>
std::tuple<double, Eigen::VectorXd, Eigen::VectorXd> check_grad(const F& f, const Eigen::VectorXd& x, double e = 1e-4)
{
    Eigen::VectorXd analytic_result, finite_diff_result;

    opt::eval_t res = f(x, true);
    analytic_result = opt::grad(res);

    finite_diff_result = Eigen::VectorXd::Zero(x.size());
    for (int j = 0; j < x.size(); j++) {
        Eigen::VectorXd test1 = x, test2 = x;
        test1[j] -= e;
        test2[j] += e;
        double res1 = opt::fun(f(test1, false));
        double res2 = opt::fun(f(test2, false));
        finite_diff_result[j] = (res2 - res1) / (2.0 * e);
    }

    return std::make_tuple((analytic_result - finite_diff_result).norm(), analytic_result, finite_diff_result);
}

Eigen::VectorXd make_v1(double x)
{
    return tools::make_vector(x);
}

Eigen::VectorXd make_v2(double x1, double x2)
{
    Eigen::VectorXd v2(2);
    v2 << x1, x2;
    return v2;
}

struct Params {
    struct kernel : public defaults::kernel {
    };

    struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
    };

    struct kernel_maternfivehalves {
        BO_PARAM(double, sigma_sq, 1);
        BO_PARAM(double, l, 0.25);
    };

    struct mean_constant : public defaults::mean_constant {
    };

    struct opt_rprop : public defaults::opt_rprop {
    };

    struct acqui_ucb : public defaults::acqui_ucb {
    };

    struct opt_gridsearch : public defaults::opt_gridsearch {
    };
};

BOOST_AUTO_TEST_CASE(test_gp_check_lf_grad)
{
    using namespace limbo;

    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::FunctionARD<Params, mean::Constant<Params>>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp(4, 2);

    std::vector<Eigen::VectorXd> observations, samples, test_samples, test_samples_mean, test_samples_kernel_mean;
    double e = 1e-4;

    // Random samples and test samples
    int N = 40, M = 100;

    for (int i = 0; i < N; i++) {
        samples.push_back(tools::random_vector(4));
        Eigen::VectorXd ob(2);
        ob << std::cos(samples[i](0)), std::sin(samples[i](1));
        observations.push_back(ob);
    }

    for (int i = 0; i < M; i++) {
        test_samples.push_back(tools::random_vector(gp.kernel_function().h_params_size()));
        test_samples_mean.push_back(tools::random_vector(gp.mean_function().h_params_size()));
        test_samples_kernel_mean.push_back(tools::random_vector(gp.kernel_function().h_params_size() + gp.mean_function().h_params_size()));
    }

    gp.compute(samples, observations);

    model::gp::KernelLFOpt<Params>::KernelLFOptimization<GP_t> kernel_optimization(gp);

    Eigen::VectorXd results(M);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_optimization, test_samples[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);

    model::gp::KernelMeanLFOpt<Params>::KernelMeanLFOptimization<GP_t> kernel_mean_optimization(gp);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_mean_optimization, test_samples_kernel_mean[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);

    model::gp::MeanLFOpt<Params>::MeanLFOptimization<GP_t> mean_optimization(gp);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(mean_optimization, test_samples_mean[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);
}

BOOST_AUTO_TEST_CASE(test_gp_check_lf_grad_noise)
{
    using namespace limbo;
    struct Parameters {
        struct kernel : public defaults::kernel {
            BO_PARAM(bool, optimize_noise, true);
        };

        struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        };

        struct kernel_maternfivehalves {
            BO_PARAM(double, sigma_sq, 1);
            BO_PARAM(double, l, 0.25);
        };

        struct mean_constant : public defaults::mean_constant {
        };

        struct opt_rprop : public defaults::opt_rprop {
        };

        struct acqui_ucb : public defaults::acqui_ucb {
        };

        struct opt_gridsearch : public defaults::opt_gridsearch {
        };
    };

    using KF_t = kernel::SquaredExpARD<Parameters>;
    using Mean_t = mean::FunctionARD<Params, mean::Constant<Parameters>>;
    using GP_t = model::GP<Parameters, KF_t, Mean_t>;

    GP_t gp(4, 2);

    std::vector<Eigen::VectorXd> observations, samples, test_samples, test_samples_kernel_mean;
    double e = 1e-4;

    // Random samples and test samples
    int N = 40, M = 100;

    for (int i = 0; i < N; i++) {
        samples.push_back(tools::random_vector(4));
        Eigen::VectorXd ob(2);
        ob << std::cos(samples[i](0)), std::sin(samples[i](1));
        observations.push_back(ob);
    }

    for (int i = 0; i < M; i++) {
        test_samples.push_back(tools::random_vector(gp.kernel_function().h_params_size()));
        test_samples_kernel_mean.push_back(tools::random_vector(gp.kernel_function().h_params_size() + gp.mean_function().h_params_size()));
    }

    gp.compute(samples, observations);

    model::gp::KernelLFOpt<Parameters>::KernelLFOptimization<GP_t> kernel_optimization(gp);

    Eigen::VectorXd results(M);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_optimization, test_samples[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);

    model::gp::KernelMeanLFOpt<Parameters>::KernelMeanLFOptimization<GP_t> kernel_mean_optimization(gp);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_mean_optimization, test_samples_kernel_mean[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);
}

BOOST_AUTO_TEST_CASE(test_gp_check_loo_grad)
{
    using namespace limbo;

    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp(4, 2);

    std::vector<Eigen::VectorXd> observations, samples, test_samples;
    double e = 1e-4;

    // Random samples and test samples
    int N = 40, M = 100;

    for (int i = 0; i < N; i++) {
        samples.push_back(tools::random_vector(4));
        Eigen::VectorXd ob(2);
        ob << std::cos(samples[i](0)), std::sin(samples[i](1));
        observations.push_back(ob);
    }

    for (int i = 0; i < M; i++) {
        test_samples.push_back(tools::random_vector(gp.kernel_function().h_params_size()));
    }

    gp.compute(samples, observations);

    model::gp::KernelLooOpt<Params>::KernelLooOptimization<GP_t> kernel_optimization(gp);

    Eigen::VectorXd results(M);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_optimization, test_samples[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);
}

BOOST_AUTO_TEST_CASE(test_gp_check_loo_grad_noise)
{
    using namespace limbo;
    struct Parameters {
        struct kernel : public defaults::kernel {
            BO_PARAM(bool, optimize_noise, true);
        };

        struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        };

        struct kernel_maternfivehalves {
            BO_PARAM(double, sigma_sq, 1);
            BO_PARAM(double, l, 0.25);
        };

        struct mean_constant : public defaults::mean_constant {
        };

        struct opt_rprop : public defaults::opt_rprop {
        };

        struct acqui_ucb : public defaults::acqui_ucb {
        };

        struct opt_gridsearch : public defaults::opt_gridsearch {
        };
    };

    using KF_t = kernel::SquaredExpARD<Parameters>;
    using Mean_t = mean::Constant<Parameters>;
    using GP_t = model::GP<Parameters, KF_t, Mean_t>;

    GP_t gp(4, 2);

    std::vector<Eigen::VectorXd> observations, samples, test_samples;
    double e = 1e-4;

    // Random samples and test samples
    int N = 40, M = 100;

    for (int i = 0; i < N; i++) {
        samples.push_back(tools::random_vector(4));
        Eigen::VectorXd ob(2);
        ob << std::cos(samples[i](0)), std::sin(samples[i](1));
        observations.push_back(ob);
    }

    for (int i = 0; i < M; i++) {
        test_samples.push_back(tools::random_vector(gp.kernel_function().h_params_size()));
    }

    gp.compute(samples, observations);

    model::gp::KernelLooOpt<Parameters>::KernelLooOptimization<GP_t> kernel_optimization(gp);

    Eigen::VectorXd results(M);

    for (int i = 0; i < M; i++) {
        auto res = check_grad(kernel_optimization, test_samples[i], 1e-4);
        results(i) = std::get<0>(res);
        // std::cout << std::get<1>(res).transpose() << " vs " << std::get<2>(res).transpose() << " --> " << results(i) << std::endl;
    }

    BOOST_CHECK(results.array().sum() < M * e);
}

BOOST_AUTO_TEST_CASE(test_gp_check_inv_kernel_computation)
{
    using namespace limbo;

    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::FunctionARD<Params, mean::Constant<Params>>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp(4, 2);

    std::vector<Eigen::VectorXd> observations, samples;

    // Random samples
    int N = 10;

    for (int i = 0; i < N; i++) {
        samples.push_back(tools::random_vector(4));
        Eigen::VectorXd ob(2);
        ob << std::cos(samples[i](0)), std::sin(samples[i](1));
        observations.push_back(ob);
    }

    gp.compute(samples, observations);

    // Check that variable is properly initialized
    BOOST_CHECK(!gp.inv_kernel_computed());

    // Check that kernel is computed
    gp.compute_inv_kernel();
    BOOST_CHECK(gp.inv_kernel_computed());

    // Check recompute alternatives
    gp.recompute(false, false);
    BOOST_CHECK(gp.inv_kernel_computed());

    gp.recompute(true, false);
    BOOST_CHECK(gp.inv_kernel_computed());

    gp.recompute(true, true);
    BOOST_CHECK(!gp.inv_kernel_computed());

    // Check add_sample
    gp.compute_inv_kernel();
    gp.add_sample(samples.back(), observations.back());
    BOOST_CHECK(!gp.inv_kernel_computed());

    // Check different implicit computations of inverse kernel
    gp.compute_kernel_grad_log_lik();
    BOOST_CHECK(gp.inv_kernel_computed());

    gp.recompute();
    BOOST_CHECK(!gp.inv_kernel_computed());
    gp.compute_mean_grad_log_lik();
    BOOST_CHECK(gp.inv_kernel_computed());

    gp.recompute();
    BOOST_CHECK(!gp.inv_kernel_computed());
    gp.compute_log_loo_cv();
    BOOST_CHECK(gp.inv_kernel_computed());

    gp.recompute();
    BOOST_CHECK(!gp.inv_kernel_computed());
    gp.compute_kernel_grad_log_loo_cv();
    BOOST_CHECK(gp.inv_kernel_computed());
}

BOOST_AUTO_TEST_CASE(test_gp_dim)
{
    using namespace limbo;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp; // no init with dim

    std::vector<Eigen::VectorXd> observations = {make_v2(5, 5), make_v2(10, 10),
        make_v2(5, 5)};
    std::vector<Eigen::VectorXd> samples = {make_v2(1, 1), make_v2(2, 2), make_v2(3, 3)};

    gp.compute(samples, observations);

    Eigen::VectorXd mu;
    double sigma;
    std::tie(mu, sigma) = gp.query(make_v2(1, 1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(std::abs((mu(1) - 5)) < 1);

    BOOST_CHECK(sigma <= 2. * (Params::kernel::noise() + 1e-8));
}

BOOST_AUTO_TEST_CASE(test_gp)
{
    using namespace limbo;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp;
    std::vector<Eigen::VectorXd> observations = {make_v1(5), make_v1(10),
        make_v1(5)};
    std::vector<Eigen::VectorXd> samples = {make_v1(1), make_v1(2), make_v1(3)};

    gp.compute(samples, observations);

    Eigen::VectorXd mu;
    double sigma;
    std::tie(mu, sigma) = gp.query(make_v1(1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma <= 2. * (Params::kernel::noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(2));
    BOOST_CHECK(std::abs((mu(0) - 10)) < 1);
    BOOST_CHECK(sigma <= 2. * (Params::kernel::noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(3));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma <= 2. * (Params::kernel::noise() + 1e-8));

    for (double x = 0; x < 4; x += 0.05) {
        Eigen::VectorXd mu;
        double sigma;
        std::tie(mu, sigma) = gp.query(make_v1(x));
        BOOST_CHECK(gp.mu(make_v1(x)) == mu);
        BOOST_CHECK(gp.sigma(make_v1(x)) == sigma);
        // std::cout << x << " " << mu << " " << mu.array() - sigma << " "
        //           << mu.array() + sigma << std::endl;
    }
}

BOOST_AUTO_TEST_CASE(test_gp_identical_samples)
{
    using namespace limbo;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp;
    std::vector<Eigen::VectorXd> observations;
    std::vector<Eigen::VectorXd> samples;
    for (int i = 0; i < 10; i++) {
        samples.push_back(make_v1(1));
        observations.push_back(make_v1(std::cos(1)));
    }

    gp.compute(samples, observations);

    GP_t gp2;
    for (int i = 0; i < 10; i++) {
        gp2.add_sample(make_v1(1), make_v1(std::cos(1)));
    }

    // Compute kernel matrix
    Eigen::MatrixXd kernel;
    size_t n = samples.size();
    kernel.resize(n, n);

    for (size_t i = 0; i < n; i++)
        for (size_t j = 0; j <= i; ++j)
            kernel(i, j) = gp.kernel_function()(samples[i], samples[j], i, j);

    for (size_t i = 0; i < n; i++)
        for (size_t j = 0; j < i; ++j)
            kernel(j, i) = kernel(i, j);

    // Reconstruct kernels from cholesky decomposition
    Eigen::MatrixXd reconstructed_kernel = gp.matrixL() * gp.matrixL().transpose();
    Eigen::MatrixXd reconstructed_kernel2 = gp2.matrixL() * gp2.matrixL().transpose();

    // Check if the reconstructed kernels match the actual one
    BOOST_CHECK(kernel.isApprox(reconstructed_kernel, 1e-5));
    BOOST_CHECK(kernel.isApprox(reconstructed_kernel2, 1e-5));

    // Check if incremental cholesky produces the same result
    Eigen::VectorXd mu1, mu2;
    double s1, s2;

    std::tie(mu1, s1) = gp.query(make_v1(1));
    std::tie(mu2, s2) = gp2.query(make_v1(1));

    BOOST_CHECK((mu1 - mu2).norm() < 1e-4);
    BOOST_CHECK(std::sqrt((s1 - s2) * (s1 - s2)) < 1e-4);
}

BOOST_AUTO_TEST_CASE(test_gp_bw_inversion)
{
    using namespace limbo;
    size_t N = 1000;
    size_t failures = 0;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    for (size_t i = 0; i < N; i++) {

        std::vector<Eigen::VectorXd> observations;
        std::vector<Eigen::VectorXd> samples;
        tools::rgen_double_t rgen(0.0, 10);
        for (size_t i = 0; i < 100; i++) {
            observations.push_back(make_v1(rgen.rand()));
            samples.push_back(make_v1(rgen.rand()));
        }

        GP_t gp;
        auto t1 = std::chrono::steady_clock::now();
        gp.compute(samples, observations);
        // auto time_init = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t1).count();
        // std::cout.precision(17);
        // std::cout << "Time running first batch: " << time_init << "us" << std::endl
        //           << std::endl;

        observations.push_back(make_v1(rgen.rand()));
        samples.push_back(make_v1(rgen.rand()));

        t1 = std::chrono::steady_clock::now();
        gp.add_sample(samples.back(), observations.back());
        auto time_increment = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t1).count();
        // std::cout << "Time running increment: " << time_increment << "us" << std::endl
        //           << std::endl;

        t1 = std::chrono::steady_clock::now();
        gp.recompute(true);
        auto time_recompute = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t1).count();
        // std::cout << "Time recomputing: " << time_recompute << "us" << std::endl
        //           << std::endl;

        GP_t gp2;
        t1 = std::chrono::steady_clock::now();
        gp2.compute(samples, observations);
        auto time_full = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t1).count();
        // std::cout << "Time running whole batch: " << time_full << "us" << std::endl
        //           << std::endl;

        bool failed = false;

        Eigen::VectorXd s = make_v1(rgen.rand());
        if ((gp.mu(s) - gp2.mu(s)).norm() >= 1e-5)
            failed = true;
        if (!gp.matrixL().isApprox(gp2.matrixL(), 1e-5))
            failed = true;
        if (time_full <= time_increment)
            failed = true;
        if (time_recompute <= time_increment)
            failed = true;

        if (failed)
            failures++;
    }

    BOOST_CHECK(double(failures) / double(N) < 0.1);
}

BOOST_AUTO_TEST_CASE(test_gp_no_samples_acqui_opt)
{
    using namespace limbo;

    struct FirstElem {
        double operator()(const Eigen::VectorXd& x) const
        {
            return x(0);
        }
    };

    using acquiopt_t = opt::GridSearch<Params>;

    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;
    using acquisition_function_t = acqui::UCB<Params, GP_t>;

    GP_t gp(2, 2);

    acquisition_function_t acqui(gp, 0);
    acquiopt_t acqui_optimizer;

    // we do not have gradient in our current acquisition function
    auto acqui_optimization =
        [&](const Eigen::VectorXd& x, bool g) { return acqui(x, FirstElem(), g); };
    Eigen::VectorXd starting_point = tools::random_vector(2);
    Eigen::VectorXd test = acqui_optimizer(acqui_optimization, starting_point, true);
    BOOST_CHECK(test(0) < 1e-5);
    BOOST_CHECK(test(1) < 1e-5);
}

BOOST_AUTO_TEST_CASE(test_gp_auto)
{
    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t, model::gp::KernelLFOpt<Params>>;

    GP_t gp;
    std::vector<Eigen::VectorXd> observations = {make_v1(5), make_v1(10), make_v1(5)};
    std::vector<Eigen::VectorXd> samples = {make_v1(1), make_v1(2), make_v1(3)};

    gp.compute(samples, observations, false);
    gp.optimize_hyperparams();

    Eigen::VectorXd mu;
    double sigma;
    std::tie(mu, sigma) = gp.query(make_v1(1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma <= 2. * (gp.kernel_function().noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(2));
    BOOST_CHECK(std::abs((mu(0) - 10)) < 1);
    BOOST_CHECK(sigma <= 2. * (gp.kernel_function().noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(3));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma <= 2. * (gp.kernel_function().noise() + 1e-8));
}

BOOST_AUTO_TEST_CASE(test_gp_init_variance)
{
    using namespace limbo;

    struct Parameters {
        struct kernel : public defaults::kernel {
        };

        struct kernel_squared_exp_ard {
            BO_PARAM(int, k, 0);
            BO_PARAM(double, sigma_sq, 10);
        };
        struct kernel_exp {
            BO_PARAM(double, sigma_sq, 10);
            BO_PARAM(double, l, 1);
        };
        struct kernel_maternthreehalves {
            BO_PARAM(double, sigma_sq, 10);
            BO_PARAM(double, l, 0.25);
        };
        struct kernel_maternfivehalves {
            BO_PARAM(double, sigma_sq, 10);
            BO_PARAM(double, l, 0.25);
        };
    };

    // MaternThreeHalves
    using GP1_t = model::GP<Params, kernel::MaternThreeHalves<Parameters>, mean::Constant<Params>>;

    GP1_t gp1(1, 1);

    double sigma = gp1.sigma(tools::random_vector(1));

    BOOST_CHECK_CLOSE(sigma, 10.0, 1);

    // MaternFiveHalves
    using GP2_t = model::GP<Params, kernel::MaternFiveHalves<Parameters>, mean::Constant<Params>>;

    GP2_t gp2(1, 1);

    sigma = gp2.sigma(tools::random_vector(1));

    BOOST_CHECK_CLOSE(sigma, 10.0, 1);

    // Exponential
    using GP3_t = model::GP<Params, kernel::Exp<Parameters>, mean::Constant<Params>>;

    GP3_t gp3(1, 1);

    sigma = gp3.sigma(tools::random_vector(1));

    BOOST_CHECK_CLOSE(sigma, 10.0, 1);

    // ARD Squared Exponential
    using GP4_t = model::GP<Params, kernel::SquaredExpARD<Parameters>, mean::Constant<Params>>;

    GP4_t gp4(1, 1);

    sigma = gp4.sigma(tools::random_vector(1));

    BOOST_CHECK_CLOSE(sigma, 10.0, 1);
}

BOOST_AUTO_TEST_CASE(test_sparse_gp)
{
    using namespace limbo;
    constexpr size_t N = 10;
    constexpr size_t M = 100;
    size_t failures = 0;

    struct SparseParams {
        struct mean_constant : public defaults::mean_constant {
        };
        struct kernel : public defaults::kernel {
        };
        struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        };
        struct opt_rprop : public defaults::opt_rprop {
        };
        struct model_sparse_gp {
            BO_PARAM(int, max_points, M / 3);
        };
    };

    using KF_t = kernel::SquaredExpARD<SparseParams>;
    using MF_t = mean::Constant<SparseParams>;
    using GP_t = model::GP<SparseParams, KF_t, MF_t, model::gp::KernelLFOpt<SparseParams, opt::Rprop<SparseParams>>>;
    using SparsifiedGP_t = model::SparsifiedGP<SparseParams, KF_t, MF_t, model::gp::KernelLFOpt<SparseParams, opt::Rprop<SparseParams>>>;

    for (size_t i = 0; i < N; i++) {

        std::vector<Eigen::VectorXd> observations;
        std::vector<Eigen::VectorXd> samples;
        tools::rgen_double_t rgen(0.0, 10);
        for (size_t i = 0; i < M; i++) {
            observations.push_back(make_v1(rgen.rand()));
            samples.push_back(make_v1(rgen.rand()));
        }

        GP_t gp;
        auto t1 = std::chrono::steady_clock::now();
        gp.compute(samples, observations, false);
        gp.optimize_hyperparams();
        auto time_full = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t1).count();

        SparsifiedGP_t sgp;
        auto t2 = std::chrono::steady_clock::now();
        sgp.compute(samples, observations, false);
        sgp.optimize_hyperparams();
        auto time_sparse = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - t2).count();

        if (time_full <= time_sparse)
            failures++;
    }

    BOOST_CHECK(double(failures) / double(N) < 0.1);
}

BOOST_AUTO_TEST_CASE(test_sparse_gp_accuracy)
{
    using namespace limbo;
    constexpr size_t N = 20;
    constexpr size_t M = 100;
    size_t failures = 0;

    struct SparseParams {
        struct mean_constant : public defaults::mean_constant {
        };
        struct kernel : public defaults::kernel {
        };
        struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        };
        struct opt_rprop : public defaults::opt_rprop {
        };
        struct model_sparse_gp {
            BO_PARAM(int, max_points, M / 2);
        };
    };

    using KF_t = kernel::SquaredExpARD<SparseParams>;
    using MF_t = mean::Constant<SparseParams>;
    using GP_t = model::GP<SparseParams, KF_t, MF_t, model::gp::KernelLFOpt<SparseParams, opt::Rprop<SparseParams>>>;
    using SparsifiedGP_t = model::SparsifiedGP<SparseParams, KF_t, MF_t, model::gp::KernelLFOpt<SparseParams, opt::Rprop<SparseParams>>>;

    for (size_t i = 0; i < N; i++) {

        std::vector<Eigen::VectorXd> observations;
        std::vector<Eigen::VectorXd> samples;
        tools::rgen_double_t rgen(-2., 2.);
        for (size_t i = 0; i < M; i++) {
            samples.push_back(make_v1(rgen.rand()));
            observations.push_back(make_v1(std::cos(samples.back()[0])));
        }

        std::vector<Eigen::VectorXd> test_observations;
        std::vector<Eigen::VectorXd> test_samples;
        for (size_t i = 0; i < M; i++) {
            test_samples.push_back(make_v1(rgen.rand()));
            test_observations.push_back(make_v1(std::cos(test_samples.back()[0])));
        }

        GP_t gp;
        gp.compute(samples, observations, false);
        gp.optimize_hyperparams();

        SparsifiedGP_t sgp;
        sgp.compute(samples, observations, false);
        sgp.optimize_hyperparams();

        bool failed = false;

        // check if normal GP and sparse GP produce very similar results
        // in the learned points
        for (size_t i = 0; i < M; i++) {
            Eigen::VectorXd gp_val, sgp_val;
            double gp_sigma, sgp_sigma;
            std::tie(gp_val, gp_sigma) = gp.query(samples[i]);
            std::tie(sgp_val, sgp_sigma) = sgp.query(samples[i]);

            if (std::abs(gp_val[0] - sgp_val[0]) > 1e-2 || std::abs(gp_sigma - sgp_sigma) > 1e-2)
                failed = true;
        }

        // check if normal GP and sparse GP produce very similar results
        // in the test points
        for (size_t i = 0; i < M; i++) {
            Eigen::VectorXd gp_val, sgp_val;
            double gp_sigma, sgp_sigma;
            std::tie(gp_val, gp_sigma) = gp.query(test_samples[i]);
            std::tie(sgp_val, sgp_sigma) = sgp.query(test_samples[i]);

            if (std::abs(gp_val[0] - sgp_val[0]) > 1e-2 || std::abs(gp_sigma - sgp_sigma) > 1e-2)
                failed = true;
        }

        // check if normal GP and sparse GP produce very similar errors
        // in the test points
        for (size_t i = 0; i < M; i++) {
            double gp_val = gp.mu(test_samples[i])[0];
            double sgp_val = sgp.mu(test_samples[i])[0];

            double gp_error_val = std::abs(gp_val - test_observations[i][0]);
            double sgp_error_val = std::abs(sgp_val - test_observations[i][0]);

            if (std::abs(gp_error_val - sgp_error_val) > 1e-2)
                failed = true;
        }

        if (failed)
            failures++;
    }

    BOOST_CHECK(double(failures) / double(N) < 0.1);
}

BOOST_AUTO_TEST_CASE(test_multi_gp_dim)
{
    using namespace limbo;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using GP_t = model::GP<Params, KF_t, Mean_t>;

    GP_t gp; // no init with dim

    std::vector<Eigen::VectorXd> observations = {make_v2(5, 5), make_v2(10, 10),
        make_v2(5, 5)};
    std::vector<Eigen::VectorXd> samples = {make_v2(1, 1), make_v2(2, 2), make_v2(3, 3)};

    gp.compute(samples, observations);

    using MultiGP_t = model::MultiGP<Params, model::GP, KF_t, Mean_t>;
    MultiGP_t multi_gp;
    multi_gp.compute(samples, observations);

    Eigen::VectorXd mu;
    double sigma;
    std::tie(mu, sigma) = gp.query(make_v2(1, 1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(std::abs((mu(1) - 5)) < 1);

    BOOST_CHECK(sigma <= 2. * (Params::kernel::noise() + 1e-8));

    Eigen::VectorXd mu_multi, sigma_multi;

    std::tie(mu_multi, sigma_multi) = multi_gp.query(make_v2(1, 1));
    BOOST_CHECK(std::abs((mu_multi(0) - 5)) < 1);
    BOOST_CHECK(std::abs((mu_multi(1) - 5)) < 1);

    BOOST_CHECK(sigma_multi(0) <= 2. * (Params::kernel::noise() + 1e-8));
    BOOST_CHECK(sigma_multi(1) <= 2. * (Params::kernel::noise() + 1e-8));

    BOOST_CHECK(std::abs(mu_multi(0) - mu(0)) < 1e-6);
    BOOST_CHECK(std::abs(mu_multi(1) - mu(1)) < 1e-6);
    BOOST_CHECK(std::abs(sigma_multi(0) - sigma) < 1e-6);
    BOOST_CHECK(std::abs(sigma_multi(1) - sigma) < 1e-6);
}

BOOST_AUTO_TEST_CASE(test_multi_gp)
{
    using namespace limbo;

    using KF_t = kernel::MaternFiveHalves<Params>;
    using Mean_t = mean::Constant<Params>;
    using MultiGP_t = model::MultiGP<Params, model::GP, KF_t, Mean_t>;

    MultiGP_t gp;
    std::vector<Eigen::VectorXd> observations = {make_v1(5), make_v1(10),
        make_v1(5)};
    std::vector<Eigen::VectorXd> samples = {make_v1(1), make_v1(2), make_v1(3)};

    gp.compute(samples, observations);

    Eigen::VectorXd mu, sigma;
    std::tie(mu, sigma) = gp.query(make_v1(1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (Params::kernel::noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(2));
    BOOST_CHECK(std::abs((mu(0) - 10)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (Params::kernel::noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(3));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (Params::kernel::noise() + 1e-8));

    for (double x = 0; x < 4; x += 0.05) {
        Eigen::VectorXd mu, sigma;
        std::tie(mu, sigma) = gp.query(make_v1(x));
        BOOST_CHECK(gp.mu(make_v1(x)) == mu);
        BOOST_CHECK(gp.sigma(make_v1(x)) == sigma);
    }
}

BOOST_AUTO_TEST_CASE(test_sparse_multi_gp)
{
    using namespace limbo;
    constexpr size_t N = 20;
    constexpr size_t M = 100;
    size_t failures = 0;

    struct SparseParams {
        struct mean_constant : public defaults::mean_constant {
        };
        struct kernel : public defaults::kernel {
        };
        struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        };
        struct opt_rprop : public defaults::opt_rprop {
        };
        struct model_sparse_gp {
            BO_PARAM(int, max_points, M / 2);
        };
    };

    using KF_t = kernel::SquaredExpARD<SparseParams>;
    using MF_t = mean::Constant<SparseParams>;
    using MultiGP_t = model::MultiGP<SparseParams, model::GP, KF_t, MF_t, model::multi_gp::ParallelLFOpt<SparseParams, model::gp::KernelLFOpt<SparseParams>>>;
    using MultiSparseGP_t = model::MultiGP<SparseParams, model::SparsifiedGP, KF_t, MF_t, model::multi_gp::ParallelLFOpt<SparseParams, model::gp::KernelLFOpt<SparseParams>>>;

    for (size_t i = 0; i < N; i++) {

        std::vector<Eigen::VectorXd> observations;
        std::vector<Eigen::VectorXd> samples;
        tools::rgen_double_t rgen(-2., 2.);
        for (size_t i = 0; i < M; i++) {
            samples.push_back(make_v1(rgen.rand()));
            observations.push_back(make_v1(std::cos(samples.back()[0])));
        }

        std::vector<Eigen::VectorXd> test_observations;
        std::vector<Eigen::VectorXd> test_samples;
        for (size_t i = 0; i < M; i++) {
            test_samples.push_back(make_v1(rgen.rand()));
            test_observations.push_back(make_v1(std::cos(test_samples.back()[0])));
        }

        MultiGP_t gp;
        gp.compute(samples, observations, false);
        gp.optimize_hyperparams();

        MultiSparseGP_t sgp;
        sgp.compute(samples, observations, false);
        sgp.optimize_hyperparams();

        bool failed = false;

        // check if normal GP and sparse GP produce very similar results
        // in the learned points
        for (size_t i = 0; i < M; i++) {
            Eigen::VectorXd gp_val, sgp_val, gp_sigma, sgp_sigma;
            std::tie(gp_val, gp_sigma) = gp.query(samples[i]);
            std::tie(sgp_val, sgp_sigma) = sgp.query(samples[i]);

            if (std::abs(gp_val[0] - sgp_val[0]) > 1e-2 || std::abs(gp_sigma[0] - sgp_sigma[0]) > 1e-2)
                failed = true;
        }

        // check if normal GP and sparse GP produce very similar results
        // in the test points
        for (size_t i = 0; i < M; i++) {
            Eigen::VectorXd gp_val, sgp_val, gp_sigma, sgp_sigma;
            std::tie(gp_val, gp_sigma) = gp.query(test_samples[i]);
            std::tie(sgp_val, sgp_sigma) = sgp.query(test_samples[i]);

            if (std::abs(gp_val[0] - sgp_val[0]) > 1e-2 || std::abs(gp_sigma[0] - sgp_sigma[0]) > 1e-2)
                failed = true;
        }

        // check if normal GP and sparse GP produce very similar errors
        // in the test points
        for (size_t i = 0; i < M; i++) {
            double gp_val = gp.mu(test_samples[i])[0];
            double sgp_val = sgp.mu(test_samples[i])[0];

            double gp_error_val = std::abs(gp_val - test_observations[i][0]);
            double sgp_error_val = std::abs(sgp_val - test_observations[i][0]);

            if (std::abs(gp_error_val - sgp_error_val) > 1e-2)
                failed = true;
        }

        if (failed)
            failures++;
    }

    BOOST_CHECK(double(failures) / double(N) < 0.1);
}

BOOST_AUTO_TEST_CASE(test_multi_gp_auto)
{
    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::Constant<Params>;
    using MultiGP_t = model::MultiGP<Params, model::GP, KF_t, Mean_t, model::multi_gp::ParallelLFOpt<Params, model::gp::KernelLFOpt<Params>>>;

    MultiGP_t gp;
    std::vector<Eigen::VectorXd> observations = {make_v2(5, 5), make_v2(10, 10), make_v2(5, 5)};
    std::vector<Eigen::VectorXd> samples = {make_v1(1), make_v1(2), make_v1(3)};

    gp.compute(samples, observations, false);
    gp.optimize_hyperparams();

    Eigen::VectorXd mu, sigma;
    std::tie(mu, sigma) = gp.query(make_v1(1));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (gp.gp_models()[0].kernel_function().noise() + 1e-8));
    BOOST_CHECK(sigma(1) <= 2. * (gp.gp_models()[1].kernel_function().noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(2));
    BOOST_CHECK(std::abs((mu(0) - 10)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (gp.gp_models()[0].kernel_function().noise() + 1e-8));
    BOOST_CHECK(sigma(1) <= 2. * (gp.gp_models()[1].kernel_function().noise() + 1e-8));

    std::tie(mu, sigma) = gp.query(make_v1(3));
    BOOST_CHECK(std::abs((mu(0) - 5)) < 1);
    BOOST_CHECK(sigma(0) <= 2. * (gp.gp_models()[0].kernel_function().noise() + 1e-8));
    BOOST_CHECK(sigma(1) <= 2. * (gp.gp_models()[1].kernel_function().noise() + 1e-8));
}

BOOST_AUTO_TEST_CASE(test_multi_gp_recompute)
{
    using KF_t = kernel::SquaredExpARD<Params>;
    using Mean_t = mean::Constant<Params>;
    using MultiGP_t = model::MultiGP<Params, model::GP, KF_t, Mean_t>;

    MultiGP_t gp;

    gp.add_sample(make_v2(1, 1), make_v1(2));
    gp.add_sample(make_v2(2, 2), make_v1(10));

    // make sure that the observations are properly passed
    BOOST_CHECK(gp._observations[0](0) == 2.);
    BOOST_CHECK(gp._observations[1](0) == 10.);

    // make sure the child GPs have the proper observations
    BOOST_CHECK(gp.gp_models()[0]._observations.row(0)[0] == (2. - gp.mean_function().h_params()[0]));
    BOOST_CHECK(gp.gp_models()[0]._observations.row(1)[0] == (10. - gp.mean_function().h_params()[0]));

    // now change the mean function parameters
    gp.mean_function().set_h_params(make_v1(2));

    // make sure that the observations are properly passed
    BOOST_CHECK(gp._observations[0](0) == 2.);
    BOOST_CHECK(gp._observations[1](0) == 10.);

    // make sure the child GPs do not have the proper observations
    BOOST_CHECK(!(gp.gp_models()[0]._observations.row(0)[0] == (2. - gp.mean_function().h_params()[0])));
    BOOST_CHECK(!(gp.gp_models()[0]._observations.row(1)[0] == (10. - gp.mean_function().h_params()[0])));

    // recompute the GP
    gp.recompute();

    // make sure that the observations are properly passed
    BOOST_CHECK(gp._observations[0](0) == 2.);
    BOOST_CHECK(gp._observations[1](0) == 10.);

    // make sure the child GPs have the proper observations
    BOOST_CHECK(gp.gp_models()[0]._observations.row(0)[0] == (2. - gp.mean_function().h_params()[0]));
    BOOST_CHECK(gp.gp_models()[0]._observations.row(1)[0] == (10. - gp.mean_function().h_params()[0]));
}


/*
 * Copyright 2009-2021 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE nblist_3body_test

// Standard includes
#include <string>
#include <vector>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/csg/bead.h"
#include "votca/csg/beadlist.h"
#include "votca/csg/nblist_3body.h"
#include "votca/csg/topology.h"

using namespace std;
using namespace votca::csg;

BOOST_AUTO_TEST_SUITE(nblist_3body_test)

BOOST_AUTO_TEST_CASE(test_nblist_3body_constructor) { NBList_3Body nb; }

BOOST_AUTO_TEST_CASE(test_nblist_3body_generate_list) {
  NBList_3Body nb;

  nb.setCutoff(2.0);

  Topology top;

  Eigen::Matrix3d m = 5 * Eigen::Matrix3d::Identity();

  top.setBox(m);

  Eigen::Vector3d pos = Eigen::Vector3d::Zero();

  Molecule *mol;
  mol = top.CreateMolecule("UNKNOWN");

  string bead_type_name = "CG";
  top.RegisterBeadType(bead_type_name);

  string name = "dummy1";
  votca::Index resnr = 0;
  double mass = 1.0;
  double charge = -1.0;
  Bead *b;
  b = top.CreateBead(Bead::spherical, name, bead_type_name, resnr, mass,
                     charge);
  b->setPos(pos);
  mol->AddBead(b, bead_type_name);

  name = "dummy2";
  resnr = 0;
  mass = 2.0;
  charge = -2.0;
  b = top.CreateBead(Bead::spherical, name, bead_type_name, resnr, mass,
                     charge);
  mol->AddBead(b, bead_type_name);
  pos = Eigen::Vector3d::UnitX();
  b->setPos(pos);

  name = "dummy3";
  resnr = 0;
  mass = 3.0;
  charge = -3.0;
  b = top.CreateBead(Bead::spherical, name, bead_type_name, resnr, mass,
                     charge);
  mol->AddBead(b, bead_type_name);
  pos[0] = 1.0;
  pos[1] = 1.0;
  pos[2] = 0.0;
  b->setPos(pos);

  BeadList beads;
  beads.Generate(top, "CG");

  nb.Generate(beads, true);

  BOOST_CHECK_EQUAL(nb.size(), 3);

  NBList_3Body::iterator triple_iter;
  triple_iter = nb.begin();
  BOOST_CHECK_EQUAL((*triple_iter)->bead1()->getId(), 0);
  BOOST_CHECK_EQUAL((*triple_iter)->bead2()->getId(), 1);
  BOOST_CHECK_EQUAL((*triple_iter)->bead3()->getId(), 2);
  BOOST_CHECK_CLOSE((*triple_iter)->dist12(), 1.0, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist13(), 1.414214, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist23(), 1.0, 1e-4);

  ++triple_iter;

  BOOST_CHECK_EQUAL((*triple_iter)->bead1()->getId(), 1);
  BOOST_CHECK_EQUAL((*triple_iter)->bead2()->getId(), 0);
  BOOST_CHECK_EQUAL((*triple_iter)->bead3()->getId(), 2);
  BOOST_CHECK_CLOSE((*triple_iter)->dist12(), 1.0, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist13(), 1.0, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist23(), 1.414214, 1e-4);

  ++triple_iter;

  BOOST_CHECK_EQUAL((*triple_iter)->bead1()->getId(), 2);
  BOOST_CHECK_EQUAL((*triple_iter)->bead2()->getId(), 0);
  BOOST_CHECK_EQUAL((*triple_iter)->bead3()->getId(), 1);
  BOOST_CHECK_CLOSE((*triple_iter)->dist12(), 1.414214, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist13(), 1.0, 1e-4);
  BOOST_CHECK_CLOSE((*triple_iter)->dist23(), 1.0, 1e-4);
}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
//!
//! \file   Utility_IsFloatingPoint.hpp
//! \author Alex Robinson
//! \brief  IsFloatingPoint helper class specializations
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_IS_FLOATING_POINT_HPP
#define UTILITY_IS_FLOATING_POINT_HPP

// Std Lib Includes
#include <type_traits>

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

// FRENSIE Includes
#include "Utility_IsFloatingPointDecl.hpp"

namespace Utility{

/*! \brief Partial specialization of IsFloatingPoint for
 * boost::units::quantity types.
 */
template<typename Unit, typename T>
struct IsFloatingPoint<boost::units::quantity<Unit,T>, typename std::enable_if<std::is_floating_point<T>::value>::type> : public boost::true_type {};

/*! \brief Partial specialization of IsFloatingPoint for
 * boost::multiprecision::number<boost::multiprecision::cpp_dec_float<int> >
 */
template<unsigned N>
struct IsFloatingPoint<boost::multiprecision::number<boost::multiprecision::backends::cpp_dec_float<N> > > : public boost::true_type {};

} // end Utility namespace

#endif // end UTILITY_IS_FLOATING_POINT_HPP

//---------------------------------------------------------------------------//
// end Utility_IsFloatingPoint.hpp
//---------------------------------------------------------------------------//