/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

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.
==============================================================================*/

#pragma once

#include <sfc/PeanoAlgorithms.hpp>
#include <time/Stopwatch.hpp>

#include <Eigen/Dense>

#include <functional>
#include <random>

namespace sfcpp {
namespace test {
double hilbert2DNeighborPerformance(size_t level, size_t numSamples);
double hilbert3DNeighborPerformance(size_t level, size_t numSamples);
double morton2DNeighborPerformance(size_t level, size_t numSamples);
double sierpinski2DNeighborPerformance(size_t level, size_t numSamples);

Eigen::MatrixXd doTimeMeasurements(size_t lmin, size_t lmax,
                                   std::function<double(size_t)> func);

void createPeanoDimPerformancePlots(size_t numSamples);
void createPeanoDepthPerformancePlots(size_t numSamples);
void create2DPerformancePlots(size_t numSamples);
void createStatePerformancePlots(size_t numSamples);

template <size_t d>
double peanoNeighborPerformance(size_t level, size_t numSamples,
                                size_t tableDepth) {
  sfc::PeanoAlgorithms<d> peano(level, tableDepth, tableDepth);
  auto numPoints = peano.getNumPoints();
  size_t numFacets = 2 * d;
  std::mt19937 gen;
  std::uniform_int_distribution<uint32_t> idxDist(0, numPoints - 1);
  std::uniform_int_distribution<uint32_t> faceDist(0, numFacets - 1);

  size_t sum = 0;

  time::Stopwatch stopwatch;

  for (size_t i = 0; i < numSamples; ++i) {
    auto idx = idxDist(gen);
    auto face = faceDist(gen);
    sum += peano.computeCellNeighborByLookup(idx, (sum + face) % numFacets);
  }

  double time = stopwatch.elapsedSeconds();

  std::cout << "Time for computation: " << time / numSamples * 1000000000
            << "ns\n";

  gen.seed(gen.default_seed);

  stopwatch.start();

  for (size_t i = 0; i < numSamples; ++i) {
    sum += (idxDist(gen) + faceDist(gen) + sum) % numFacets;
  }

  double defaultTime = stopwatch.elapsedSeconds();

  std::cout << "Time for noOp: " << defaultTime / numSamples * 1000000000
            << "ns\n";

  std::cout << "sum: " << sum << "\n";

  return (time - defaultTime) * 1000000000 / numSamples;
}

template <size_t d>
double peanoStatePerformance(size_t level, size_t numSamples,
                             size_t tableDepth) {
  sfc::PeanoAlgorithms<d> peano(level, tableDepth, tableDepth);
  auto numPoints = peano.getNumPoints();
  std::mt19937 gen;
  std::uniform_int_distribution<uint32_t> idxDist(0, numPoints - 1);

  size_t sum = 0;

  time::Stopwatch stopwatch;

  for (size_t i = 0; i < numSamples; ++i) {
    auto idx = idxDist(gen);
    sum += peano.computeOrientationBinaryByLookup((sum + idx) % numPoints);
  }

  double time = stopwatch.elapsedSeconds();

  std::cout << "Time for computation: " << time / numSamples * 1000000000
            << "ns\n";

  gen.seed(gen.default_seed);

  stopwatch.start();

  for (size_t i = 0; i < numSamples; ++i) {
    sum += (idxDist(gen) + sum) % numPoints;
  }

  double defaultTime = stopwatch.elapsedSeconds();

  std::cout << "Time for noOp: " << defaultTime / numSamples * 1000000000
            << "ns\n";

  std::cout << "sum: " << sum << "\n";

  return (time - defaultTime) * 1000000000 / numSamples;
}
}  // namespace test
}  // namespace sfcpp


#include <epl_plugin.hpp>
#include <stdexcept>
#include <type_traits>
#include <iostream>

//TODO #include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using com::apama::epl::EPLPlugin;
using com::apama::epl::data_t;
using com::apama::epl::custom_t;
using com::apama::epl::get;
using com::apama::epl::apply_visitor;
using com::apama::epl::const_visitor;

namespace com::apamax::multiprecision {

class multiprecision_error: public std::runtime_error
{
	using std::runtime_error::runtime_error;
};

/** Random plugin class */
class Multiprecision: public EPLPlugin<Multiprecision>
{
private:
	struct MultiprecisionBase
	{
		enum type_t {
			ARBITRARY=1,
			FIXED_128=128,
			FIXED_256=256,
			FIXED_512=512,
			FIXED_1024=1024
		};
		virtual void add(const data_t &o) = 0;
		virtual void sub(const data_t &o) = 0;
		virtual void set(const data_t &o) = 0;
		virtual void mod(const data_t &o) = 0;
		virtual void pow(const data_t &o) = 0;
		virtual void mul(const data_t &o) = 0;
		virtual void div(const data_t &o) = 0;
		virtual void abs() = 0;
		virtual int64_t comp(const data_t &o) = 0;
		virtual std::string toString() = 0;
		virtual double toFloat() = 0;
		virtual int64_t toInteger(int64_t offset) = 0;
		virtual custom_t<MultiprecisionBase> clone() = 0;
		virtual enum type_t type() = 0;
	};

	struct setOp { template<typename A, typename B> void operator()(A&a, const B&b) { a.assign(b); } };
	struct subOp { template<typename A, typename B> void operator()(A&a, const B&b) { a -= b; } };
	struct addOp { template<typename A, typename B> void operator()(A&a, const B&b) { a += b; } };
	struct modOp { template<typename A, typename B> void operator()(A&a, const B&b) { a %= b; } };
	struct mulOp { template<typename A, typename B> void operator()(A&a, const B&b) { a *= b; } };
	struct divOp { template<typename A, typename B> void operator()(A&a, const B&b) { a /= b; } };
	struct powOp { template<typename A, typename B> void operator()(A&a, const B&b) { /* TODO a = ::pow(a, b);*/ } };
	struct compOp { template<typename A, typename B> int64_t operator()(A&a, const B&b) { return a.compare(b); } };

	template<typename T, typename OP, typename RV>
	struct biopVisitor: public const_visitor<biopVisitor<T, OP, RV>, RV>
	{
		biopVisitor(T &t): t{t} {}
		T &t;
		RV visitInteger(int64_t i) const { return OP()(t, i); }
		RV visitCustom(const sag_underlying_custom_t &i) const;
	};

	template<MultiprecisionBase::type_t TYPE, typename IMPL>
	struct MultiprecisionBaseImpl: public MultiprecisionBase
	{
		IMPL data;
		MultiprecisionBaseImpl(const IMPL &initial): data(initial) { }
		MultiprecisionBaseImpl(const data_t &initial) { set(initial); }
		type_t type() override { return TYPE; }
		void set(const data_t &o) override {
			if (SAG_DATA_STRING == o.type_tag()) {
				data.assign(get<const char*>(o));
			} else {
				apply_visitor(biopVisitor<IMPL, setOp, void>(data), o);
			}
		}

		void add(const data_t &o) override { apply_visitor(biopVisitor<IMPL, addOp, void>(data), o); }
		void sub(const data_t &o) override { apply_visitor(biopVisitor<IMPL, subOp, void>(data), o); }
		void mod(const data_t &o) override { apply_visitor(biopVisitor<IMPL, modOp, void>(data), o); }
		void pow(const data_t &o) override { apply_visitor(biopVisitor<IMPL, powOp, void>(data), o); }
		void mul(const data_t &o) override { apply_visitor(biopVisitor<IMPL, mulOp, void>(data), o); }
		void div(const data_t &o) override { apply_visitor(biopVisitor<IMPL, divOp, void>(data), o); }
		void abs() override { /* TODO data = ::abs(data);*/ }
		int64_t comp(const data_t &o) override { return apply_visitor(biopVisitor<IMPL, compOp, int64_t>(data), o); }
		std::string toString() override { return data.str(); }
		double toFloat() override { return data.template convert_to<double>(); }
		int64_t toInteger(int64_t offset) override
		{
			IMPL tmp = data;
			tmp <<= offset;
			return tmp.template convert_to<int64_t>();
		}
		custom_t<MultiprecisionBase> clone() override;
	};

	template<MultiprecisionBase::type_t SIZE>
	using FixedPrecision = MultiprecisionBaseImpl<SIZE, boost::multiprecision::number<boost::multiprecision::cpp_int_backend<SIZE, SIZE, boost::multiprecision::signed_magnitude, boost::multiprecision::checked, void> > >;

	//TODO using ArbitraryPrecision = MultiprecisionBaseImpl<MultiprecisionBase::ARBITRARY, boost::multiprecision::mpz_int>;
	using ArbitraryPrecision = MultiprecisionBaseImpl<MultiprecisionBase::ARBITRARY, boost::multiprecision::number<boost::multiprecision::cpp_int_backend<128, 128, boost::multiprecision::signed_magnitude, boost::multiprecision::checked, void> >>;

public:
	Multiprecision(): base_plugin_t("Multiprecision") {}
	/** expose the methods to EPL */
	static void initialize(base_plugin_t::method_data_t &md)
	{
		md.registerMethod<decltype(&Multiprecision::createFixedPrecision), &Multiprecision::createFixedPrecision>("createFixedPrecision", "action<integer, any> returns chunk");
		md.registerMethod<decltype(&Multiprecision::createArbitraryPrecision), &Multiprecision::createArbitraryPrecision>("createArbitraryPrecision", "action<integer, any> returns chunk");
		md.registerMethod<decltype(&Multiprecision::sub), &Multiprecision::sub>("sub", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::set), &Multiprecision::set>("set", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::mod), &Multiprecision::mod>("mod", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::pow), &Multiprecision::pow>("pow", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::mul), &Multiprecision::mul>("mul", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::div), &Multiprecision::div>("div", "action<chunk, any>");
		md.registerMethod<decltype(&Multiprecision::comp), &Multiprecision::comp>("comp", "action<chunk, any> returns integer");
		md.registerMethod<decltype(&Multiprecision::abs), &Multiprecision::abs>("abs");
		md.registerMethod<decltype(&Multiprecision::toString), &Multiprecision::toString>("toString");
		md.registerMethod<decltype(&Multiprecision::toFloat), &Multiprecision::toFloat>("toFloat");
		md.registerMethod<decltype(&Multiprecision::toInteger), &Multiprecision::toInteger>("toInteger");
		md.registerMethod<decltype(&Multiprecision::toFixed), &Multiprecision::toFixed>("toFixed", "action<chunk, integer> returns com.apamax.bignum.FixedPrecisionInt");
		md.registerMethod<decltype(&Multiprecision::toArbitrary), &Multiprecision::toArbitrary>("toArbitrary", "action<chunk> returns com.apamax.bignum.ArbitraryPrecisionInt");
		md.registerMethod<decltype(&Multiprecision::clone), &Multiprecision::clone>("clone");
	}
private:

	custom_t<MultiprecisionBase> createFixedPrecision(int64_t precision, const data_t &initial)
	{
		switch (precision) {
			case 128: {
				custom_t<FixedPrecision<MultiprecisionBase::FIXED_128>> val{new FixedPrecision<MultiprecisionBase::FIXED_128>(initial)};
				return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
			}
			case 256: {
				custom_t<FixedPrecision<MultiprecisionBase::FIXED_256>> val{new FixedPrecision<MultiprecisionBase::FIXED_256>(initial)};
				return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
			}
			case 512: {
				custom_t<FixedPrecision<MultiprecisionBase::FIXED_512>> val{new FixedPrecision<MultiprecisionBase::FIXED_512>(initial)};
				return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
			}
			case 1024: {
				custom_t<FixedPrecision<MultiprecisionBase::FIXED_1024>> val{new FixedPrecision<MultiprecisionBase::FIXED_1024>(initial)};
				return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
			}
			default: throw multiprecision_error("Invalid fixed precision, must be one of 128, 156, 512 or 1024");
		}
	}
	custom_t<MultiprecisionBase> createArbitraryPrecision(const data_t &initial)
	{
		custom_t<ArbitraryPrecision> val{new ArbitraryPrecision(initial)};
		return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
	}

	void add(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->add(o); }
	void sub(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->sub(o); }
	void set(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->set(o); }
	void mod(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->mod(o); }
	void pow(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->pow(o); }
	void mul(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->mul(o); }
	void div(const custom_t<MultiprecisionBase> &v, const data_t &o) { v->div(o); }
	int64_t comp(const custom_t<MultiprecisionBase> &v, const data_t &o) { return v->comp(o); }
	void abs(const custom_t<MultiprecisionBase> &v) { v->abs(); }
	std::string toString(const custom_t<MultiprecisionBase> &v) { return v->toString(); }
	double toFloat(const custom_t<MultiprecisionBase> &v) { return v->toFloat(); }
	double toInteger(const custom_t<MultiprecisionBase> &v, int64_t offset) { return v->toInteger(offset); }
	data_t toFixed(const custom_t<MultiprecisionBase> &v, int64_t size) {return data_t(); /*TODO*/}
	data_t toArbitrary(const custom_t<MultiprecisionBase> &v) { return data_t(); /*TODO*/}
	custom_t<MultiprecisionBase> clone(const custom_t<MultiprecisionBase> &v) { return v->clone(); }
};

template<typename T, typename OP, typename RV>
RV Multiprecision::biopVisitor<T, OP, RV>::visitCustom(const sag_underlying_custom_t &i) const
{
	const custom_t<MultiprecisionBase> &base = reinterpret_cast<const custom_t<MultiprecisionBase> &>(i);
	switch (base->type()) {
		case MultiprecisionBase::ARBITRARY:
			return OP()(t, static_cast<const ArbitraryPrecision&>(*base).data.convert_to<T>());
		case MultiprecisionBase::FIXED_128:
			return OP()(t, static_cast<const FixedPrecision<MultiprecisionBase::FIXED_128>&>(*base).data.convert_to<T>());
		case MultiprecisionBase::FIXED_256:
			return OP()(t, static_cast<const FixedPrecision<MultiprecisionBase::FIXED_256>&>(*base).data.convert_to<T>());
		case MultiprecisionBase::FIXED_512:
			return OP()(t, static_cast<const FixedPrecision<MultiprecisionBase::FIXED_512>&>(*base).data.convert_to<T>());
		case MultiprecisionBase::FIXED_1024:
			return OP()(t, static_cast<const FixedPrecision<MultiprecisionBase::FIXED_1024>&>(*base).data.convert_to<T>());
		default:
				assert(false); // shouldn't happen
				throw multiprecision_error("Unknown error");
	}
}

template<Multiprecision::MultiprecisionBase::type_t TYPE, typename IMPL>
custom_t<Multiprecision::MultiprecisionBase> Multiprecision::MultiprecisionBaseImpl<TYPE, IMPL>::clone()
{
	if constexpr (TYPE == MultiprecisionBase::ARBITRARY) {
		custom_t<ArbitraryPrecision> val{new ArbitraryPrecision(data)};
		return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
	} else {
		custom_t<FixedPrecision<TYPE>> val{new FixedPrecision<TYPE>(data)};
		return std::move(reinterpret_cast<custom_t<MultiprecisionBase>&>(val));
	}
}

APAMA_DECLARE_EPL_PLUGIN(Multiprecision)

} // com::apamax::multiprecision


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *
 *    Notes:
 *      Test tolerance was set at 5.0e-15 (or 5.0e-7 for floats) instead of epsilon due to
 *      rounding errors in Eigen types with entries over a number of orders of magnitude,
 *      presumably causing the observed larger than epsilon relative differences between
 *      expected and computed values.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <vector>

#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <Eigen/Core>

#include "tudat/basics/testMacros.h"

#include "tudat/astro/basic_astro/accelerationModel.h"
#include "tudat/astro/basic_astro/testAccelerationModels.h"
#include "tudat/astro/basic_astro/testBody.h"
#include "tudat/basics/basicTypedefs.h"

namespace tudat
{
namespace unit_tests
{

using basic_astrodynamics::AccelerationModel;
using basic_astrodynamics::updateAndGetAcceleration;

BOOST_AUTO_TEST_SUITE( test_accelerationModel )

//! Test whether DerivedAccelerationModel (acceleration data type=Eigen::Vector3d) functions
//! correctly.
BOOST_AUTO_TEST_CASE( test_derived3dAccelerationModel )
{
    using basic_astrodynamics::AccelerationModel3dPointer;

    // Shortcuts.
    typedef TestBody< 3, double > TestBody3d;
    typedef std::shared_ptr< TestBody3d > TestBody3dPointer;
    typedef DerivedAccelerationModel< > DerivedAccelerationModel3d;

    // Create body with initial state and time.
    TestBody3dPointer body = std::make_shared< TestBody3d >(
                ( Eigen::Vector6d( ) << 1.1, 2.2, 3.3, -0.1, 0.2, 0.3 ).finished( ), 2.0 );

    // Create acceleration model using DerivedAccelerationModel class, and pass pointers to
    // functions in body.
    AccelerationModel3dPointer accelerationModel3d
            = std::make_shared< DerivedAccelerationModel3d >(
                std::bind( &TestBody3d::getCurrentPosition, body ),
                std::bind( &TestBody3d::getCurrentTime, body ) );

    // Declare container of computed accelerations.
    std::vector< Eigen::Vector3d > computedAccelerations( 3 );

    // Get acceleration vector before members are updated.
    computedAccelerations.at( 0 ) = accelerationModel3d->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                -1.1, ( Eigen::Vector6d( )
                        << -0.45, 10.63, -9.81, 0.11, 0.22, 0.33 ).finished( ) );

    // Update acceleration model members.
    accelerationModel3d->updateMembers( );

    // Get acceleration vector, now after members have been updated.
    computedAccelerations.at( 1 ) = accelerationModel3d->getAcceleration( );

    // Update time and state.
    body->setCurrentTimeAndState(
                4.6, ( Eigen::Vector6d( )
                       << -87.685, 101.44, -1.38, -0.12, 0.23, -0.34 ).finished( ) );

    // Update and get acceleration with single function.
    computedAccelerations.at( 2 ) = updateAndGetAcceleration( accelerationModel3d );

    // Set expected accelerations.
    std::vector< Eigen::Vector3d > expectedAccelerations;
    expectedAccelerations.push_back( Eigen::Vector3d(   1.1,     2.2,   3.3  ) / (  2.0 *  2.0 ) );
    expectedAccelerations.push_back( Eigen::Vector3d(  -0.45,   10.63, -9.81 ) / ( -1.1 * -1.1 ) );
    expectedAccelerations.push_back( Eigen::Vector3d( -87.685, 101.44, -1.38 ) / (  4.6 *  4.6 ) );

    // Check that the acceleration vectors before and after the update match expected values.
    for ( unsigned int i = 0; i < computedAccelerations.size( ); i++ )
    {
        TUDAT_CHECK_MATRIX_BASE( computedAccelerations.at( i ), expectedAccelerations.at( i ) )
                BOOST_CHECK_CLOSE_FRACTION( computedAccelerations.at( i ).coeff( row, col ),
                                   expectedAccelerations.at( i ).coeff( row, col ), 5.0e-15 );
    }
}

////! Test whether DerivedAccelerationModel (acceleration data type=Eigen::Vector1i) functions
////! correctly.
//BOOST_AUTO_TEST_CASE( test_derived1iAccelerationModel )
//{
//    // Shortcuts.
//    typedef TestBody< 1, int > TestBody1i;
//    typedef std::shared_ptr< TestBody1i > TestBody1iPointer;
//    typedef AccelerationModel< int > AccelerationModel1i;
//    typedef std::shared_ptr< AccelerationModel1i > AccelerationModel1iPointer;
//    typedef DerivedAccelerationModel< int, int, int > DerivedAccelerationModel1i;

//    // Create body with initial state and time.
//    TestBody1iPointer body = std::make_shared< TestBody1i >( Eigen::Vector2i( 20, 1 ), 2 );

//    // Create acceleration model using DerivedAccelerationModel class, and pass pointers to
//    // functions in body.
//    AccelerationModel1iPointer accelerationModeli1
//            = std::make_shared< DerivedAccelerationModel1i >(
//                std::bind( ( &TestBody1i::getCurrentPosition ), body ),
//                std::bind( ( &TestBody1i::getCurrentTime ), body ) );

//    // Declare container of computed accelerations.
//    std::vector< int > computedAccelerations( 3 );

//    // Get scalar acceleration before members are updated.
//    computedAccelerations.at( 0 ) = accelerationModeli1->getAcceleration( );

//    // Update time and state.
//    body->setCurrentTimeAndState( -3, Eigen::Vector2i( 90, 3 ) );

//    // Update acceleration model members.
//    accelerationModeli1->updateMembers( );

//    // Get scalar acceleration, now after members have been updated.
//    computedAccelerations.at( 1 ) = accelerationModeli1->getAcceleration( );

//    // Update time and state.
//    body->setCurrentTimeAndState( 4, Eigen::Vector2i( 16, -4 ) );

//    // Update and get scalar acceleration with single function.
//    computedAccelerations.at( 2 ) = updateAndGetAcceleration( accelerationModeli1 );

//    // Set expected accelerations.
//    const std::vector< int > expectedAccelerations = { 20 / ( 2 * 2 ), 90 / ( -3 * -3 ), 16 / ( 4 * 4 ) };

//    // Check that the acceleration vectors before and after the update match expected values.
//    for ( unsigned int i = 0; i < computedAccelerations.size( ); i++ )
//    {
//        BOOST_CHECK_EQUAL( computedAccelerations.at( i ), expectedAccelerations.at( i ) );
//    }
//}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
///
/// Code related to BTE calculations beyond the RTA.

#include <constants.hpp>
#include <structures.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <isotopic_scattering.hpp>
#include <boost/mpi.hpp>
#include <Eigen/Dense>

namespace alma {
namespace beyondRTA {
/// Obtain the thermal conductivity of a bulk system
/// under the full BTE (beyond Relaxation Time Approximation).
/// This is achieved by deterministic solution a linear system
/// for the irreducible q-points.
///
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] syms - symmetry operations object
/// @param[in] threeph_procs - list of 3-phonon processes
/// @param[in] twoph_procs - list of 2-phonon processes
/// @param[in] w0 - RTA scattering rates
/// @param[in] T - temperature in K
/// @param[in] iterative - use iterative Eigen solver for faster computation
/// @return the thermal conductivity tensor in SI units
Eigen::MatrixXd calc_kappa(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    const std::vector<alma::Threeph_process>& threeph_procs,
    const std::vector<alma::Twoph_process>& twoph_procs,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    double T,
    bool iterative,
    boost::mpi::communicator& world);
} // namespace beyondRTA
} // namespace alma


#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <loopclosure-common/types.h>
#include <maplab-common/feature-descriptor-ref.h>
#include <maplab-common/test/testing-entrypoint.h>

#include "vocabulary-tree/simple-kmeans.h"
#include "vocabulary-tree/types.h"

static const int kDescriptorBytes = 64;

TEST(VocabularyTree, SimpleKMeans_BinaryClustering) {
  std::mt19937 generator(40);
  static const size_t K = 10;

  loop_closure::DescriptorContainer descriptors;

  descriptors.resize(kDescriptorBytes, 50 * K);

  using loop_closure::DescriptorType;
  typedef std::vector<DescriptorType> FeatureVector;
  std::shared_ptr<FeatureVector> centers(new FeatureVector);
  std::vector<unsigned int> membership;

  std::vector<DescriptorType> descriptor_refs;
  for (int i = 0; i < descriptors.cols(); ++i) {
    DescriptorType descriptor_ref(
        &descriptors(0, i), descriptors.rows(), false);
    descriptor_ref.SetRandom(generator());
    descriptor_refs.push_back(descriptor_ref);
  }

  DescriptorType descriptor_zero(kDescriptorBytes);
  descriptor_zero.SetZero();

  loop_closure::SimpleKmeans<DescriptorType,
                             loop_closure::distance::Hamming<DescriptorType> >
      kmeans(descriptor_zero);
  kmeans.Cluster(descriptor_refs, K, generator(), &membership, &centers);

  std::vector<unsigned int> membercnt;
  membercnt.resize(centers->size(), 0);
  for (unsigned int member : membership) {
    ++membercnt[member];
  }
  for (unsigned int count : membercnt) {
    EXPECT_NE(count, static_cast<unsigned int>(0));
  }

  loop_closure::distance::Hamming<DescriptorType> hamming_distance;

  for (int descriptor_idx = 0; descriptor_idx < descriptors.cols();
       ++descriptor_idx) {
    DescriptorType descriptor(
        &descriptors(0, descriptor_idx), descriptors.rows(), false);
    int closest_center = -1;
    unsigned int closest_distance = std::numeric_limits<unsigned int>::max();
    unsigned int second_closest_distance =
        std::numeric_limits<unsigned int>::max();
    for (size_t center_idx = 0; center_idx < centers->size(); ++center_idx) {
      unsigned int distance =
          hamming_distance(descriptor, centers->at(center_idx));
      if (distance < closest_distance) {
        second_closest_distance = closest_distance;
        closest_distance = distance;
        closest_center = center_idx;
      }
    }
    // Check that we don't have a trivial solution.
    EXPECT_NE(second_closest_distance, closest_distance);
    EXPECT_NE(closest_center, -1);
    unsigned int dist_closest_center =
        hamming_distance(descriptor, centers->at(membership[descriptor_idx]));
    EXPECT_LE(dist_closest_center, closest_distance + kDescriptorBytes * 2);
  }
}

MAPLAB_UNITTEST_ENTRYPOINT


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_LOCAL_OPERATOR_HPP
#define NETKET_LOCAL_OPERATOR_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <algorithm>
#include <cassert>
#include <complex>
#include <iostream>
#include <iterator>
#include <limits>
#include <map>
#include <vector>
#include "Hilbert/abstract_hilbert.hpp"
#include "Utils/kronecker_product.hpp"
#include "Utils/next_variation.hpp"
#include "abstract_operator.hpp"

namespace netket {

/**
    Class for local operators acting on a list of sites and for generic local
    Hilbert spaces.
*/

class LocalOperator : public AbstractOperator {
 public:
  using MelType = Complex;
  using MatType = std::vector<std::vector<MelType>>;
  using SiteType = std::vector<int>;
  using MapType = std::map<std::vector<double>, int>;
  using StateType = std::vector<std::vector<double>>;
  using ConnType = std::vector<std::vector<int>>;
  using VectorType = AbstractOperator::VectorType;
  using VectorRefType = AbstractOperator::VectorRefType;
  using VectorConstRefType = AbstractOperator::VectorConstRefType;

 private:
  const AbstractHilbert &hilbert_;
  std::vector<MatType> mat_;
  std::vector<SiteType> sites_;

  std::vector<MapType> invstate_;
  std::vector<StateType> states_;
  std::vector<ConnType> connected_;

  double constant_;

  std::size_t nops_;

  static constexpr double mel_cutoff_ = 1.0e-6;

 public:
  explicit LocalOperator(const AbstractHilbert &hilbert, double constant = 0.)
      : hilbert_(hilbert), constant_(constant), nops_(0) {}

  explicit LocalOperator(const AbstractHilbert &hilbert,
                         const std::vector<MatType> &mat,
                         const std::vector<SiteType> &sites,
                         double constant = 0.)
      : hilbert_(hilbert), constant_(constant) {
    for (std::size_t i = 0; i < mat.size(); i++) {
      Push(mat[i], sites[i]);
    }
    Init();
  }

  explicit LocalOperator(const AbstractHilbert &hilbert, const MatType &mat,
                         const SiteType &sites, double constant = 0.)
      : hilbert_(hilbert), constant_(constant) {
    Push(mat, sites);
    // TODO sort sites and swap columns of mat accordingly
    Init();
  }

  void Push(const MatType &mat, const SiteType &sites) {
    std::size_t found = std::distance(
        sites_.begin(), std::find(sites_.begin(), sites_.end(), sites));
    if (found < sites_.size()) {
      for (std::size_t i = 0; i < mat_[found].size(); i++) {
        for (std::size_t j = 0; j < mat_[found][i].size(); j++) {
          mat_[found][i][j] += mat[i][j];
        }
      }
    } else {
      mat_.push_back(mat);
      sites_.push_back(sites);
    }
  }

  void Init() {
    if (!hilbert_.IsDiscrete()) {
      throw InvalidInputError(
          "Cannot construct operators on infinite local hilbert spaces");
    }
    if (sites_.size() != mat_.size()) {
      throw InvalidInputError(
          "Operators must specify a consistent set of acting_on specifiers");
    }

    nops_ = mat_.size();

    connected_.clear();
    states_.clear();
    invstate_.clear();

    connected_.resize(nops_);
    states_.resize(nops_);
    invstate_.resize(nops_);

    for (std::size_t op = 0; op < nops_; op++) {
      const auto sites = sites_[op];
      const auto mat = mat_[op];
      auto &connected = connected_[op];
      auto &states = states_[op];
      auto &invstate = invstate_[op];

      if (*std::max_element(sites.begin(), sites.end()) >= hilbert_.Size() ||
          *std::min_element(sites.begin(), sites.end()) < 0) {
        throw InvalidInputError("Operator acts on an invalid set of sites");
      }

      auto localstates = hilbert_.LocalStates();
      const auto localsize = localstates.size();

      // Finding the non-zero matrix elements
      const double epsilon = mel_cutoff_;

      connected.resize(mat.size());

      if (mat.size() != std::pow(localsize, sites.size())) {
        throw InvalidInputError(
            "Matrix size in operator is inconsistent with Hilbert space");
      }

      for (std::size_t i = 0; i < mat.size(); i++) {
        for (std::size_t j = 0; j < mat[i].size(); j++) {
          if (mat.size() != mat[i].size()) {
            throw InvalidInputError(
                "Matrix size in operator is inconsistent with Hilbert space");
          }

          if (i != j && std::abs(mat[i][j]) > epsilon) {
            connected[i].push_back(j);
          }
        }
      }

      // Construct the mapping
      // Internal index -> State
      std::vector<double> st(sites.size(), 0);

      do {
        states.push_back(st);
      } while (netket::next_variation(st.begin(), st.end(), localsize - 1));

      for (std::size_t i = 0; i < states.size(); i++) {
        for (std::size_t k = 0; k < states[i].size(); k++) {
          states[i][k] = localstates[states[i][k]];
        }
      }

      // Now construct the inverse mapping
      // State -> Internal index
      std::size_t k = 0;
      for (auto state : states) {
        invstate[state] = k;
        k++;
      }

      assert(k == mat.size());
    }
  }

  void FindConn(VectorConstRefType v, std::vector<Complex> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const override {
    assert(v.size() == hilbert_.Size());

    connectors.clear();
    newconfs.clear();
    mel.clear();

    connectors.resize(1);
    newconfs.resize(1);
    mel.resize(1);

    mel[0] = constant_;
    connectors[0].resize(0);
    newconfs[0].resize(0);

    for (std::size_t opn = 0; opn < nops_; opn++) {
      int st1 = StateNumber(v, opn);

      assert(st1 < int(mat_[opn].size()));
      assert(st1 < int(connected_[opn].size()));

      mel[0] += (mat_[opn][st1][st1]);

      // off-diagonal part
      for (auto st2 : connected_[opn][st1]) {
        connectors.push_back(sites_[opn]);
        assert(st2 < int(states_[opn].size()));
        newconfs.push_back(states_[opn][st2]);
        mel.push_back(mat_[opn][st1][st2]);
      }
    }
  }

  // FindConn for a specific operator
  void FindConn(std::size_t opn, VectorConstRefType v,
                std::vector<Complex> &mel,
                std::vector<std::vector<int>> &connectors,
                std::vector<std::vector<double>> &newconfs) const {
    assert(opn < mat_.size() && opn >= 0);

    mel.resize(1, 0.);
    connectors.resize(1);
    newconfs.resize(1);

    int st1 = StateNumber(v, opn);
    assert(st1 < int(mat_[opn].size()));
    assert(st1 < int(connected_[opn].size()));

    mel[0] = (mat_[opn][st1][st1]);

    // off-diagonal part
    for (auto st2 : connected_[opn][st1]) {
      connectors.push_back(sites_[opn]);
      assert(st2 < int(states_[opn].size()));
      newconfs.push_back(states_[opn][st2]);
      mel.push_back(mat_[opn][st1][st2]);
    }
  }

  inline int StateNumber(VectorConstRefType v, int opn) const {
    // TODO use a mask instead of copies
    std::vector<double> state(sites_[opn].size());
    for (std::size_t i = 0; i < sites_[opn].size(); i++) {
      state[i] = v(sites_[opn][i]);
    }
    return invstate_[opn].at(state);
  }

  // Product of two local operators, performing KroneckerProducts as necessary
  friend LocalOperator operator*(const LocalOperator &lhs,
                                 const LocalOperator &rhs) {
    // TODO
    // assert(lhs.Hilbert() == rhs.Hilbert());
    // check if sites have intersections, in that case this algorithm is wrong
    std::vector<MatType> mat;
    std::vector<SiteType> sites;

    for (std::size_t opn = 0; opn < lhs.mat_.size(); opn++) {
      for (std::size_t opn1 = 0; opn1 < rhs.mat_.size(); opn1++) {
        if (lhs.sites_[opn] == rhs.sites_[opn1]) {
          mat.push_back(netket::MatrixProduct(lhs.mat_[opn], rhs.mat_[opn1]));
          sites.push_back(lhs.sites_[opn]);
        } else {
          mat.push_back(
              netket::KroneckerProduct(lhs.mat_[opn], rhs.mat_[opn1]));
          SiteType sitesum = lhs.sites_[opn];
          sitesum.insert(sitesum.end(), rhs.sites_[opn1].begin(),
                         rhs.sites_[opn1].end());
          sites.push_back(sitesum);
        }
      }
    }
    auto constant = lhs.constant_ * rhs.constant_;
    auto opret = LocalOperator(lhs.GetHilbert(), mat, sites, constant);

    if (std::abs(lhs.constant_) > mel_cutoff_) {
      opret += lhs.constant_ * rhs;
    }
    if (std::abs(rhs.constant_) > mel_cutoff_) {
      opret += rhs.constant_ * lhs;
    }

    return opret;
  }

  friend LocalOperator operator+(const LocalOperator &lhs,
                                 const LocalOperator &rhs) {
    assert(rhs.hilbert_.LocalStates().size() ==
           lhs.hilbert_.LocalStates().size());

    auto sites = lhs.sites_;
    auto mat = lhs.mat_;

    sites.insert(sites.end(), rhs.sites_.begin(), rhs.sites_.end());
    mat.insert(mat.end(), rhs.mat_.begin(), rhs.mat_.end());

    return LocalOperator(lhs.GetHilbert(), mat, sites,
                         lhs.constant_ + rhs.constant_);
  }

  friend LocalOperator operator+(const LocalOperator &lhs, double constant) {
    auto sites = lhs.sites_;
    auto mat = lhs.mat_;

    return LocalOperator(lhs.GetHilbert(), mat, sites,
                         lhs.constant_ + constant);
  }

  LocalOperator &operator+=(const LocalOperator &rhs) {
    assert(rhs.hilbert_.LocalStates().size() ==
           this->hilbert_.LocalStates().size());

    this->sites_.insert(this->sites_.end(), rhs.sites_.begin(),
                        rhs.sites_.end());
    this->mat_.insert(this->mat_.end(), rhs.mat_.begin(), rhs.mat_.end());
    this->constant_ += rhs.constant_;
    this->Init();

    return *this;
  }

  LocalOperator &operator+=(double constant) {
    this->constant_ += constant;
    return *this;
  }

  template <class T>
  friend LocalOperator operator*(T lhs, const LocalOperator &rhs) {
    assert(std::imag(lhs) == 0.);
    auto mat = rhs.mat_;
    auto sites = rhs.sites_;

    for (std::size_t opn = 0; opn < mat.size(); opn++) {
      for (std::size_t i = 0; i < mat[opn].size(); i++) {
        for (std::size_t j = 0; j < mat[opn][i].size(); j++)
          mat[opn][i][j] *= lhs;
      }
    }

    return LocalOperator(rhs.GetHilbert(), mat, sites,
                         std::real(lhs * rhs.constant_));
  }

  const std::vector<MatType> &LocalMatrices() const { return mat_; }
  const std::vector<SiteType> &ActingOn() const { return sites_; }

  const AbstractHilbert &GetHilbert() const noexcept override {
    return hilbert_;
  }

  std::size_t Size() const { return mat_.size(); }
};  // namespace netket

}  // namespace netket
#endif


#include <terra/interpreter.h>

#include <terra/expr_visitor.h>
#include <terra/line_observer.h>
#include <terra/tile.h>
#include <terra/tile_observer.h>

#include <terra/exprs/literals.h>
#include <terra/exprs/var_ref.h>
#include <terra/exprs/vector_combiner.h>

#include <algorithm>
#include <type_traits>
#include <vector>

#include <Eigen/Dense>

namespace terra {

namespace {

template<typename Scalar>
Scalar
Clamp(Scalar x, Scalar min, Scalar max) noexcept
{
  return std::min(std::max(x, min), max);
}

struct BuiltinVars final
{
  float uCenter;
  float vCenter;
};

namespace impl {

template<typename Scalar, size_t Size>
using Vector = Eigen::Matrix<Scalar, Size, 1>;

template<typename Type>
class Expr
{
public:
  virtual ~Expr() = default;

  virtual Type Eval(const BuiltinVars&) const noexcept = 0;
};

template<typename Scalar, size_t Size>
using VectorExpr = Expr<Vector<Scalar, Size>>;

class UCenterExpr final : public Expr<float>
{
public:
  float Eval(const BuiltinVars& builtinVars) const noexcept override
  {
    return builtinVars.uCenter;
  }
};

class VCenterExpr final : public Expr<float>
{
public:
  float Eval(const BuiltinVars& builtinVars) const noexcept override
  {
    return builtinVars.vCenter;
  }
};

template<typename Scalar, size_t Size>
class VectorCombinerExpr final : public VectorExpr<Scalar, Size>
{
public:
  VectorCombinerExpr(std::array<std::unique_ptr<Expr<Scalar>>, Size> elements)
    : mElements(std::move(elements))
  {}

  auto Eval(const BuiltinVars& builtinVars) const noexcept
    -> Vector<Scalar, Size> override
  {
    Vector<Scalar, Size> result;

    for (size_t i = 0; i < Size; i++)
      result(i) = mElements[i]->Eval(builtinVars);

    return result;
  }

private:
  std::array<std::unique_ptr<Expr<Scalar>>, Size> mElements;
};

template<typename Scalar>
class LiteralExpr final : public Expr<Scalar>
{
public:
  LiteralExpr(Scalar value) noexcept
    : mValue(value)
  {}

  Scalar Eval(const BuiltinVars&) const noexcept override { return mValue; }

private:
  Scalar mValue;
};

} // namespace impl

template<typename Type>
class ExprBuilder final : public ExprVisitor
{
public:
  auto TakeExpr() -> std::unique_ptr<impl::Expr<Type>>
  {
    return std::move(mExpr);
  }

  void Visit(const VarRefExpr& varRef) override
  {
    switch (varRef.GetID()) {
      case VarRefExpr::ID::CenterU:
        if constexpr (std::is_same<Type, float>::value)
          mExpr.reset(new impl::UCenterExpr());
        break;
      case VarRefExpr::ID::CenterV:
        if constexpr (std::is_same<Type, float>::value)
          mExpr.reset(new impl::VCenterExpr());
        break;
    }
  }

  void Visit(const IntLiteralExpr& intLiteral) override
  {
    if constexpr (std::is_same<Type, int>::value)
      mExpr.reset(new impl::LiteralExpr<int>(intLiteral.GetValue()));
  }

  void Visit(const FloatLiteralExpr& floatLiteral) override
  {
    if constexpr (std::is_same<Type, float>::value)
      mExpr.reset(new impl::LiteralExpr<float>(floatLiteral.GetValue()));
  }

  void Visit(const FloatToIntExpr&) override {}

  void Visit(const IntToFloatExpr&) override {}

  void Visit(const UnaryExpr&) override {}

  void Visit(const BinaryExpr&) override {}

  void Visit(const VectorCombiner<2>& vecCombiner) override
  {
    if constexpr (std::is_same<Type, impl::Vector<float, 2>>::value)
      HandleVectorCombiner(vecCombiner);
  }

  void Visit(const VectorCombiner<3>& vecCombiner) override
  {
    if constexpr (std::is_same<Type, impl::Vector<float, 3>>::value)
      HandleVectorCombiner(vecCombiner);
  }

  void Visit(const VectorCombiner<4>& vecCombiner) override
  {
    if constexpr (std::is_same<Type, impl::Vector<float, 4>>::value)
      HandleVectorCombiner(vecCombiner);
  }

private:
  template<size_t Size>
  void HandleVectorCombiner(const VectorCombiner<Size>& vecCombiner)
  {
    std::array<std::unique_ptr<impl::Expr<float>>, Size> elements;

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

      ExprBuilder<float> elementBuilder;

      vecCombiner.GetElement(i).Accept(elementBuilder);

      elements[i] = elementBuilder.TakeExpr();
    }

    mExpr.reset(new impl::VectorCombinerExpr<float, Size>(std::move(elements)));
  }

private:
  std::unique_ptr<impl::Expr<Type>> mExpr;
};

struct FrameStatus final
{
  FrameStatus(size_t tilesPerRow, size_t tilesPerCol, size_t w, size_t h)
    : tileCount(tilesPerRow * tilesPerCol)
    , tileIndex(0)
    , resX(w)
    , resY(h)
  {}

  size_t tileCount = 0;

  size_t tileIndex = 0;

  size_t resX = 0;

  size_t resY = 0;

  size_t GetNextXOffset() const noexcept
  {
    return (tileIndex * TileSize()) % resX;
  }

  size_t GetNextYOffset() const noexcept
  {
    return ((tileIndex * TileSize()) / resX) * TileSize();
  }

  size_t GetNextWidth() const noexcept
  {
    auto offset = GetNextXOffset();

    auto end = offset + TileSize();

    return std::min(end, resX) - offset;
  }

  size_t GetNextHeight() const noexcept
  {
    auto offset = GetNextYOffset();

    auto end = offset + TileSize();

    return std::min(end, resY) - offset;
  }
};

class RenderTask final
{
public:
  RenderTask(Tile& tile, const impl::Expr<float>& heightExpr)
    : mTile(tile)
    , mHeightExpr(heightExpr)
  {}

  void operator()() noexcept
  {
    auto& buffer = mTile.GetBuffer();

    for (size_t i = 0; i < (TileSize() * TileSize()); i++) {

      size_t x = i % TileSize();
      size_t y = i / TileSize();

      BuiltinVars builtinVars;
      builtinVars.uCenter = (x + 0.5f) / TileSize();
      builtinVars.vCenter = (y + 0.5f) / TileSize();

      auto h = mHeightExpr.Eval(builtinVars);

      buffer[(i * 4) + 0] = h;
      buffer[(i * 4) + 1] = 0;
      buffer[(i * 4) + 2] = 0;
      buffer[(i * 4) + 3] = 0;
    }
  }

private:
  Tile& mTile;

  const impl::Expr<float>& mHeightExpr;
};

class TileInterpreterImpl final : public TileInterpreter
{
public:
  void AddTileObserver(std::shared_ptr<TileObserver> observer) override
  {
    mTileObservers.emplace_back(std::move(observer));
  }

  bool FrameIsDone() const noexcept override { return TilesRemaining() == 0; }

  size_t TilesRemaining() const noexcept override
  {
    if (!mFrameStatus)
      return 0;

    return mFrameStatus->tileCount - mFrameStatus->tileIndex;
  }

  bool BeginFrame() override
  {
    if (mFrameStatus)
      return false;

    size_t tilesPerRow = (mResX + (TileSize() - 1)) / TileSize();

    size_t tilesPerCol = (mResY + (TileSize() - 1)) / TileSize();

    mFrameStatus.reset(new FrameStatus(tilesPerRow, tilesPerCol, mResX, mResY));

    return true;
  }

  bool PollTiles(size_t) override
  {
    if (!mFrameStatus || !mHeightExpr)
      return false;

    auto x = mFrameStatus->GetNextXOffset();
    auto y = mFrameStatus->GetNextYOffset();
    auto w = mFrameStatus->GetNextWidth();
    auto h = mFrameStatus->GetNextHeight();

    Tile tile(x, y, w, h);

    RenderTask renderTask(tile, *mHeightExpr);

    renderTask();

    NotifyTileObservers(tile);

    mFrameStatus->tileIndex++;

    return true;
  }

  bool EndFrame() override
  {
    if (!mFrameStatus)
      return false;

    mFrameStatus.reset();

    return true;
  }

  bool SetHeightExpr(const terra::Expr& heightExpr) override
  {
    ExprBuilder<float> exprBuilder;

    heightExpr.Accept(exprBuilder);

    mHeightExpr = exprBuilder.TakeExpr();

    return !!mHeightExpr;
  }

  void SetResolution(size_t w, size_t h) override
  {
    mResX = w;
    mResY = h;
  }

private:
  void NotifyTileObservers(const Tile& tile)
  {
    for (auto& tileObserver : mTileObservers)
      tileObserver->Observe(tile);
  }

private:
  size_t mResX = 0;

  size_t mResY = 0;

  std::vector<std::shared_ptr<TileObserver>> mTileObservers;

  std::unique_ptr<FrameStatus> mFrameStatus;

  std::unique_ptr<impl::Expr<float>> mHeightExpr;
};

} // namespace

auto
TileInterpreter::Make() -> std::unique_ptr<TileInterpreter>
{
  return std::unique_ptr<TileInterpreter>(new TileInterpreterImpl());
}

namespace {

class LineInterpreterImpl final : public LineInterpreter
{
public:
  LineInterpreterImpl(size_t w, size_t h, LineObserver& lineObserver)
    : mWidth(w)
    , mHeight(h)
    , mLineObserver(lineObserver)
  {}

  bool SetHeightExpr(const Expr& heightExpr) override
  {
    ExprBuilder<float> exprBuilder;

    heightExpr.Accept(exprBuilder);

    mHeightExpr = exprBuilder.TakeExpr();

    return true;
  }

  bool SetColorExpr(const Expr& expr) override
  {
    ExprBuilder<impl::Vector<float, 3>> exprBuilder;

    expr.Accept(exprBuilder);

    mColorExpr = exprBuilder.TakeExpr();

    return true;
  }

  bool Execute() override
  {
    if (!mHeightExpr || !mColorExpr)
      return false;

    std::vector<float> heightAndRgbBuffer(mWidth * 4);

    for (size_t y = 0; y < mHeight; y++) {

      for (size_t x = 0; x < mWidth; x++) {

        BuiltinVars builtinVars;
        builtinVars.uCenter = (x + 0.5f) / mWidth;
        builtinVars.vCenter = (y + 0.5f) / mHeight;

        auto h = mHeightExpr->Eval(builtinVars);

        auto c = mColorExpr->Eval(builtinVars);

        heightAndRgbBuffer[(x * 4) + 0] = h;
        heightAndRgbBuffer[(x * 4) + 1] = Clamp(c(0) * 255.0f, 0.0f, 255.0f);
        heightAndRgbBuffer[(x * 4) + 2] = Clamp(c(1) * 255.0f, 0.0f, 255.0f);
        heightAndRgbBuffer[(x * 4) + 3] = Clamp(c(2) * 255.0f, 0.0f, 255.0f);
      }

      mLineObserver.Observe(heightAndRgbBuffer.data());
    }

    return true;
  }

private:
  size_t mWidth;

  size_t mHeight;

  LineObserver& mLineObserver;

  std::unique_ptr<impl::Expr<float>> mHeightExpr;

  std::unique_ptr<impl::Expr<impl::Vector<float, 3>>> mColorExpr;
};

} // namespace

auto
LineInterpreter::Make(size_t w, size_t h, LineObserver& lineObserver)
  -> std::unique_ptr<LineInterpreter>
{
  using Ret = std::unique_ptr<LineInterpreter>;

  return Ret(new LineInterpreterImpl(w, h, lineObserver));
}

} // namespace terra


#pragma warning(disable: 4819)

#include <thread>
#include <chrono>
#include <algorithm>
#include <chrono>

#include <boost/filesystem.hpp>

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "setting.hpp"
#include "visible_check.h"

// sort pointcloud based on the distance from the camera
void cloud_sort_by_distance_from_camera(
	const Vec3& cam_pos,
	const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
	const auto sqrd_dist_from_cam = [&](const pcl::PointXYZ& p) {
		return (cam_pos[0] - p.x) * (cam_pos[0] - p.x) + (cam_pos[1] - p.y) * (cam_pos[1] - p.y) + (cam_pos[2] - p.z) * (cam_pos[2] - p.z);
	};

	std::sort(cloud->begin(), cloud->end(),
		[&](const pcl::PointXYZ& lhs, const pcl::PointXYZ& rhs) {
		return sqrd_dist_from_cam(lhs) < sqrd_dist_from_cam(rhs);
	});
}

// pcl::PointCloud<pcl::PointXYZ>::Ptr -> std::vector<float>
std::vector<float> cloud2vector(const pcl::PointCloud<pcl::PointXYZ>::Ptr& c)
{
	std::vector<float> v(c->size() * 3);
	for (int i = 0; i < c->size(); ++i)
	{
		v[i * 3 + 0] = (*c)[i].x;
		v[i * 3 + 1] = (*c)[i].y;
		v[i * 3 + 2] = (*c)[i].z;
	}

	return v;
}

// std::vector<float> -> pcl::PointCloud<pcl::PointXYZ>::Ptr
pcl::PointCloud<pcl::PointXYZ>::Ptr vector2cloud(const std::vector<float>& v)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr c(new pcl::PointCloud<pcl::PointXYZ>);
	for (int i = 0; i < v.size() / 3; ++i)
	{
		c->push_back(pcl::PointXYZ(v[i * 3 + 0], v[i * 3 + 1], v[i * 3 + 2]));
	}

	return c;
}

int main(int argc, char** argv)
{
	VisibleSceneMakerSetting setting(argc, argv);
	const float lambda_sqrd = setting.lambda * setting.lambda;

	//
	// load
	//

	const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_load(new pcl::PointCloud<pcl::PointXYZ>);

	if (boost::filesystem::path(setting.load_path).extension() == ".pcd")
	{
		if (pcl::io::loadPCDFile(setting.load_path, *cloud_load) == -1)
		{
			error_exit("Load error.");
		}
	}
	else if (boost::filesystem::path(setting.load_path).extension() == ".ply")
	{
		if (pcl::io::loadPLYFile(setting.load_path, *cloud_load) == -1)
		{
			error_exit("Load error.");
		}
	}
	else
	{
		error_exit("PLY or PCD file are only available to load.");
	}

	std::cout << "Input cloud: " << cloud_load->size() << std::endl;

	//
	// visiblity check
	//

	const auto start = std::chrono::system_clock::now();

	// check form camera1
	const Vec3 cam_pos_1 {
		static_cast<float>(setting.camera_1_x),
		static_cast<float>(setting.camera_1_y),
		static_cast<float>(setting.camera_1_z)
	};
	cloud_sort_by_distance_from_camera(cam_pos_1, cloud_load);
	auto cloud_visible1_v = visible_check(cam_pos_1, cloud2vector(cloud_load), lambda_sqrd);
	auto cloud_visible1 = vector2cloud(cloud_visible1_v);

	// check form camera2
	const Vec3 cam_pos_2 {
		static_cast<float>(setting.camera_2_x),
		static_cast<float>(setting.camera_2_y),
		static_cast<float>(setting.camera_2_z)
	};
	cloud_sort_by_distance_from_camera(cam_pos_2, cloud_visible1);
	auto cloud_save_v = visible_check(cam_pos_1, cloud2vector(cloud_visible1), lambda_sqrd);
	auto cloud_save = vector2cloud(cloud_save_v);

	const auto end = std::chrono::system_clock::now();
	std::cout << "time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << " [msec]" << std::endl;

	//
	// save
	//

	if (cloud_save->size() > 0)
	{
		if (boost::filesystem::path(setting.save_path).extension() == ".pcd")
		{
			pcl::io::savePCDFileBinary(setting.save_path, *cloud_save);
		}
		else if (boost::filesystem::path(setting.save_path).extension() == ".ply")
		{
			pcl::io::savePLYFileBinary(setting.save_path, *cloud_save);
		}
		else
		{
			error_exit("PLY or PCD file are only available to save.");
		}
	}
	else
	{
		error_exit("No point is exist in visible scene. Please chenge lambda and/or camera positions.");
	}

	//
	// visualization
	//

	if (setting.visualization)
	{
		pcl::visualization::PCLVisualizer::Ptr viewer_input(new pcl::visualization::PCLVisualizer("Input"));
		if (cloud_load->size() > 0)
		{
			viewer_input->addPointCloud(cloud_load);
		}

		pcl::visualization::PCLVisualizer::Ptr viewer_output(new pcl::visualization::PCLVisualizer("Output"));
		if (cloud_save->size() > 0)
		{
			viewer_output->addPointCloud(cloud_save);
		}

		viewer_input->spinOnce();
		viewer_output->spinOnce();

		while (!viewer_input->wasStopped() && !viewer_output->wasStopped())
		{
			viewer_input->spinOnce();
			viewer_output->spinOnce();
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
		}
	}
	return 0;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//
#include "InferenceTestImage.hpp"

#include <boost/core/ignore_unused.hpp>
#include <boost/format.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <array>

#define STB_IMAGE_IMPLEMENTATION
#include <stb/stb_image.h>

#define STB_IMAGE_RESIZE_IMPLEMENTATION
#include <stb/stb_image_resize.h>

#define STB_IMAGE_WRITE_IMPLEMENTATION
#include <stb/stb_image_write.h>

namespace
{

unsigned int GetImageChannelIndex(ImageChannelLayout channelLayout, ImageChannel channel)
{
    switch (channelLayout)
    {
    case ImageChannelLayout::Rgb:
        return static_cast<unsigned int>(channel);
    case ImageChannelLayout::Bgr:
        return 2u - static_cast<unsigned int>(channel);
    default:
        throw UnknownImageChannelLayout(boost::str(boost::format("Unknown layout %1%")
            % static_cast<int>(channelLayout)));
    }
}

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

inline void PutData(std::vector<float> & data,
                    const unsigned int width,
                    const unsigned int x,
                    const unsigned int y,
                    const unsigned int c,
                    float value)
{
    data[(3*((y*width)+x)) + c] = value;
}

std::vector<float> ResizeBilinearAndNormalize(const InferenceTestImage & image,
                                              const unsigned int outputWidth,
                                              const unsigned int outputHeight,
                                              const float scale,
                                              const std::array<float, 3>& mean,
                                              const std::array<float, 3>& stddev)
{
    std::vector<float> out;
    out.resize(outputWidth * outputHeight * 3);

    // We follow the definition of TensorFlow and AndroidNN: the top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int inputWidth = image.GetWidth();
    const unsigned int inputHeight = image.GetHeight();

    // How much to scale pixel coordinates in the output image to get the corresponding pixel coordinates
    // in the input image.
    const float scaleY = boost::numeric_cast<float>(inputHeight) / boost::numeric_cast<float>(outputHeight);
    const float scaleX = boost::numeric_cast<float>(inputWidth) / boost::numeric_cast<float>(outputWidth);

    uint8_t rgb_x0y0[3];
    uint8_t rgb_x1y0[3];
    uint8_t rgb_x0y1[3];
    uint8_t rgb_x1y1[3];

    for (unsigned int y = 0; y < outputHeight; ++y)
    {
        // Corresponding real-valued height coordinate in input image.
        const float iy = boost::numeric_cast<float>(y) * scaleY;

        // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation).
        const float fiy = floorf(iy);
        const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

        // Interpolation weight (range [0,1])
        const float yw = iy - fiy;

        for (unsigned int x = 0; x < outputWidth; ++x)
        {
            // Real-valued and discrete width coordinates in input image.
            const float ix = boost::numeric_cast<float>(x) * scaleX;
            const float fix = floorf(ix);
            const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

            // Interpolation weight (range [0,1]).
            const float xw = ix - fix;

            // Discrete width/height coordinates of texels below and to the right of (x0, y0).
            const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
            const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

            std::tie(rgb_x0y0[0], rgb_x0y0[1], rgb_x0y0[2]) = image.GetPixelAs3Channels(x0, y0);
            std::tie(rgb_x1y0[0], rgb_x1y0[1], rgb_x1y0[2]) = image.GetPixelAs3Channels(x1, y0);
            std::tie(rgb_x0y1[0], rgb_x0y1[1], rgb_x0y1[2]) = image.GetPixelAs3Channels(x0, y1);
            std::tie(rgb_x1y1[0], rgb_x1y1[1], rgb_x1y1[2]) = image.GetPixelAs3Channels(x1, y1);

            for (unsigned c=0; c<3; ++c)
            {
                const float ly0 = Lerp(float(rgb_x0y0[c]), float(rgb_x1y0[c]), xw);
                const float ly1 = Lerp(float(rgb_x0y1[c]), float(rgb_x1y1[c]), xw);
                const float l = Lerp(ly0, ly1, yw);
                PutData(out, outputWidth, x, y, c, ((l / scale) - mean[c]) / stddev[c]);
            }
        }
    }
    return out;
}

} // namespace

InferenceTestImage::InferenceTestImage(char const* filePath)
 : m_Width(0u)
 , m_Height(0u)
 , m_NumChannels(0u)
{
    int width;
    int height;
    int channels;

    using StbImageDataPtr = std::unique_ptr<unsigned char, decltype(&stbi_image_free)>;
    StbImageDataPtr stbData(stbi_load(filePath, &width, &height, &channels, 0), &stbi_image_free);

    if (stbData == nullptr)
    {
        throw InferenceTestImageLoadFailed(boost::str(boost::format("Could not load the image at %1%") % filePath));
    }

    if (width == 0 || height == 0)
    {
        throw InferenceTestImageLoadFailed(boost::str(boost::format("Could not load empty image at %1%") % filePath));
    }

    m_Width = boost::numeric_cast<unsigned int>(width);
    m_Height = boost::numeric_cast<unsigned int>(height);
    m_NumChannels = boost::numeric_cast<unsigned int>(channels);

    const unsigned int sizeInBytes = GetSizeInBytes();
    m_Data.resize(sizeInBytes);
    memcpy(m_Data.data(), stbData.get(), sizeInBytes);
}

std::tuple<uint8_t, uint8_t, uint8_t> InferenceTestImage::GetPixelAs3Channels(unsigned int x, unsigned int y) const
{
    if (x >= m_Width || y >= m_Height)
    {
        throw InferenceTestImageOutOfBoundsAccess(boost::str(boost::format("Attempted out of bounds image access. "
            "Requested (%1%, %2%). Maximum valid coordinates (%3%, %4%).") % x % y % (m_Width - 1) % (m_Height - 1)));
    }

    const unsigned int pixelOffset = x * GetNumChannels() + y * GetWidth() * GetNumChannels();
    const uint8_t* const pixelData = m_Data.data() + pixelOffset;
    BOOST_ASSERT(pixelData <= (m_Data.data() + GetSizeInBytes()));

    std::array<uint8_t, 3> outPixelData;
    outPixelData.fill(0);

    const unsigned int maxChannelsInPixel = std::min(GetNumChannels(), static_cast<unsigned int>(outPixelData.size()));
    for (unsigned int c = 0; c < maxChannelsInPixel; ++c)
    {
        outPixelData[c] = pixelData[c];
    }

    return std::make_tuple(outPixelData[0], outPixelData[1], outPixelData[2]);
}

void InferenceTestImage::StbResize(InferenceTestImage& im, const unsigned int newWidth, const unsigned int newHeight)
{
    std::vector<uint8_t> newData;
    newData.resize(newWidth * newHeight * im.GetNumChannels() * im.GetSingleElementSizeInBytes());

    // boost::numeric_cast<>() is used for user-provided data (protecting about overflows).
    // static_cast<> is ok for internal data (assumes that, when internal data was originally provided by a user,
    // a boost::numeric_cast<>() handled the conversion).
    const int nW = boost::numeric_cast<int>(newWidth);
    const int nH = boost::numeric_cast<int>(newHeight);

    const int w = static_cast<int>(im.GetWidth());
    const int h = static_cast<int>(im.GetHeight());
    const int numChannels = static_cast<int>(im.GetNumChannels());

    const int res = stbir_resize_uint8(im.m_Data.data(), w, h, 0, newData.data(), nW, nH, 0, numChannels);
    if (res == 0)
    {
        throw InferenceTestImageResizeFailed("The resizing operation failed");
    }

    im.m_Data.swap(newData);
    im.m_Width = newWidth;
    im.m_Height = newHeight;
}

std::vector<float> InferenceTestImage::Resize(unsigned int newWidth,
                                              unsigned int newHeight,
                                              const armnn::CheckLocation& location,
                                              const ResizingMethods meth,
                                              const std::array<float, 3>& mean,
                                              const std::array<float, 3>& stddev,
                                              const float scale)
{
    std::vector<float> out;
    if (newWidth == 0 || newHeight == 0)
    {
        throw InferenceTestImageResizeFailed(boost::str(boost::format("None of the dimensions passed to a resize "
            "operation can be zero. Requested width: %1%. Requested height: %2%.") % newWidth % newHeight));
    }

    switch (meth) {
        case ResizingMethods::STB:
        {
            StbResize(*this, newWidth, newHeight);
            break;
        }
        case ResizingMethods::BilinearAndNormalized:
        {
            out = ResizeBilinearAndNormalize(*this, newWidth, newHeight, scale, mean, stddev);
            break;
        }
        default:
            throw InferenceTestImageResizeFailed(boost::str(
                boost::format("Unknown resizing method asked ArmNN only supports {STB, BilinearAndNormalized} %1%")
                              % location.AsString()));
    }
    return out;
}

void InferenceTestImage::Write(WriteFormat format, const char* filePath) const
{
    const int w = static_cast<int>(GetWidth());
    const int h = static_cast<int>(GetHeight());
    const int numChannels = static_cast<int>(GetNumChannels());
    int res = 0;

    switch (format)
    {
    case WriteFormat::Png:
        {
            res = stbi_write_png(filePath, w, h, numChannels, m_Data.data(), 0);
            break;
        }
    case WriteFormat::Bmp:
        {
            res = stbi_write_bmp(filePath, w, h, numChannels, m_Data.data());
            break;
        }
    case WriteFormat::Tga:
        {
            res = stbi_write_tga(filePath, w, h, numChannels, m_Data.data());
            break;
        }
    default:
        throw InferenceTestImageWriteFailed(boost::str(boost::format("Unknown format %1%")
            % static_cast<int>(format)));
    }

    if (res == 0)
    {
        throw InferenceTestImageWriteFailed(boost::str(boost::format("An error occurred when writing to file %1%")
            % filePath));
    }
}

template <typename TProcessValueCallable>
std::vector<float> GetImageDataInArmNnLayoutAsFloats(ImageChannelLayout channelLayout,
    const InferenceTestImage& image,
    TProcessValueCallable processValue)
{
    const unsigned int h = image.GetHeight();
    const unsigned int w = image.GetWidth();

    std::vector<float> imageData;
    imageData.resize(h * w * 3);

    for (unsigned int j = 0; j < h; ++j)
    {
        for (unsigned int i = 0; i < w; ++i)
        {
            uint8_t r, g, b;
            std::tie(r, g, b) = image.GetPixelAs3Channels(i, j);

            // ArmNN order: C, H, W
            const unsigned int rDstIndex = GetImageChannelIndex(channelLayout, ImageChannel::R) * h * w + j * w + i;
            const unsigned int gDstIndex = GetImageChannelIndex(channelLayout, ImageChannel::G) * h * w + j * w + i;
            const unsigned int bDstIndex = GetImageChannelIndex(channelLayout, ImageChannel::B) * h * w + j * w + i;

            imageData[rDstIndex] = processValue(ImageChannel::R, float(r));
            imageData[gDstIndex] = processValue(ImageChannel::G, float(g));
            imageData[bDstIndex] = processValue(ImageChannel::B, float(b));
        }
    }

    return imageData;
}

std::vector<float> GetImageDataInArmNnLayoutAsNormalizedFloats(ImageChannelLayout layout,
    const InferenceTestImage& image)
{
    return GetImageDataInArmNnLayoutAsFloats(layout, image,
        [](ImageChannel channel, float value)
        {
            boost::ignore_unused(channel);
            return value / 255.f;
        });
}

std::vector<float> GetImageDataInArmNnLayoutAsFloatsSubtractingMean(ImageChannelLayout layout,
    const InferenceTestImage& image,
    const std::array<float, 3>& mean)
{
    return GetImageDataInArmNnLayoutAsFloats(layout, image,
        [layout, &mean](ImageChannel channel, float value)
        {
            const unsigned int channelIndex = GetImageChannelIndex(layout, channel);
            return value - mean[channelIndex];
        });
}

std::vector<float> GetImageDataAsNormalizedFloats(ImageChannelLayout layout,
                                                  const InferenceTestImage& image)
{
    std::vector<float> imageData;
    const unsigned int h = image.GetHeight();
    const unsigned int w = image.GetWidth();

    const unsigned int rDstIndex = GetImageChannelIndex(layout, ImageChannel::R);
    const unsigned int gDstIndex = GetImageChannelIndex(layout, ImageChannel::G);
    const unsigned int bDstIndex = GetImageChannelIndex(layout, ImageChannel::B);

    imageData.resize(h * w * 3);
    unsigned int offset = 0;

    for (unsigned int j = 0; j < h; ++j)
    {
        for (unsigned int i = 0; i < w; ++i)
        {
            uint8_t r, g, b;
            std::tie(r, g, b) = image.GetPixelAs3Channels(i, j);

            imageData[offset+rDstIndex] = float(r) / 255.0f;
            imageData[offset+gDstIndex] = float(g) / 255.0f;
            imageData[offset+bDstIndex] = float(b) / 255.0f;
            offset += 3;
        }
    }

    return imageData;
}


#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/ogre_helpers/line.h>
#include "ndt_rviz_visualisation/ndt_line_visual.hpp"

namespace perception_oru{
  NDTLineVisual::NDTLineVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node ){
    scene_manager_ = scene_manager;
    frame_node_ = parent_node->createChildSceneNode();
    NDT_l1_.reset(new rviz::Line(scene_manager_,frame_node_ ));
    NDT_l2_.reset(new rviz::Line(scene_manager_,frame_node_ ));
    NDT_l3_.reset(new rviz::Line(scene_manager_,frame_node_ ));
  }

  NDTLineVisual::~NDTLineVisual()
  {
    scene_manager_->destroySceneNode( frame_node_ );
  }

  void NDTLineVisual::setCell(ndt_map::NDTCellMsg cell, double resolution){
    Ogre::Vector3 position(cell.mean_x,cell.mean_y,cell.mean_z);
    //eigen values aka size of the elipsoid
    Eigen::Matrix3d cov;
    int m_itr=0;
    for(int i=0;i<3;i++){
      for(int j=0;j<3;j++){
        cov(i,j)=cell.cov_matrix[m_itr];
        m_itr++;
      }
    }
    Eigen::Matrix3d m_eigVec = Eigen::Matrix3d::Zero(3,3);
    Eigen::Matrix3d m_eigVal = Eigen::Matrix3d::Zero(3,3);
    Eigen::EigenSolver<Eigen::Matrix3d> es(cov);
    m_eigVal = es.pseudoEigenvalueMatrix();
    m_eigVec = es.pseudoEigenvectors();
    m_eigVal = m_eigVal.cwiseSqrt();

    Eigen::Quaternion<double> q(m_eigVec);
    Ogre::Vector3 scale(m_eigVal(0,0),m_eigVal(1,1),m_eigVal(2,2));
    Ogre::Quaternion orient(q.w(),q.x(),q.y(),q.z());

    // Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> Sol (cov);
    // Eigen::Matrix3d evecs;
    // Eigen::Vector3d evals;
    // evecs = Sol.eigenvectors().real();
    // evals = Sol.eigenvalues().real();
    // Eigen::Quaternion<double> q(evecs);
    // Ogre::Vector3 scale(30*evals(0),30*evals(1),30*evals(2));
    // Ogre::Quaternion orient(q.w(),q.x(),q.y(),q.z());

    NDT_l1_->setPosition(position);
    NDT_l1_->setOrientation(orient);
    NDT_l1_->setPoints(Ogre::Vector3(-scale[0],0,0), Ogre::Vector3(scale[0],0, 0));

    NDT_l2_->setPosition(position);
    NDT_l2_->setOrientation(orient);
    NDT_l2_->setPoints(Ogre::Vector3(0,-scale[1],0), Ogre::Vector3(0,scale[1],0));

    NDT_l3_->setPosition(position);
    NDT_l3_->setOrientation(orient);
    NDT_l3_->setPoints(Ogre::Vector3(0,0,-scale[2]), Ogre::Vector3(0,0,scale[2]));

    // NDT_elipsoid_->setScale(scale);
    // NDT_elipsoid_->setPosition(position);
    // NDT_elipsoid_->setOrientation(orient);

  }

  void NDTLineVisual::setFramePosition( const Ogre::Vector3& position ){
    frame_node_->setPosition( position );
  }

  void NDTLineVisual::setFrameOrientation( const Ogre::Quaternion& orientation ){
    frame_node_->setOrientation( orientation );
  }

  void NDTLineVisual::setColor( float r, float g, float b, float a ){
    NDT_l1_->setColor( r, g, b, a );
    NDT_l2_->setColor( r, g, b, a );
    NDT_l3_->setColor( r, g, b, a );
  }
}


#include "biggles/observation.hpp"
#include "biggles/detail/random.hpp"
#include "biggles/mh_moves/mh_moves.hpp"
#include "biggles/mh_moves/utility.hpp"
#include "biggles/track.hpp"
#include "biggles/tracker.hpp"
#include "biggles/kalman_filter.hpp"
#include "biggles/simulate.hpp"
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <string>

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

typedef boost::shared_ptr< const kalman_filter > shared_const_kalman_filter_ptr;

std::ostream& operator<<(std::ostream& os, const kalman_filter::state_vector& state) {
    os << boost::format("[% .4f, % .4f, % .4f, % .4f]") % state[0] % state[1] % state[2] % state[3];
    return os;
}

typedef std::deque<float>::const_iterator data_iter;
std::pair<float, float> mean_stdev(data_iter b, data_iter e) {
    float sum = std::accumulate(b, e, 0.0f);
    float mean = sum / std::distance(b, e);

    float sq_sum = std::inner_product(b, e, b, 0.0f);
    float stdev = std::sqrt(sq_sum / std::distance(b, e) - mean * mean);
    return std::make_pair(mean, stdev);
}

float max_deviation_from_line( const observation_collection &oc) {
    time_stamp start = oc.first_time_stamp();
    time_stamp end = oc.last_time_stamp();
    float x_off = x(*oc.begin());
    float y_off = y(*oc.begin());
    float delta_x = (x(oc.back()) - x_off)/float(end-start-1);
    float delta_y = (y(oc.back()) - y_off)/float(end-start-1);
    float result = 0.f;
    for (time_stamp ts = start; ts != end; ++ts) {
        observation_collection::const_range obs_at_t = oc.at_time_stamp(ts);
        float line_x = x_off + (ts - start) * delta_x;
        float line_y = y_off + (ts - start) * delta_y;
        for (; obs_at_t.first != obs_at_t.second; ++(obs_at_t.first)) {
            float diff = hypotf(line_x - x(*obs_at_t.first), line_y - y(*obs_at_t.first));
            if (diff > result) result = diff;
        }
    }
    return result;
}

shared_const_track_ptr generate_rotating_track(int duration) {
    const float delta_angle = 2.0f*3.1415f/float(duration);
    observation_collection oc;
    for (time_stamp t = 0; t < duration; ++t) {
        float angle = t*delta_angle;
        oc.insert(new_obs(std::cos(angle), std::sin(angle), t));
    }
    return shared_const_track_ptr(new track(0, duration, oc.begin(), oc.end(), 1.0f));
}

shared_const_track_ptr generate_linear_track(int duration, float dx, float dy) {
    observation_collection oc;
    for (time_stamp t = 0; t < duration; ++t) oc.insert(new_obs( t*dx, t*dy, t));
    return shared_const_track_ptr(new track(0, duration, oc.begin(), oc.end(), 1.0f));
}

shared_const_track_ptr generate_linear_track(
    time_stamp begin_ts, time_stamp end_ts, float x0, float y0, float dx, float dy, float loc_err)
{
    observation_collection oc;
    for (time_stamp ts = begin_ts; ts < end_ts; ++ts) oc.insert(new_obs(
        x0 + (ts - begin_ts)*dx + sampling::sample_normal() * loc_err,
        y0 + (ts - begin_ts)*dy + sampling::sample_normal() * loc_err,
        ts));
    return shared_const_track_ptr(new track(begin_ts, end_ts, oc.begin(), oc.end(), 1.0f));
}

boost::shared_ptr<const observation_collection> generate_observations(
    time_stamp begin_ts, time_stamp end_ts, float x0, float y0, float stdev, float lambda)
{
    boost::shared_ptr<observation_collection> oc_ptr(new observation_collection);
    for (time_stamp ts = begin_ts; ts < end_ts; ++ts)
    {
        int num_obs = sampling::poisson(lambda);
        for (int i = 0; i < num_obs; ++i) {
            oc_ptr->insert(
                new_obs(x0 + sampling::sample_normal() * stdev, y0 + sampling::sample_normal() * stdev, ts));
        }
    }
    return oc_ptr;
}

void print_kalman_filter_states(const kalman_filter::states_and_cov_deque& sc) {
    kalman_filter::states_and_cov_deque::const_iterator sc_iter;
    int i = 0;
    for (sc_iter = sc.begin(); sc_iter != sc.end(); ++sc_iter)
        std::cout
            << boost::format("%3d: ") % i++
            << sc_iter->first
            << ", "
            << boost::format("v = % .4f") % hypotf(sc_iter->first[1], sc_iter->first[3])
            << ", "
            << boost::format("log(||C||) = % .4f") % logf(sc_iter->second.determinant())
            << std::endl;
}

std::string print_parameters(const model::parameters& params) {
    std::stringstream para_ss;
    para_ss << boost::format("b = %.2f, c = %.2f, o = %.2f%%, s = %.2f%%, cr = %.2f")
            % model::mean_new_tracks_per_frame(params)
            % model::mean_false_observations_per_frame(params)
            % (model::generate_observation_probability(params) * 100.f)
            % (model::frame_to_frame_survival_probability(params) * 100.f)
            % model::constraint_radius(params)
            << ", "
            << boost::format("R = ((%.2f, %.2f), (%.2f, %.2f))")
            % model::observation_error_covariance(params)(0, 0)
            % model::observation_error_covariance(params)(0, 1)
            % model::observation_error_covariance(params)(1, 0)
            % model::observation_error_covariance(params)(1, 1)
    ;
    return para_ss.str();
}

float find_good_parameters(partition_ptr_t& const_partition, model::parameters& good_parameters, const int nloops) {
    float best_pdf = -std::numeric_limits<int>::max();
    model::parameters target_parameters;
    model::process_noise_covariance(target_parameters) = detail::initQ();
    for (int i = 0; i < nloops; ++i) {
        sample_model_parameters_given_partition(*const_partition, target_parameters);
        float log_pdf = model::log_partition_given_parameters_and_data_density(*const_partition, target_parameters);
        if (log_pdf > best_pdf) {
            best_pdf = log_pdf;
            good_parameters = target_parameters;
        }
    }
    return best_pdf;
}

bool generate_parameters(model::parameters& params) {
    model::mean_new_tracks_per_frame(params)           = 1.2f;
    model::mean_false_observations_per_frame(params)   = 0.001f;
    model::frame_to_frame_survival_probability(params) = 0.9;
    model::generate_observation_probability(params)    = 0.9f;
    float sigma_R = 0.3f;
    model::observation_error_covariance(params)       << sigma_R * sigma_R, 0, 0, sigma_R * sigma_R;
    model::process_noise_covariance(params) = detail::initQ();
    model::constraint_radius(params) = sampling::uniform_real(0, 10);
    return true;
}

std::string partition_properties(partition_ptr_t &const_partition) {
    const track_collection &tracks(const_partition->tracks());
    size_t num_tracks = tracks.size();
    track_collection::const_iterator tr_iter;
    float avg_track_duration = 0.0f;
    float avg_track_size = 0.0f;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        avg_track_duration += float((*tr_iter)->duration());
        avg_track_size += float((*tr_iter)->size());
    }
    avg_track_duration /= float(num_tracks);
    avg_track_size /= float(num_tracks);
    float o_obs = avg_track_size/avg_track_duration;
    float b_obs = float(num_tracks)/float(const_partition->duration());
    float c_obs = float(const_partition->clutter().size())/float(const_partition->duration());
    float s_obs = 1.f - 1.f/avg_track_duration;
    std::stringstream pp_ss;
    pp_ss << boost::format("num.tracks = %d, avg.length = %.1f, avg.obs.count = %.1f, "
        "observed: b = %.2f, c = %.2f, o = %.2f, s = %.2f")
        % num_tracks % avg_track_duration % avg_track_size % b_obs % c_obs % o_obs % s_obs
    ;
    return pp_ss.str();
}

int plot_tracks(const track_collection &tracks, const model::parameters &const_parameters) {
    track_collection::const_iterator tr_iter;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        const track & track0(**tr_iter);
        shared_const_kalman_filter_ptr kf = track0.make_kalman_filter(const_parameters);
        float log_posterior = track0.log_posterior(const_parameters);
        const kalman_filter::states_and_cov_deque & sncs(kf->corrections());
        const observation_collection &oc(track0.observations());
        std::deque <float> diffs;
        time_stamp ts=track0.first_time_stamp();
        std::stringstream diff_ss;
        std::stringstream cov_ss;
        std::stringstream track_ss;
        diff_ss <<  "| ";
        track_ss << "| ";
        track_ss << std::string(ts, ' ');
        diff_ss << std::string(ts, ' ');
        cov_ss   << "log(cov) = ";
        for (size_t i=0; i < sncs.size(); ++i, ++ts) {
            kalman_filter::state_vector state = sncs[i].first;
            kalman_filter::covariance_matrix cov = sncs[i].second;
            observation_collection::const_range obs_at_t(oc.at_time_stamp(ts));
            cov_ss << boost::format(" %.1f") % log10f(cov.determinant());
            if (obs_at_t.first != obs_at_t.second) {
                observation obs = *obs_at_t.first;
                float diff = hypotf(x(obs)-state[0], y(obs)-state[2]);
                diff_ss << boost::format("%d") % int(diff);
                diffs.push_back(diff);
                track_ss << "O";
            }
            else {
               diff_ss << "_";
               track_ss << "_";
            }
        }
        diag("log(PDF) = %8.2f %s", log_posterior, track_ss.str().c_str());
        diag("                    %s", diff_ss.str().c_str());
        diag("%s", cov_ss.str().c_str());
        float max_diff = *std::max_element(diffs.begin(), diffs.end());
        float radius = track0.radius();
        float max_dev = max_deviation_from_line(oc);
        if (max_diff >= max_dev or max_dev >= radius) {
            //diag("max diff = %.2f, dev from line = %.2f, radius = %.2f", max_diff, max_dev, radius);
            //diag("%s", diff_ss.str().c_str());
            //diag("length = %ld, observations = %.2f%%", track0.duration(), 100.f*float(track0.size())/float(track0.duration()));
        }
    }
    return 0;
}

int plot_states_of_tracks(const track_collection &tracks, const model::parameters &const_parameters) {
    track_collection::const_iterator tr_iter;
    for (tr_iter = tracks.begin(); tr_iter != tracks.end(); ++tr_iter) {
        const track& track0(**tr_iter);
        shared_const_kalman_filter_ptr kf = track0.make_kalman_filter(const_parameters);
        diag("track [%ld, %ld]", track0.first_time_stamp(), track0.last_time_stamp());
        print_kalman_filter_states(kf->corrections());
    }
    return 0;
}

struct olink {
    observation to, from;
    olink(const observation &t, const observation &f) : to(t), from(f) {}
    bool operator<(const olink &o) const { return to < o.to or (to == o.to and from < o.from); }
};

int test1() {
    const int total = 1;
    const time_stamp last_ts(80);
    boost::shared_ptr<const observation_collection> clutter_obs(generate_observations(0, last_ts, 0.f, 0.f, 1.5f, 3.f));
    clutter_ptr clutter(new clutter_t(clutter_obs->begin(), clutter_obs->end()));
    boost::shared_ptr<const track_collection> tracks(new track_collection);
    partition_ptr_t original_part_ptr(new partition(tracks, clutter));
    for (int i=0; i< total; ++i) {
        diag(" *** iteration %d *** ", i);
        model::parameters params;
        model::process_noise_covariance(params) = detail::initQ();
        std::map<olink, float> link_count;
        generate_parameters(params);
        const std::string bsp("\b\b\b\b\b\b");
        const int nloops(5000);
        diag("number of loops: = %d", nloops);
        tracker tracking(params, original_part_ptr);
        for(int loop=0; loop<nloops; ++loop) {
            //if (loop % 500 == 0) {
            //    std::clog << bsp << boost::format("%3d%%") % (100*loop/nloops)  << std::flush;
            //}
            tracking();
            BOOST_FOREACH(const shared_const_track_ptr& t_p, tracking.last_partition()->tracks()) {
                const track tr0(*t_p);
                BOOST_ASSERT(tr0.size()>1);
                observation_collection::const_iterator from_it = tr0.observations().begin();
                observation_collection::const_iterator to_it(from_it);
                std::advance(to_it, 1);
                for (; to_it != tr0.observations().end(); ++to_it, ++from_it) link_count[olink(*to_it, *from_it)]++;
            }
        }
        //std::clog << bsp;
        //
        std::map<olink, float>::iterator it = link_count.begin();
        std::map<int, float> dist_count;
        std::map<int, float> from_count;
        std::map<int, float> to_count;
        std::map<int, float> dist_occur;
        std::map<int, float> from_occur;
        std::map<int, float> to_occur;
        for (; it != link_count.end(); ++it) {
              to_count[t(it->first.to)] += it->second;
            dist_count[t(it->first.to) - t(it->first.from)] += it->second;
            from_count[t(it->first.from)] += it->second;
              to_occur[t(it->first.to)] ++;
            dist_occur[t(it->first.to) - t(it->first.from)] ++;
            from_occur[t(it->first.from)] ++;
        }
        std::map<int, float>::iterator fp;
        diag("distance");
        for (int i = 0; i < int(last_ts); ++i) {
            float d_occur = dist_occur[i] == 0.f ? 1.f : float(nloops) * dist_occur[i];
            float f_occur = from_occur[i] == 0.f ? 1.f : float(nloops) * from_occur[i];
            float t_occur =   to_occur[i] == 0.f ? 1.f : float(nloops) *   to_occur[i];
            std::stringstream sstr;
            sstr
                << boost::format("%2d => %5.2f%%, %5.2f%%, %5.2f%%")
                % i
                % (dist_count[i]*100.f/ d_occur)
                % (from_count[i]*100.f/ f_occur)
                % (  to_count[i]*100.f/ t_occur)
                ;
            diag("%s", sstr.str().c_str());
        }
    }
    return 0;
}

int test2() {
    observation_collection t1_obs;
    //observation_collection cl_obs;
    t1_obs.insert(new_obs(0, 0, 0));
    t1_obs.insert(new_obs(0, 0, 1));
    t1_obs.insert(new_obs(0, 0, 2));
    t1_obs.insert(new_obs(0, 0, 3));
    track track1(0, 5, t1_obs.begin(), t1_obs.end(), 1.0);

    /*
    cl_obs.insert(new_obs(0, 0, 4));
    cl_obs.insert(new_obs(1, 1, 5));
    cl_obs.insert(new_obs(1, 1, 6));
    cl_obs.insert(new_obs(1, 1, 7));
    cl_obs.insert(new_obs(1, 1, 8));
    cl_obs.insert(new_obs(1, 1, 9));
    boost::shared_ptr<track_collection> tracks(new track_collection());
    tracks->insert(track1);
    boost::shared_ptr<const observation_collection> clutter(new observation_collection(cl_obs.begin(), cl_obs.end()));
    const partition original_partition(tracks, clutter);
    */

    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 2.f;
    model::mean_false_observations_per_frame(params)   = 1.f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.9f;
    model::constraint_radius(params) = 100.0f;
    model::observation_error_covariance(params) << 0.001f, 0.0f, 0.0f, 0.001f;
    model::process_noise_covariance(params) = detail::initQ();

    print_kalman_filter_states( track1.make_kalman_filter(params)->corrections() );
    diag("forward:");
    print_kalman_filter_states( track1.make_kalman_filter(params)->predictions() );
    diag("backward");
    print_kalman_filter_states( generate_rotating_track(20)->make_kalman_filter(params)->corrections() );
    diag("forward");
    print_kalman_filter_states( generate_rotating_track(20)->make_kalman_filter(params)->predictions() );
    diag("backward linear:");
    print_kalman_filter_states( generate_linear_track(40, 1.f, 0.f)->make_kalman_filter(params)->corrections() );
    diag("forward linear:");
    print_kalman_filter_states( generate_linear_track(40, 1.f, 0.f)->make_kalman_filter(params)->predictions() );
    return 0;
}

int test3() {
    const float loc_err(.3f);

    std::deque<float> sing_pdf;
    for (int i=0; i<1; ++i) {
        boost::shared_ptr<track_collection> tracks(new track_collection());
        tracks->insert(*generate_linear_track(0, 50, 0.f, 0.f, 1.f, 0.f, loc_err));
        clutter_ptr clutter(new clutter_t());
        partition_ptr_t original_part_ptr(new partition(tracks, clutter));
        model::parameters params;
        model::process_noise_covariance(params) = detail::initQ();
        float log_pdf = find_good_parameters(original_part_ptr, params, 1000);
        model::observation_error_covariance(params) << loc_err * loc_err, 0, 0, loc_err * loc_err;
        diag("%s, log(PDF)=%.2f", print_parameters(params).c_str(), log_pdf);
        plot_tracks(*tracks, params);
        plot_states_of_tracks(*tracks, params);
        sing_pdf.push_back(log_pdf);
    }
    std::deque<float> quad_pdf;
    for (int i=0; i<1; ++i) {
        boost::shared_ptr<track_collection> tracks(new track_collection());
        int step = 5;
        for (int j=0; j < 50; j+= step)
            tracks->insert(*generate_linear_track(j, j+step, float(j), 0.f, 1.f, 0.f, loc_err));
        clutter_ptr clutter(new clutter_t());
        partition_ptr_t original_part_ptr(new partition(tracks, clutter));
        model::parameters params;
        model::process_noise_covariance(params) = detail::initQ();
        float log_pdf = find_good_parameters(original_part_ptr, params, 1000);
        model::observation_error_covariance(params) << loc_err * loc_err, 0, 0, loc_err * loc_err;
        diag("%s, log(PDF)=%.2f", print_parameters(params).c_str(), log_pdf);
        plot_tracks(*tracks, params);
        plot_states_of_tracks(*tracks, params);
        quad_pdf.push_back(log_pdf);
    }
    /*
    std::pair<float, float> sing_stat = mean_stdev(sing_pdf.begin(), sing_pdf.end());
    std::pair<float, float> quad_stat = mean_stdev(quad_pdf.begin(), quad_pdf.end());
    diag("sing %.2f +/- %.2f", sing_stat.first, sing_stat.second);
    diag("quad %.2f +/- %.2f", quad_stat.first, quad_stat.second);
    */
    return 0;
}

int main(int argc, char** argv) {
    // seed the PRNG to ensure consistent results across runs.
    //biggles::detail::seed_prng(0xfacedead);

    plan_no_plan();

    test1();

    ok1(true);
    return exit_status();
}


/*
 * value_hash.hpp
 *
 *  Created on: 19.02.2016
 *      Author: mike_gresens
 */

#pragma once

#include <boost/variant/detail/hash_variant.hpp>

namespace boost {

size_t
hash_value(const hessian::null_t& value);

size_t
hash_value(const hessian::date_t& value);

size_t
hash_value(const hessian::map_t& value);

size_t
hash_value(const hessian::object_t& value);

}

#include <boost/functional/hash.hpp>

namespace boost {

size_t
hash_value(const hessian::null_t& value)
{
	return 0xAAAAAAAAAAAAAAAA;
}

size_t
hash_value(const hessian::date_t& value)
{
	size_t seed = 0;
	hash_combine(seed, value.date().day_count().as_number());
	hash_combine(seed, value.time_of_day().total_milliseconds());
    return seed;
}

size_t
hash_value(const hessian::map_t& value)
{
	return hash_range(value.begin(), value.end());
}

size_t
hash_value(const hessian::object_t& value)
{
	return hash_range(value.begin(), value.end());
}

}


#ifndef _BGL_GRAPH_INFO_HPP_
#define _BGL_GRAPH_INFO_HPP_

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream> //for ostream
#include <boost/graph/graphviz.hpp> //for boost related

/* Vert_Info is a data structure that is used to maintain information on
 * vertices in the graph for BGL
 */
typedef struct vert_info{
  std::string name; //vertex label
}vert_info;

/* Same as vert_info, keeps track of data for edges in BGL */

typedef struct edge_info{
  long capacity; // capacity of the link
  int  weight;	 // weight for routing algorithms
}edge_info;

/* Required to sort the map of edge_info structures ='( */
inline bool operator<(const edge_info &lhs, const edge_info &rhs){
    return lhs.weight < rhs.weight;
}

typedef boost::adjacency_list<boost::listS, boost::vecS, boost::undirectedS, vert_info, edge_info> Graph;
typedef boost::graph_traits<Graph>::edge_descriptor edge_desc;
typedef boost::graph_traits<Graph>::edge_iterator edge_iter;
typedef boost::graph_traits <Graph>::vertex_descriptor vertex_descriptor;
typedef boost::property_map < Graph, boost::vertex_name_t >::type NameMap;
//unsigned integer type for num verts in graph
typedef boost::graph_traits<Graph>::vertices_size_type size_type;
typedef boost::graph_traits<Graph>::vertex_iterator vertex_iterator;

/*  These two templates are required for printing out graph to dot format */
template <class WeightMap,class CapacityMap> class edge_writer {
  public:
    edge_writer(WeightMap w, CapacityMap c) : wm(w),cm(c) {}
    template <class Edge>
    void operator()(std::ostream &out, const Edge& e) const {
      out << "[label=\" W = " << wm[e] << "\\n C = " << cm[e] << "\"]";
  }
  private:
    WeightMap wm;
    CapacityMap cm;
};

template <class WeightMap, class CapacityMap>
inline edge_writer<WeightMap,CapacityMap>
make_edge_writer(WeightMap w,CapacityMap c) {
  return edge_writer<WeightMap,CapacityMap>(w,c);
}

#endif


/***********************************************************************
Copyright 2018 Gregory Bryant

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

#include "pointset.h"
#include "neconsolewindow.h"
#include "neview.h"
#include <boost/multiprecision/gmp.hpp>
#include <iostream>

PointSet::PointSet()
{
name = "PointSet";
}

void PointSet::render(NEView *view)
{
    //add checks to only draw on screen points
    view->getScreenBounds(&top,&bottom,&left,&right);
    for(int i=0;i<points.length();i++)
    {
        view->setPoint(points[i].first,points[i].second);
    }
}


/*
 * Copyright 2019 Tomasz bla Fortuna. All rights reserved.
 * License: MIT
 *
 * Benchmarks to approximate the performance.
 */

#include <iostream>
#include <algorithm>
#include <functional>
#include <bitset>
#include <boost/algorithm/string.hpp>
#include <charconv>

#include "trie.hpp"
#include "tritrie.hpp"
#include "flatritrie.hpp"
#include "multitritrie.hpp"
// #include "flat4.hpp"

#include "hashmap.hpp"
#include "utils.hpp"

/* Test suite with all tests. */
template<typename T>
void test_suite(T &algo, const std::string &name,
                const std::vector<uint32_t> &test_queries)
{
    std::cout << "== Test Suite for " << name << std::endl;

    /* Selected to be a deep /32 entry */
    const uint32_t ip_positive_deep = ip_to_hl("72.247.182.140");

    /* Selected to be a deep /32 entry */
    const uint32_t ip_negative = ip_to_hl("134.191.220.74");

    const int queries_cnt = test_queries.size();

    test_query("True random query test",
               algo,
               [] (int i) {return fastrand();});

    test_query("Positive random query test",
               algo,
               [test_queries, queries_cnt] (int i) {
                 return test_queries[i % queries_cnt];
               });

    test_query("Repetitive positive /32 query test",
               algo,
               [ip_positive_deep] (int i) {return ip_positive_deep;});

    test_query("Repetitive negative query test",
               algo,
               [ip_negative] (int i) {return ip_negative;});

    show_mem_usage(false);
}

void test_map(const std::vector<std::string> &test_data,
              const std::vector<uint32_t> &test_queries) {
    /* Construct map */
    IPMap<> map;
    test_generation("Hashmap", map, test_data);
    std::cout << "Hashmap size is " << map.size() << std::endl;

    test_suite(map, "Hashmap", test_queries);
    std::cout << std::endl;
}

void test_trie(const std::vector<std::string> &test_data,
               const std::vector<uint32_t> &test_queries) {
    Trie trie;
    test_generation("Trie", trie, test_data);
    std::cout << "Nodes created " << trie.size() << std::endl;

    test_suite(trie, "Trie", test_queries);

    FlaTrie flatrie;
    measure("Flatrie generation",
            [&flatrie, &trie] () {
                flatrie.build(trie);
            });

    test_suite(flatrie, "Flatrie", test_queries);

    show_mem_usage();
    std::cout << std::endl;
}

template<int BITS=8>
void test_tritrie(const std::string &name,
                  const std::vector<std::string> &test_data,
                  const std::vector<uint32_t> &test_queries) {
    Tritrie::Tritrie<BITS> tritrie;
    Tritrie::Tritrie<BITS> multi_tritrie;

    test_generation("Tritrie" + name, tritrie, test_data);
    std::cout << "Nodes created " << tritrie.size() << std::endl;
    test_suite(tritrie, "Tritrie" + name, test_queries);

    test_generation("MultiTritrie" + name, multi_tritrie, test_data);
    test_suite(multi_tritrie, "MultiTritrie" + name, test_queries);

    Tritrie::Flat<BITS> flatritrie;
    measure("Flatritrie" + name + " generation",
            [&] () {
                flatritrie.build(tritrie);
            });
    test_suite(flatritrie, "Flatritrie" + name, test_queries);
    flatritrie.debug();
    std::cout << std::endl;
}

int main() {
    #ifndef NDEBUG
    std::cout << "Watchout - for good results use benchmarks with RELEASE=1"
              << std::endl;
    #endif

    /* Prepare subnet data and query data */
    auto test_data = load_test_data("test_data.txt");
    auto test_queries = get_rnd_test_data(test_data);

    /* To get accurate RAM measurements, test one structure at a time */
    show_mem_usage(true);
    test_trie(test_data, test_queries);

    show_mem_usage(true);
    test_tritrie<8>("<8>", test_data, test_queries);

    show_mem_usage(true);
    test_tritrie<6>("<6>", test_data, test_queries);

    show_mem_usage(true);
    test_tritrie<4>("<4>", test_data, test_queries);

    show_mem_usage(true);
    test_map(test_data, test_queries);
    return 0;
}