// Source.cpp
// created by Kuangdai on 8-May-2016
// base class of source
// we only consider point sources located on the axis

#include "Source.h"
#include "Quad.h"
#include "Domain.h"
#include "Element.h"
#include "SourceTerm.h"
#include "Mesh.h"
#include "XMath.h"
#include "SpectralConstants.h"
#include "XMPI.h"
#include "MultilevelTimer.h"

Source::Source(double depth, double lat, double lon):
mDepth(depth), mLatitude(lat), mLongitude(lon) {
    // handle singularity at poles
    if (std::abs(mLatitude - 90.) < tinyDouble) {
        mLatitude = 90.;
        mLongitude = 0.;
    }
    if (std::abs(mLatitude + 90.) < tinyDouble) {
        mLatitude = -90.;
        mLongitude = 0.;
    }
}

void Source::release(Domain &domain, const Mesh &mesh) const {
    MultilevelTimer::begin("Locate Source", 2);
    // locate local
    int myrank = XMPI::nproc();
    int locTag = -1;
    RDColP interpFactZ;
    if (locate(mesh, locTag, interpFactZ)) {
        myrank = XMPI::rank();
    }

    // min recRank
    int myrank_min = XMPI::min(myrank);
    if (myrank_min == XMPI::nproc()) {
        throw std::runtime_error("Source::release || Error locating source.");
    }
    MultilevelTimer::end("Locate Source", 2);

    MultilevelTimer::begin("Compute Source", 2);
    // release to me
    if (myrank_min == XMPI::rank()) {
        // compute source term
        arPP_CMatX3 fouriers;
        const Quad *myQuad = mesh.getQuad(locTag);
        computeSourceFourier(*myQuad, interpFactZ, fouriers);
        // add to domain
        Element *myElem = domain.getElement(myQuad->getElementTag());
        domain.addSourceTerm(new SourceTerm(myElem, fouriers));
    }
    MultilevelTimer::end("Compute Source", 2);
}

bool Source::locate(const Mesh &mesh, int &locTag, RDColP &interpFactZ) const {
    MultilevelTimer::begin("R Source", 3);
    RDCol2 srcCrds = RDCol2::Zero();
    srcCrds(1) = mesh.computeRadiusRef(mDepth, mLatitude, mLongitude);
    MultilevelTimer::end("R Source", 3);

    // check range of subdomain
    if (srcCrds(0) > mesh.sMax() + tinySingle || srcCrds(0) < mesh.sMin() - tinySingle) {
        return false;
    }
    if (srcCrds(1) > mesh.zMax() + tinySingle || srcCrds(1) < mesh.zMin() - tinySingle) {
        return false;
    }
    // find host element
    RDCol2 srcXiEta;
    for (int iloc = 0; iloc < mesh.getNumQuads(); iloc++) {
        const Quad *quad = mesh.getQuad(iloc);
        if (!quad->isAxial() || quad->isFluid() || !quad->nearMe(srcCrds(0), srcCrds(1))) {
            continue;
        }
        if (quad->invMapping(srcCrds, srcXiEta)) {
            if (std::abs(srcXiEta(1)) <= 1.000001) {
                if (std::abs(srcXiEta(0) + 1.) > tinySingle) {
                    throw std::runtime_error("Source::locate || Bad source location.");
                }
                locTag = iloc;
                XMath::interpLagrange(srcXiEta(1), nPntEdge,
                    SpectralConstants::getP_GLL().data(), interpFactZ.data());
                return true;
            }
        }
    }
    return false;
}

#include "Parameters.h"
#include "Earthquake.h"
#include "PointForce.h"
#include "NullSource.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <cfloat>

void Source::parseLine(const std::string &line, const std::string &key, double &res) {
    std::stringstream ss;
    std::string keylong;
    ss << line;
    ss >> keylong;
    if (boost::icontains(keylong, key)) {
        ss >> res;
    }
}

void Source::checkValue(const std::string &key, double res) {
    if (res > DBL_MAX * .99) {
        throw std::runtime_error("Source::checkValue || "
            "Error initializing source parameter: " + key);
    }
}

void Source::buildInparam(Source *&src, const Parameters &par, int verbose) {
    if (src) {
        delete src;
    }

    // null source
    if (par.getValue<bool>("DEVELOP_NON_SOURCE_MODE")) {
        src = new NullSource();
        if (verbose) {
            XMPI::cout << src->verbose();
        }
        return;
    }

    std::string src_type = par.getValue<std::string>("SOURCE_TYPE");
    std::string src_file = par.getValue<std::string>("SOURCE_FILE");

    if (boost::iequals(src_type, "earthquake")) {
        std::string cmtfile = Parameters::sInputDirectory + "/" + src_file;
        double depth = DBL_MAX, lat = DBL_MAX, lon = DBL_MAX;
        double Mrr = DBL_MAX, Mtt = DBL_MAX, Mpp = DBL_MAX;
        double Mrt = DBL_MAX, Mrp = DBL_MAX, Mtp = DBL_MAX;
        if (XMPI::root()) {
            std::fstream fs(cmtfile, std::fstream::in);
            if (!fs) {
                throw std::runtime_error("Source::buildInparam || "
                    "Error opening CMT data file: ||" + cmtfile);
            }
            std::string line;
            while (std::getline(fs, line)) {
                parseLine(line, "latitude", lat);
                parseLine(line, "longitude", lon);
                parseLine(line, "depth", depth);
                parseLine(line, "Mrr", Mrr);
                parseLine(line, "Mtt", Mtt);
                parseLine(line, "Mpp", Mpp);
                parseLine(line, "Mrt", Mrt);
                parseLine(line, "Mrp", Mrp);
                parseLine(line, "Mtp", Mtp);
            }
            checkValue("latitude", lat);
            checkValue("longitude", lon);
            checkValue("depth", depth);
            checkValue("Mrr", Mrr);
            checkValue("Mtt", Mtt);
            checkValue("Mpp", Mpp);
            checkValue("Mrt", Mrt);
            checkValue("Mrp", Mrp);
            checkValue("Mtp", Mtp);
            // unit
            depth *= 1e3;
            Mrr *= 1e-7;
            Mtt *= 1e-7;
            Mpp *= 1e-7;
            Mrt *= 1e-7;
            Mrp *= 1e-7;
            Mtp *= 1e-7;
            fs.close();
        }
        XMPI::bcast(depth);
        XMPI::bcast(lat);
        XMPI::bcast(lon);
        XMPI::bcast(Mrr);
        XMPI::bcast(Mtt);
        XMPI::bcast(Mpp);
        XMPI::bcast(Mrt);
        XMPI::bcast(Mrp);
        XMPI::bcast(Mtp);
        src = new Earthquake(depth, lat, lon, Mrr, Mtt, Mpp, Mrt, Mrp, Mtp);
    } else if (boost::iequals(src_type, "point_force")) {
        // point force
        std::string pointffile = Parameters::sInputDirectory + "/" + src_file;
        double depth = DBL_MAX, lat = DBL_MAX, lon = DBL_MAX;
        double f1 = DBL_MAX, f2 = DBL_MAX, f3 = DBL_MAX;
        if (XMPI::root()) {
            std::fstream fs(pointffile, std::fstream::in);
            if (!fs) {
                throw std::runtime_error("Source::buildInparam || "
                    "Error opening point force data file: ||" + pointffile);
            }
            std::string line;
            while (std::getline(fs, line)) {
                parseLine(line, "latitude", lat);
                parseLine(line, "longitude", lon);
                parseLine(line, "depth", depth);
                parseLine(line, "Ft", f1);
                parseLine(line, "Fp", f2);
                parseLine(line, "Fr", f3);
            }
            checkValue("latitude", lat);
            checkValue("longitude", lon);
            checkValue("depth", depth);
            checkValue("Ft", f1);
            checkValue("Fp", f2);
            checkValue("Fr", f3);
            // unit
            depth *= 1e3;
            fs.close();
        }
        XMPI::bcast(depth);
        XMPI::bcast(lat);
        XMPI::bcast(lon);
        XMPI::bcast(f1);
        XMPI::bcast(f2);
        XMPI::bcast(f3);
        src = new PointForce(depth, lat, lon, f1, f2, f3);
    } else {
        throw std::runtime_error("Source::buildInparam || Unknown source type: " + src_type);
    }

    if (verbose) {
        XMPI::cout << src->verbose();
    }
}


//
// Copyright Jason Rice 2015
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef NBDL_MPDEF_METASTRUCT_HPP
#define NBDL_MPDEF_METASTRUCT_HPP

#include <mpdef/map.hpp>
#include <mpdef/tree_node.hpp>

#include <boost/hana.hpp>
#include <boost/mp11/bind.hpp>

namespace mpdef {

namespace hana = boost::hana;

struct metastruct_tag { };

template<typename Impl>
struct metastruct
{
  template<typename ...T>
  struct list { using hana_tag = mpdef::metastruct_tag ; };

  template<std::size_t i>
  struct at_fn
  {
    static constexpr std::size_t value = i;

    template<typename... T>
    constexpr auto operator()(list<T...>) const
      -> decltype(hana::arg<i + 1>(T{}...))
    { return {}; }

    template<typename T>
    constexpr auto operator=(T) const
      -> mpdef::tree_node<at_fn<i>, T>
    { return {}; }
  };

  template<std::size_t i>
  static constexpr at_fn<i> at_c{};
};

template<typename  M>
struct make_metastruct_fn
{
  template<typename... T>
  constexpr auto operator()(T...) const
  {
    return typename M::template list<T...>{};
  }
};

template<typename M>
constexpr make_metastruct_fn<M> make_metastruct{};

template<typename M>
struct make_metastruct_with_map_fn
{
  template<typename... Pairs>
  constexpr auto operator()(Pairs ...pairs) const
  {
    static_assert(sizeof...(Pairs) == decltype(hana::length(M::spec))::value
      , "Keys provided must match the spec for the Metastruct.");
    return hana::unpack(M::spec,
      hana::on(
        make_metastruct<M>,
        hana::partial(hana::at_key, mpdef::make_map(pairs...))
      )
    );
  }
};

template<typename M>
constexpr make_metastruct_with_map_fn<M> make_metastruct_with_map{};

template <typename Key, typename List>
using metastruct_get_impl = decltype(Key{}(List{}));

template <typename Key>
using metastruct_get = boost::mp11::mp_bind_front<metastruct_get_impl, Key>;

} // mpdef

namespace boost { namespace hana {
  template<>
  struct equal_impl<mpdef::metastruct_tag, mpdef::metastruct_tag>
  {
    template<typename T, typename U>
    static constexpr auto apply(T, U)
      -> hana::false_
    { return {}; }

    template<typename T>
    static constexpr auto apply(T, T)
      -> hana::true_
    { return {}; }
  };

}} // boost::hana

#endif


#include "core/physics/physics_manager.h"
#include "core/physics/constraints/constraint_base.h"
#include "core/physics/body_template.h"

#include "core/world/level.h"
#include "core/world/tile.h"
#include "core/world/tile_descriptor.h"

#include "core/rendering/render_manager.h"

#include "core/utility/spatial_hashmap.h"

#include <easylogging++.h>

#include <glm/detail/type_mat.hpp>
#include <glm/gtx/norm.hpp>

#include <boost/range/join.hpp>

#include <ppl.h>

#include <algorithm>
#include <bitset>

using namespace std;

namespace eversim { namespace core { namespace physics {

	body* physics_manager::add_body(body_template const& templ, glm::vec2 pos, glm::vec2 scale)
	{
		// create body
		auto* bdy = bodies.emplace();

		// reserve space for new particles

		auto const& p_tmpl = templ.particles;
		bdy->particles = allocate_particles(p_tmpl.size());
		bdy->position = bdy->old_position = pos;

		transform(
			p_tmpl.begin(), p_tmpl.end(),
			bdy->particles.begin(),
			[&](particle_descriptor const& desc)
		{
			particle p;
			p.pos = desc.pos * scale + pos;
			p.owner = bdy;
			p.inv_mass = 1.f / desc.mass;
			return p;
		});

		// create constraints
		for (auto const& cd : templ.constraints)
		{
			auto& loader = cd.factory;
			auto constraint = loader->build(cd, bdy);
			add_constraint(move(constraint));
		}

		return bdy;
	}

	void physics_manager::remove_body(body* b)
	{
		if (!b) return;
		b->kill();
		num_dead_bodies++;
	}

	void physics_manager::set_level(world::level const* l)
	{
		level = l;
	}

	void physics_manager::integrate(float dt)
	{
		apply_external_forces(dt);
		damp_velocities();
		for (auto& p : particles)
		{
			if (!p.is_alive()) continue;
			p.projected_position = p.pos + dt * p.vel;
		}

		check_collisions();

		for (auto i = 0; i < solver_iterations; ++i)
		{
			project_constraints();
		}

		finalize_changes(dt);
		call_events();
	}

	void physics_manager::atomic_step(float dt)
	{
		switch (current_state)
		{
		case simulation_state::external:
			apply_external_forces(dt);
			current_state = simulation_state::damp;
			break;
		case simulation_state::damp:
			damp_velocities();
			current_state = simulation_state::apply_velocity;
			break;
		case simulation_state::apply_velocity:
			for (auto& p : particles)
			{
				p.projected_position = p.pos + dt * p.vel;
			}
			current_state = simulation_state::check_collisions;
			break;
		case simulation_state::check_collisions:
			check_collisions();
			current_state = simulation_state::constraint_iteration;
			break;
		case simulation_state::constraint_iteration:
			if (current_iteration < solver_iterations)
			{
				project_constraints();
				current_iteration++;
			} else
			{
				current_iteration = 0;
				current_state = simulation_state::apply_changes;
			}
			break;
		case simulation_state::apply_changes:
			finalize_changes(dt);
			current_state = simulation_state::call_events;
			break;
		case simulation_state::call_events:
			call_events();
			current_state = simulation_state::external;
			break;
		default:;
			assert(!"unknown state!");
		}
	}

	void physics_manager::check_collisions()
	{
		collision_constraints.clear();
		static_collision_constraints.clear();

		possible_collisions.set_cell_size(2.f*particle_radius);
		possible_collisions.reset(particles.size(), particles.size() * 2);

		concurrency::parallel_for_each(begin(particles), end(particles),
			[this](particle& p)
		{
			if (p.is_alive())
				possible_collisions.insert(p.pos, &p);
		});

		if (level){
			for(auto&p : particles)
			{
				if (!p.is_alive())
					continue;
				particle_tile_collision(p);
			}
		}

		for (auto& p : particles)
		{
			if (!p.is_alive()) continue;
			auto index = possible_collisions.get_index(p.pos);
			for(int dx = -1; dx <=1; ++dx)
			{
				for(int dy = -1; dy <= 1; ++dy)
				{
					for(auto* other : possible_collisions.get_cell(index + glm::ivec2(dx,dy)))
					{
						if (other >= &p || other->owner == p.owner)
							continue;
						if(length(p.pos - other->pos) < 2*particle_radius)
						{
							auto c = distance_constraint{2*particle_radius};
							c.set_type(constraint_type::inequality);
							c.particles = {&p, other};
							collision_constraints.emplace_back(c);
						}
					}
				}
			}
		}
	}

	string physics_manager::get_step_name() const
	{
		switch (current_state)
		{
		case simulation_state::external: return "Applying external forces";
		case simulation_state::damp: return "Damping velocity";
		case simulation_state::apply_velocity: return "Moving particles by v*dt";
		case simulation_state::constraint_iteration:
			return "Constraint solver " + to_string(current_iteration) + "/" + to_string(solver_iterations);
		case simulation_state::apply_changes: return "Moving particles to projected pos";
		case simulation_state::check_collisions: return "Checking collisions";
		case simulation_state::call_events: return "Calling events";
		default: return "Invalid state!";
		}
	}

	auto physics_manager::get_constraints() const
		-> boost::any_range<constraint const&, boost::forward_traversal_tag>
	{
		using namespace boost;
		using namespace boost::range;
		const auto r1 = join(constraints, collision_constraints);
		const auto r2 = join(r1, static_collision_constraints);
		return r2;
	}

	void physics_manager::add_constraint(unique_ptr<constraint> c)
	{
		constraints.insert(*c);
	}

	utility::array_view<particle> physics_manager::allocate_particles(size_t num)
	{
		const auto oldsize = particles.size();
		const auto old_capacity = particles.capacity();

		const auto* old_begin = particles.data();

		if (oldsize + num > old_capacity)
		{
			auto newvec = vector<particle>();
			newvec.reserve(max(particles.capacity() * 2, oldsize + num));

			auto* ptr = newvec.data();
			for (auto& b : bodies)
			{
				if (!b.is_alive())
					continue;

				newvec.insert(newvec.end(), b.particles.begin(), b.particles.end());
				const auto s = b.particles.size();
				b.particles = utility::make_array_view(ptr, s);
				ptr += s;
			}
			particles.swap(newvec);
		}
		particles.resize(oldsize + num);
		return utility::make_array_view(particles).slice(oldsize, 0);
	}

	void physics_manager::cleanup_dead_bodies()
	{
		for(auto it = constraints.begin(); it != constraints.end(); ++it)
		{
			if(!it->is_alive())
			{
				it = constraints.erase(it);
			}
		}
		for(auto& b : bodies)
		{
			if(!b.is_alive())
			{
				for(auto& p : b.particles)
				{
					p.owner = nullptr;
				}
				bodies.erase(bodies.locate(&b));
			}
		}
		num_dead_bodies = 0;
	}

	void physics_manager::apply_external_forces(float dt)
	{
		for (auto& p : particles)
		{
			if (!p.is_alive()) continue;
			const auto* bdy = p.owner;
			const auto body_movement = bdy->position - bdy->old_position;
			p.pos += body_movement;

			const auto body_acceleration = bdy->velocity - bdy->old_velocity;
			p.vel += body_acceleration;

			if (p.inv_mass == 0.f)
				continue;

			p.vel += dt * gravity * p.owner->gravity_scale;
		}
		for(auto& b : get_bodies())
		{
			b.position = {};
			b.velocity = {};
		}
	}

	void physics_manager::damp_velocities()
	{
		for (auto& p : particles)
		{
			if (!p.is_alive()) continue;
			p.vel *= damping; // TODO: improve
		}
	}

	namespace {
		void project_single_constraint(constraint const& c, int solver_iterations)
		{
			const auto err = c();
			switch (c.get_type())
			{
			case constraint_type::equality:
				{
					if (err == 0)
						return;
					break;
				}
			case constraint_type::inequality:
				{
					if (err >= 0)
						return;
					break;
				}
			default:
				{
					assert(!"Unhandled constraint type!");
				}
			}

			static thread_local glm::vec2 cache[physics_manager::max_constraint_arity];
			auto grad = utility::make_array_view(cache).slice_n(0, c.get_arity());
			c.grad(grad);

			const auto sum = [&]
			{
				auto sum = 0.f;
				for (auto i = 0; i < c.get_arity(); ++i)
				{
					sum += c.particles[i]->inv_mass * glm::length2(grad[i]);
				}
				return sum;
			}();
			const auto scale = err / sum;

			const auto k = 1.f - powf(1.f - c.stiffness, 1.f / solver_iterations);

			for (auto i = 0; i < c.get_arity(); ++i)
			{
				auto& p = c.particles[i];
				const auto correction = -scale * p->inv_mass * grad[i] * k;
				if(glm::length(correction) > 0.5f)
				{
					LOG(WARNING) << "Way too big step! (" << correction.x << ", " << correction.y <<")";
				}
				p->projected_position += correction;
			}
		}
	}

	void physics_manager::project_constraints()
	{
		for (auto const& c : constraints)
		{
			project_single_constraint(c, solver_iterations);
		}
		for(auto const& c : collision_constraints)
		{
			project_single_constraint(c, solver_iterations);
		}
		for(auto const& c : static_collision_constraints)
		{
			project_single_constraint(c, solver_iterations);
		}
	}

	void physics_manager::finalize_changes(float dt)
	{
		for (auto& p : particles)
		{
			if (!p.is_alive()) continue;
			p.vel = (p.projected_position - p.pos) / dt;
			p.pos = p.projected_position;

			auto* const b = p.owner;
			b->velocity += p.vel;
			b->position += p.pos;
		}
		for(auto& b : get_bodies())
		{
			b.old_position = b.position = b.position/float(b.particles.size());
			b.old_velocity = b.velocity = b.velocity/float(b.particles.size());
		}
	}

	void physics_manager::call_events()
	{
		using bodypair = pair<body*, body*>;
		map<bodypair, events::dyn_col_list> bb_collisions;
		for(auto& c : collision_constraints)
		{
			auto p1 = c.particles[0];
			auto p2 = c.particles[1];

			assert(p1.base != p2.base && "Collision between particles of the same body is not allowed!");

			if(p1.base > p2.base)
			{
				swap(p1, p2);
			}

			auto key = make_pair( p1->owner, p2->owner );
			bb_collisions[key].push_back({ p1.resolve(), p2.resolve() });
		}

		map<body*, events::static_col_list> bl_collisions;
		for(auto& c : static_collision_constraints)
		{
			auto& p = *c.particles[0];
			auto& l = bl_collisions[p.owner];

			l.push_back({&p, c.tile(), c.normal()});
		}

		for(auto& p : bb_collisions)
		{
			body* a;
			body* b;
			tie(a, b) = p.first;

			auto const& list = p.second;
			body_body_collision_event(a, b, list);
		}

		for(auto& p : bl_collisions)
		{
			body_level_collision_event(p.first, p.second);
		}
	}

	void physics_manager::particle_tile_collision(particle& p)
	{
		assert(level);

		const auto pos = p.projected_position;

		auto const& t = level->get_tile_by_pos(pos);

		const auto tile_coord = t.to_tile_coordinates(pos);
		int dx = (tile_coord.x > 0) ? 1 : -1;
		int dy = (tile_coord.y > 0) ? 1 : -1;

		world::tile const* tiles[] = {
			&t,
			t.get_neighbour({ dx, 0 }),
			t.get_neighbour({  0,dy }),
			t.get_neighbour({ dx,dy }),
		};

		for(auto const* tile : tiles)
		{
			if(!tile || !tile->has_collision())
				continue;

			check_tile_collisions(p, *tile);
		}
	}

	void physics_manager::check_tile_collisions(particle const& particle, const world::tile& tile)
	{
		const auto mpos = particle.pos - tile.position();
		const auto mprojpos = particle.projected_position - tile.position();
		const auto ray = utility::line{ mpos, mprojpos };

		for(auto const& side : tile.get_collision_shape())
		{
			const auto normal = side.normal();

			if (dot(particle.vel, normal) > 0)
				continue;

			if(auto intersection = ray.intersect(side))
			{
				const auto entry = ray.lerp(*intersection) + tile.position();

				static_collision_constraints.emplace_back(tile, particle, normal, entry);
			}

			const auto dist = side.distance_to_point(mpos);
			if(dist < 0.3f * tile.size())
			{
				const auto entry = side.closest_point(mpos) + tile.position();

				static_collision_constraints.emplace_back(tile, particle, normal, entry);
			}
		}

	}

}}}


#ifndef K3_MOSAICBUILTINS
#define K3_MOSAICBUILTINS

#include <climits>
#include <cmath>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>

#include "types/BaseString.hpp"
#include "Common.hpp"
#include "collections/Collection.hpp"
#include "collections/MapE.hpp"
#include "collections/Set.hpp"
#include "collections/Seq.hpp"

namespace K3 {

class MosaicBuiltins {
 public:
  template <class A, class B>
  auto max(A a, B b) {
    return (a > b ? a : b);
  }

  template <class A, class B>
  auto min(A a, B b) {
    return (a < b ? a : b);
  }

  template <class A>
  auto abs(A a) {
    return std::abs(a);
  }

  int date_part(const string_impl& s, int y) {
    if (s == "day" || s == "DAY") {
      return y % 100;
    }
    if (s == "month" || s == "MONTH") {
      return (y % 10000) / 100;
    }
    if (s == "year" || s == "YEAR") {
      return (y / 10000);
    }
    throw std::runtime_error("Unrecognized date part key: " + std::string(s));
  }

  double real_of_int(int n) { return static_cast<double>(n); }
  int get_max_int(unit_t) { return INT_MAX; }
  int truncate(double n) { return static_cast<int>(n); }
  int regex_match_int(const string_impl& s1, const string_impl& s2) {
    bool b = std::regex_match(s2.c_str(), std::regex(s1.c_str()));
    return b ? 1 : 0;
  }

  // For routing
  /*
  void free_buckets_loop(
      Set<R_i<Address>> &results,
      const std::unordered_map<int, R_key_value<int, R_key_value<int, Seq<R_i<int>>>>> &dim_bounds,
      const std::vector<R_i<int>> free_dims,
      std::vector<R_i<int>>::const_iterator free_it,
      std::function<std::function<Address(int)>(int)> get_ring_node,
      int val,
      int max_val) {
    // Assume it exists -- don't check for error
    auto it  = dim_bounds.find((*free_it).i);
    auto dim  = it->second.value.key;
    auto &rng = it->second.value.value;
    // Check if we've reach the end
    if (free_it == free_dims.end()) {
      results.insert(get_ring_node(val)(max_val));
    } else {
      auto free_it2 = free_it + 1;
      for (auto &idx : rng.getConstContainer()) {
        val += idx.i * dim;
        free_buckets_loop(results, dim_bounds, free_dims,
            free_it2, get_ring_node, val, max_val);
      }
    }
  }
  */

  //Set<R_i<Address>> free_buckets_builtin(
  //    const std::unordered_map<int, R_key_value<int, R_key_value<int, Seq<R_i<int>>>>> &dim_bounds,
  //    const Collection<R_i<int>> &free_dims,
  //    std::function<std::function<Address(int)>(int)> get_ring_node,
  //    int bound_bucket,
  //    int max_val) {
  //  Set<R_i<Address>> results;
  //  auto &free_dims_c = free_dims.getConstContainer();
  //  free_buckets_loop(results, dim_bounds, free_dims_c, free_dims_c.begin(),
  //      get_ring_node, bound_bucket, max_val);
  //  return results;
  //}

  }; // class

}  // namespace K3

#endif


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_GTRFS_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_GTRFS_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for gtrfs is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
template< typename Trans >
inline std::ptrdiff_t gtrfs( const Trans, const fortran_int_t n,
        const fortran_int_t nrhs, const float* dl, const float* d,
        const float* du, const float* dlf, const float* df, const float* duf,
        const float* du2, const fortran_int_t* ipiv, const float* b,
        const fortran_int_t ldb, float* x, const fortran_int_t ldx,
        float* ferr, float* berr, float* work, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_SGTRFS( &lapack_option< Trans >::value, &n, &nrhs, dl, d, du, dlf,
            df, duf, du2, ipiv, b, &ldb, x, &ldx, ferr, berr, work, iwork,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
template< typename Trans >
inline std::ptrdiff_t gtrfs( const Trans, const fortran_int_t n,
        const fortran_int_t nrhs, const double* dl, const double* d,
        const double* du, const double* dlf, const double* df,
        const double* duf, const double* du2, const fortran_int_t* ipiv,
        const double* b, const fortran_int_t ldb, double* x,
        const fortran_int_t ldx, double* ferr, double* berr, double* work,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_DGTRFS( &lapack_option< Trans >::value, &n, &nrhs, dl, d, du, dlf,
            df, duf, du2, ipiv, b, &ldb, x, &ldx, ferr, berr, work, iwork,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
template< typename Trans >
inline std::ptrdiff_t gtrfs( const Trans, const fortran_int_t n,
        const fortran_int_t nrhs, const std::complex<float>* dl,
        const std::complex<float>* d, const std::complex<float>* du,
        const std::complex<float>* dlf, const std::complex<float>* df,
        const std::complex<float>* duf, const std::complex<float>* du2,
        const fortran_int_t* ipiv, const std::complex<float>* b,
        const fortran_int_t ldb, std::complex<float>* x,
        const fortran_int_t ldx, float* ferr, float* berr,
        std::complex<float>* work, float* rwork ) {
    fortran_int_t info(0);
    LAPACK_CGTRFS( &lapack_option< Trans >::value, &n, &nrhs, dl, d, du, dlf,
            df, duf, du2, ipiv, b, &ldb, x, &ldx, ferr, berr, work, rwork,
            &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
template< typename Trans >
inline std::ptrdiff_t gtrfs( const Trans, const fortran_int_t n,
        const fortran_int_t nrhs, const std::complex<double>* dl,
        const std::complex<double>* d, const std::complex<double>* du,
        const std::complex<double>* dlf, const std::complex<double>* df,
        const std::complex<double>* duf, const std::complex<double>* du2,
        const fortran_int_t* ipiv, const std::complex<double>* b,
        const fortran_int_t ldb, std::complex<double>* x,
        const fortran_int_t ldx, double* ferr, double* berr,
        std::complex<double>* work, double* rwork ) {
    fortran_int_t info(0);
    LAPACK_ZGTRFS( &lapack_option< Trans >::value, &n, &nrhs, dl, d, du, dlf,
            df, duf, du2, ipiv, b, &ldb, x, &ldx, ferr, berr, work, rwork,
            &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to gtrfs.
//
template< typename Value, typename Enable = void >
struct gtrfs_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct gtrfs_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR,
            typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, detail::workspace2< WORK,
            IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixX >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorD >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDLF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDUF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDU2 >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixX >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorFERR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBERR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorFERR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBERR >::value) );
        BOOST_ASSERT( bindings::size(berr) >= bindings::size_column(b) );
        BOOST_ASSERT( bindings::size(d) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(df) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(dl) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(dlf) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(du) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(du2) >= bindings::size_column_op(d,
                trans())-2 );
        BOOST_ASSERT( bindings::size(duf) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(ipiv) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( bindings::size_column_op(d, trans()) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_column_op(d, trans()) ));
        BOOST_ASSERT( bindings::size_column(b) >= 0 );
        BOOST_ASSERT( bindings::size_column_op(d, trans()) >= 0 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(x) == 1 ||
                bindings::stride_minor(x) == 1 );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(d, trans())) );
        BOOST_ASSERT( bindings::stride_major(x) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(d, trans())) );
        return detail::gtrfs( trans(), bindings::size_column_op(d, trans()),
                bindings::size_column(b), bindings::begin_value(dl),
                bindings::begin_value(d), bindings::begin_value(du),
                bindings::begin_value(dlf), bindings::begin_value(df),
                bindings::begin_value(duf), bindings::begin_value(du2),
                bindings::begin_value(ipiv), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(x),
                bindings::stride_major(x), bindings::begin_value(ferr),
                bindings::begin_value(berr),
                bindings::begin_value(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_column_op(d, trans()) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column_op(d, trans()) ) );
        return invoke( dl, d, du, dlf, df, duf, du2, ipiv, b, x, ferr, berr,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        return invoke( dl, d, du, dlf, df, duf, du2, ipiv, b, x, ferr, berr,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return 3*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t n ) {
        return n;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct gtrfs_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR,
            typename WORK, typename RWORK >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, detail::workspace2< WORK,
            RWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixB >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixX >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorFERR >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorBERR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorD >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDU >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDLF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDUF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorDU2 >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< VectorDL >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixX >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixX >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorFERR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorBERR >::value) );
        BOOST_ASSERT( bindings::size(berr) >= bindings::size_column(b) );
        BOOST_ASSERT( bindings::size(d) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(df) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(dl) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(dlf) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(du) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(du2) >= bindings::size_column_op(d,
                trans())-2 );
        BOOST_ASSERT( bindings::size(duf) >= bindings::size_column_op(d,
                trans())-1 );
        BOOST_ASSERT( bindings::size(ipiv) >= bindings::size_column_op(d,
                trans()) );
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_rwork( bindings::size_column_op(d, trans()) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_column_op(d, trans()) ));
        BOOST_ASSERT( bindings::size_column(b) >= 0 );
        BOOST_ASSERT( bindings::size_column_op(d, trans()) >= 0 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(x) == 1 ||
                bindings::stride_minor(x) == 1 );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(d, trans())) );
        BOOST_ASSERT( bindings::stride_major(x) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(d, trans())) );
        return detail::gtrfs( trans(), bindings::size_column_op(d, trans()),
                bindings::size_column(b), bindings::begin_value(dl),
                bindings::begin_value(d), bindings::begin_value(du),
                bindings::begin_value(dlf), bindings::begin_value(df),
                bindings::begin_value(duf), bindings::begin_value(du2),
                bindings::begin_value(ipiv), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(x),
                bindings::stride_major(x), bindings::begin_value(ferr),
                bindings::begin_value(berr),
                bindings::begin_value(work.select(value_type())),
                bindings::begin_value(work.select(real_type())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_column_op(d, trans()) ) );
        bindings::detail::array< real_type > tmp_rwork( min_size_rwork(
                bindings::size_column_op(d, trans()) ) );
        return invoke( dl, d, du, dlf, df, duf, du2, ipiv, b, x, ferr, berr,
                workspace( tmp_work, tmp_rwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename VectorDL, typename VectorD, typename VectorDU,
            typename VectorDLF, typename VectorDF, typename VectorDUF,
            typename VectorDU2, typename VectorIPIV, typename MatrixB,
            typename MatrixX, typename VectorFERR, typename VectorBERR >
    static std::ptrdiff_t invoke( const VectorDL& dl, const VectorD& d,
            const VectorDU& du, const VectorDLF& dlf, const VectorDF& df,
            const VectorDUF& duf, const VectorDU2& du2,
            const VectorIPIV& ipiv, const MatrixB& b, MatrixX& x,
            VectorFERR& ferr, VectorBERR& berr, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef tag::column_major order;
        typedef typename result_of::trans_tag< VectorD, order >::type trans;
        return invoke( dl, d, du, dlf, df, duf, du2, ipiv, b, x, ferr, berr,
                minimal_workspace() );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return 2*n;
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array rwork.
    //
    static std::ptrdiff_t min_size_rwork( const std::ptrdiff_t n ) {
        return n;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the gtrfs_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for gtrfs. Its overload differs for
// * User-defined workspace
//
template< typename VectorDL, typename VectorD, typename VectorDU,
        typename VectorDLF, typename VectorDF, typename VectorDUF,
        typename VectorDU2, typename VectorIPIV, typename MatrixB,
        typename MatrixX, typename VectorFERR, typename VectorBERR,
        typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
gtrfs( const VectorDL& dl, const VectorD& d, const VectorDU& du,
        const VectorDLF& dlf, const VectorDF& df, const VectorDUF& duf,
        const VectorDU2& du2, const VectorIPIV& ipiv, const MatrixB& b,
        MatrixX& x, VectorFERR& ferr, VectorBERR& berr, Workspace work ) {
    return gtrfs_impl< typename bindings::value_type<
            VectorDL >::type >::invoke( dl, d, du, dlf, df, duf, du2, ipiv, b,
            x, ferr, berr, work );
}

//
// Overloaded function for gtrfs. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename VectorDL, typename VectorD, typename VectorDU,
        typename VectorDLF, typename VectorDF, typename VectorDUF,
        typename VectorDU2, typename VectorIPIV, typename MatrixB,
        typename MatrixX, typename VectorFERR, typename VectorBERR >
inline typename boost::disable_if< detail::is_workspace< VectorBERR >,
        std::ptrdiff_t >::type
gtrfs( const VectorDL& dl, const VectorD& d, const VectorDU& du,
        const VectorDLF& dlf, const VectorDF& df, const VectorDUF& duf,
        const VectorDU2& du2, const VectorIPIV& ipiv, const MatrixB& b,
        MatrixX& x, VectorFERR& ferr, VectorBERR& berr ) {
    return gtrfs_impl< typename bindings::value_type<
            VectorDL >::type >::invoke( dl, d, du, dlf, df, duf, du2, ipiv, b,
            x, ferr, berr, optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


#include <fstream>
#include <limits>

#include <aslam/calibration/target-aprilgrid.h>
#include <aslam/cameras/camera.h>
#include <aslam/common/pose-types.h>
#include <aslam/geometric-vision/pnp-pose-estimator.h>
#include <glog/logging.h>
#include <Eigen/Core>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include <cv_bridge/cv_bridge.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#pragma GCC diagnostic pop

DEFINE_string(bag, "", "Rosbag path and filename.");
DEFINE_string(topic, "/cam0/image_raw", "Image topic name.");
DEFINE_bool(draw_extraction, false, "Show extracted corners.");
DEFINE_string(eval_camera_yaml, "",
              "Camera model definition to evaluate reprojection errors.");

DEFINE_string(camera_pose_T_C_G_output_file, "camera_pose.csv",
              "Outputs the camera poses for all successful observations. "
              "format: [frame_id, x, y, z, x, y, z, w] (JPL quaternion "
              "convention)");

DEFINE_string(camera_pose_T_C_G_output_file_timestamped,
              "camera_poses_timestamped.csv",
              "Outputs the camera poses for all successful observations. "
              "format: [timestamp, x, y, z, x, y, z, w] (JPL quaternion "
              "convention)");

DEFINE_string(
    camera_pose_T_C_G_output_file_mm, "camera_pose_mm.csv",
    "Outputs the camera poses for all successful observations. "
    "format: [frame_id, x[mm], y[mm], z[mm], x, y, z, w] (JPL quaternion "
    "convention)");

DEFINE_string(
    camera_pose_T_C_G_output_file_mm_timestamped,
    "camera_poses_mm_timestamped.csv",
    "Outputs the camera poses for all successful observations. "
    "format: [timestamp, x[mm], y[mm], z[mm], x, y, z, w] (JPL quaternion "
    "convention)");

DEFINE_bool(output_position_in_mm, false,
            "Converts the camera position into millimeters when outputting to "
            "csv file.");

DEFINE_bool(write_csv_header, true, "Write a header into the csv files.");

DEFINE_double(inlier_ratio_for_good_camera_pose, 0.4,
              "Minimal inlier ratio required for exported camera pose.");

DEFINE_int32(april_tag_number_vertical, 6, "Vertical number of april tags");
DEFINE_int32(april_tag_number_horizontal, 6, "Horizontal number of april tags");
DEFINE_double(april_tag_size_m, 0.055, "Size of april tags in meters.");
DEFINE_double(april_tag_gap_size_m, 0.0165,
              "Size of gaps between april tags in meters.");
DEFINE_int32(april_tag_pixel_boarder, 2,
             "Size of april tag boarder in pixels.");

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InstallFailureSignalHandler();
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;

  std::ofstream reprojection_logger;
  std::ofstream camera_pose_logger;
  std::ofstream camera_pose_logger_timestamped;
  std::ofstream camera_pose_logger_mm;
  std::ofstream camera_pose_logger_mm_timestamped;

  // Create target and detector.
  aslam::calibration::TargetAprilGrid::TargetConfiguration aprilgrid_config;
  aprilgrid_config.num_tag_rows = FLAGS_april_tag_number_vertical;
  aprilgrid_config.num_tag_cols = FLAGS_april_tag_number_horizontal;
  aprilgrid_config.tag_size_meter = FLAGS_april_tag_size_m;
  aprilgrid_config.tag_inbetween_space_meter = FLAGS_april_tag_gap_size_m;
  aprilgrid_config.black_tag_border_bits = FLAGS_april_tag_pixel_boarder;

  LOG(INFO) << "April Tag Target:";
  LOG(INFO) << "Dimension:     " << aprilgrid_config.num_tag_rows << "x"
            << aprilgrid_config.num_tag_cols;
  LOG(INFO) << "Tag size:      " << aprilgrid_config.tag_size_meter;
  LOG(INFO) << "Gap size:      " << aprilgrid_config.tag_inbetween_space_meter;
  LOG(INFO) << "Border pixels: " << aprilgrid_config.black_tag_border_bits;

  aslam::calibration::TargetAprilGrid::Ptr aprilgrid(
      new aslam::calibration::TargetAprilGrid(aprilgrid_config));

  aslam::calibration::DetectorAprilGrid::DetectorConfiguration
      aprilgrid_detector_config;
  aslam::calibration::DetectorAprilGrid aprilgird_detector(
      aprilgrid, aprilgrid_detector_config);

  // Load rosbag and connect to image topic.
  rosbag::Bag bag;
  bag.open(FLAGS_bag, rosbag::bagmode::Read);
  rosbag::View bag_view(bag, rosbag::TopicQuery(FLAGS_topic));

  // Iterate over all images of the topic and extract the calibration targets.
  std::vector<aslam::calibration::TargetObservation::Ptr> target_observations;
  target_observations.reserve(bag_view.size());

  std::vector<double> timestamps;

  // common::ProgressBar progress(bag_view.size());
  for (const rosbag::MessageInstance& message : bag_view) {
    sensor_msgs::ImageConstPtr image_message =
        message.instantiate<sensor_msgs::Image>();
    LOG_IF(FATAL, !image_message) << "Can only process image messages.";

    timestamps.push_back(image_message->header.stamp.toSec());

    // Convert image to cv::Mat.
    cv::Mat image;
    if ((image_message->encoding == "16UC1") ||
        (image_message->encoding == "mono16")) {
      sensor_msgs::Image img;
      img.header = image_message->header;
      img.height = image_message->height;
      img.width = image_message->width;
      img.is_bigendian = image_message->is_bigendian;
      img.step = image_message->step;
      img.data = image_message->data;
      img.encoding = "mono16";

      // Scale up the image to take use of the full 16bit range before encoding
      // it to 8bit range.
      cv_bridge::CvImagePtr cv_ptr_tmp;
      cv_ptr_tmp =
          cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO16);
      cv::normalize(cv_ptr_tmp->image, cv_ptr_tmp->image, 0,
                    std::numeric_limits<uint16_t>::max(), cv::NORM_MINMAX);
      cv_ptr_tmp->toImageMsg(img);

      cv_bridge::CvImageConstPtr cv_ptr;
      try {
        cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
      } catch (const cv_bridge::Exception& e) {
        LOG(FATAL) << "cv_bridge exception: " << e.what();
      }
      CHECK(cv_ptr);
      image = cv_ptr->image;
    } else {
      cv_bridge::CvImageConstPtr cv_ptr;
      try {
        cv_ptr = cv_bridge::toCvShare(image_message,
                                      sensor_msgs::image_encodings::MONO8);
      } catch (const cv_bridge::Exception& e) {
        LOG(FATAL) << "cv_bridge exception: " << e.what();
      }
      CHECK(cv_ptr);
      image = cv_ptr->image;
    }

    // Extract calibration targets.
    aslam::calibration::TargetObservation::Ptr observation =
        aprilgird_detector.detectTargetInImage(image);

    // Only keep the observation if it was successful.
    target_observations.emplace_back(observation);

    // Optionally draw the extracted corners.
    if (FLAGS_draw_extraction) {
      cv::Mat image_w_corners;
      cv::cvtColor(image, image_w_corners, CV_GRAY2BGR);
      cv::namedWindow("extracted corners");

      if (observation.get() != nullptr) {
        // Detection successful.
        observation->drawCornersIntoImage(&image_w_corners);
      } else {
        // Detection failed.
        cv::putText(image_w_corners, "failed",
                    cv::Point(10, image_w_corners.rows / 2), 5, 5,
                    cv::Scalar(0, 0, 255), 5, 5);
      }
      cv::imshow("extracted corners", image_w_corners);
      cv::waitKey(1);
    }
  }
  LOG(INFO) << "Number of observations in total: " << target_observations.size()
            << " from " << bag_view.size() << " images.";

  // Optionally evaluate all the reprojection errors of all target
  // observations.
  if (!FLAGS_eval_camera_yaml.empty()) {
    const std::string kLogFilename = "corner_reprojections.csv";
    reprojection_logger.open(kLogFilename);
    if (FLAGS_write_csv_header) {
      reprojection_logger
          << "frame_id, corner_id, pnp_success, pnp_inlier, reproj_success, "
             "meas_u, meas_v, reproj_u, reproj_v"
          << std::endl;
      ;
    }

    camera_pose_logger.open(FLAGS_camera_pose_T_C_G_output_file);
    if (FLAGS_write_csv_header) {
      camera_pose_logger << "sequence_number, x [m], y[m], z[m], x, y, z, w"
                         << std::endl;
    }

    camera_pose_logger_timestamped.open(
        FLAGS_camera_pose_T_C_G_output_file_timestamped);
    if (FLAGS_write_csv_header) {
      camera_pose_logger_timestamped
          << "timestamp, x [m], y[m], z[m], x, y, z, w" << std::endl;
    }

    if (FLAGS_output_position_in_mm) {
      CHECK(!FLAGS_camera_pose_T_C_G_output_file_mm.empty());
      camera_pose_logger_mm.open(FLAGS_camera_pose_T_C_G_output_file_mm);
      if (FLAGS_write_csv_header) {
        camera_pose_logger_mm
            << "sequence_number, x [mm], y[mm], z[mm], x, y, z, w" << std::endl;
      }

      camera_pose_logger_mm_timestamped.open(
          FLAGS_camera_pose_T_C_G_output_file_mm_timestamped);
      if (FLAGS_write_csv_header) {
        camera_pose_logger_mm_timestamped
            << "timestamp, x [mm], y[mm], z[mm], x, y, z, w" << std::endl;
      }
    }

    aslam::Camera::Ptr camera;
    camera->deserializeFromFile(FLAGS_eval_camera_yaml);
    CHECK(camera);

    constexpr bool kRunNonlinearRefinement = true;
    const double kPixelSigma = 0.5;
    const int kMaxRansacIters = 1000;
    aslam::geometric_vision::PnpPoseEstimator pnp(kRunNonlinearRefinement);

    size_t number_observation_success = 0u;
    size_t number_pnp_success = 0u;
    size_t number_of_good_camera_poses = 0u;
    size_t frame_id = 0;
    CHECK_EQ(target_observations.size(), timestamps.size());
    for (const aslam::calibration::TargetObservation::Ptr& obs :
         target_observations) {
      if (obs) {
        number_observation_success++;
        const Eigen::Matrix2Xd& keypoints_measured = obs->getObservedCorners();
        const Eigen::Matrix3Xd G_corner_positions =
            obs->getCorrespondingTargetPoints();

        aslam::Transformation T_G_C;
        std::vector<int> inliers;
        int num_iters = 0;
        bool pnp_success = pnp.absolutePoseRansacPinholeCam(
            keypoints_measured, G_corner_positions, kPixelSigma,
            kMaxRansacIters, camera, &T_G_C, &inliers, &num_iters);

        const double number_inliers = inliers.size();
        const double number_keypoints = keypoints_measured.cols();
        const double inlier_ratio = number_inliers / number_keypoints;
        const bool sufficient_inliers =
            inlier_ratio > FLAGS_inlier_ratio_for_good_camera_pose;

        // Reproject the corners.
        Eigen::Matrix3Xd C_corner_positions =
            T_G_C.inverse().transformVectorized(G_corner_positions);

        // Only output the succesful poses.
        if (pnp_success) {
          ++number_pnp_success;
          if (sufficient_inliers) {
            ++number_of_good_camera_poses;

            Eigen::Matrix3d rotation_matrix = T_G_C.getRotationMatrix();
            Eigen::Quaterniond quaternion(rotation_matrix);
            if (quaternion.w() < 0.0) {
              quaternion.x() = -quaternion.x();
              quaternion.y() = -quaternion.y();
              quaternion.z() = -quaternion.z();
              quaternion.w() = -quaternion.w();
            }
            Eigen::Vector3d position = T_G_C.getPosition();
            CHECK_LT(frame_id, timestamps.size());
            constexpr double kMetersToMillimeters = 1000.0;

            camera_pose_logger
                << std::setprecision(std::numeric_limits<double>::max_digits10)
                << frame_id << ", " << position.x() << ", " << position.y()
                << ", " << position.z() << ", " << quaternion.x() << ", "
                << quaternion.y() << ", " << quaternion.z() << ", "
                << quaternion.w() << std::endl;
            camera_pose_logger_timestamped
                << std::setprecision(std::numeric_limits<double>::max_digits10)
                << timestamps[frame_id] << ", " << position.x() << ", "
                << position.y() << ", " << position.z() << ", "
                << quaternion.x() << ", " << quaternion.y() << ", "
                << quaternion.z() << ", " << quaternion.w() << std::endl;

            if (FLAGS_output_position_in_mm) {
              camera_pose_logger_mm
                  << std::setprecision(
                         std::numeric_limits<double>::max_digits10)
                  << frame_id << ", " << kMetersToMillimeters * position.x()
                  << ", " << kMetersToMillimeters * position.y() << ", "
                  << kMetersToMillimeters * position.z() << ", "
                  << quaternion.x() << ", " << quaternion.y() << ", "
                  << quaternion.z() << ", " << quaternion.w() << std::endl;
              camera_pose_logger_mm_timestamped
                  << std::setprecision(
                         std::numeric_limits<double>::max_digits10)
                  << timestamps[frame_id] << ", "
                  << kMetersToMillimeters * position.x() << ", "
                  << kMetersToMillimeters * position.y() << ", "
                  << kMetersToMillimeters * position.z() << ", "
                  << quaternion.x() << ", " << quaternion.y() << ", "
                  << quaternion.z() << ", " << quaternion.w() << std::endl;
            }
          }
        }

        Eigen::Matrix2Xd keypoints_reprojected;
        std::vector<aslam::ProjectionResult> projection_result;
        camera->project3Vectorized(C_corner_positions, &keypoints_reprojected,
                                   &projection_result);

        CHECK_EQ(obs->numObservedCorners(),
                 static_cast<size_t>(keypoints_measured.cols()));
        for (size_t idx = 0u; idx < obs->numObservedCorners(); ++idx) {
          const bool pnp_inlier =
              (std::find(inliers.begin(), inliers.end(),
                         static_cast<int>(idx)) != inliers.end());
          reprojection_logger
              << frame_id << ", " << obs->getObservedCornerIds()(idx) << ", "
              << pnp_success << ", " << pnp_inlier << ", "
              << projection_result[idx].isKeypointVisible() << ", "
              << keypoints_measured(0, idx) << ", "
              << keypoints_measured(1, idx) << ", "
              << keypoints_reprojected(0, idx) << ", "
              << keypoints_reprojected(1, idx) << std::endl;
        }
      }
      ++frame_id;
    }

    LOG(INFO) << "#########################################";
    LOG(INFO) << "Total # observations:     " << frame_id;
    LOG(INFO) << "      # successful:       " << number_observation_success;
    LOG(INFO) << "      # pnp successful:   " << number_pnp_success;
    LOG(INFO) << "      # inlier count ok:  " << number_of_good_camera_poses;
    LOG(INFO) << "Written reprojected corners to:    " << kLogFilename;
    LOG(INFO) << "Written extracted camera poses to: "
              << FLAGS_camera_pose_T_C_G_output_file_timestamped;
    LOG(INFO) << "#########################################";
  }
  reprojection_logger.close();
  camera_pose_logger.close();
  camera_pose_logger_timestamped.close();
  camera_pose_logger_mm.close();
  camera_pose_logger_mm_timestamped.close();

  return 0;
}


#include <iostream>
#include <functional>
#include <Eigen/Eigen>
#include <unistd.h>

#include <pangolin/pangolin.h>
#include <SceneGraph/SceneGraph.h>
#include "BulletWrapper.h"

using namespace std;

int main( int /*argc*/, char** /*argv*/ )
{
    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateWindowAndBind("Main",640,480);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
    glewInit();

    // Scenegraph to hold GLObjects and relative transformations
    SceneGraph::GLSceneGraph glGraph;

    SceneGraph::GLLight light(10,10,-100);
    glGraph.AddChild(&light);

    SceneGraph::GLGrid grid(10,1,true);
    glGraph.AddChild(&grid);

    SceneGraph::GLCube cube;
    cube.SetPose(0,0,20,M_PI_4,M_PI_4,0.1);
    glGraph.AddChild(&cube);

    const SceneGraph::AxisAlignedBoundingBox bbox = glGraph.ObjectAndChildrenBounds();
    const Eigen::Vector3d center = bbox.Center();
    const double size = bbox.Size().norm();
    const double far = 10*size;
    const double near = far / 1E3;

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
        pangolin::ProjectionMatrix(640,480,420,420,320,240,near,far),
        pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) + size/4, center(0), center(1), center(2), pangolin::AxisZ)
    );

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
          .SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d))
          .SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));

    // Add our views as children to the base container.
    pangolin::DisplayBase().AddDisplay(view3d);

    // Physics stuff
    // Build the broadphase (approximate collision detection)
    btDbvtBroadphase broadphase;
    btDefaultCollisionConfiguration collisionConfiguration;
    btCollisionDispatcher dispatcher(&collisionConfiguration);
    btSequentialImpulseConstraintSolver solver;
    btDiscreteDynamicsWorld dynamicsWorld(&dispatcher,&broadphase,&solver,&collisionConfiguration);
    dynamicsWorld.setGravity(btVector3(0,0,-10));

    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1),0);
    btCollisionShape* fallShape = new btBoxShape(toBulletVec3( cube.ObjectBounds().HalfSizeFromOrigin() ) );

    btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)));
    btRigidBody::btRigidBodyConstructionInfo
            groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    groundRigidBody->setRestitution(0.1);
    dynamicsWorld.addRigidBody(groundRigidBody);

    SceneGraphMotionState* fallMotionState = new SceneGraphMotionState(cube);
    btScalar mass = 1;
    btVector3 fallInertia(0,0,0);
    fallShape->calculateLocalInertia(mass,fallInertia);
    btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
    btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
    fallRigidBody->setRestitution(5);
    dynamicsWorld.addRigidBody(fallRigidBody);

    // Default hooks for exiting (Esc) and fullscreen (tab).
    while( !pangolin::ShouldQuit() )
    {
        // Clear whole screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Swap frames and Process Events
        pangolin::FinishFrame();

        // Pause for 1/60th of a second.
        usleep(1E6 / 60);
        dynamicsWorld.stepSimulation(1/60.f,10);
    }

    dynamicsWorld.removeRigidBody(fallRigidBody);
    delete fallRigidBody->getMotionState();
    delete fallRigidBody;

    dynamicsWorld.removeRigidBody(groundRigidBody);
    delete groundRigidBody->getMotionState();
    delete groundRigidBody;

    delete fallShape;

    delete groundShape;

    return 0;
}


#ifndef __IRODS_HASH_HPP__
#define __IRODS_HASH_HPP__

// =-=-=-=-=-=-=-
// hash_map include
#include <boost/unordered_map.hpp>
#define HASH_TYPE boost::unordered_map

#include <string>
#include "irods_stacktrace.hpp"
#include "irods_log.hpp"

namespace irods {

    struct irods_string_hash {
        enum {
            // parameters for hash table
            bucket_size = 4, // 0 < bucket_size
            min_buckets = 8

        }; // min_buckets = 2 ^^ N, 0 < N

        size_t operator()( const std::string& s1 ) const {
            if ( s1.empty() ) {
                std::cerr << irods::stacktrace().dump();
                rodsLog(
                    LOG_NOTICE,
                    "irods_string_hash - empty string value" );
                return 0;
            }

            // hash string s1 to size_t value
            const unsigned char *p = ( const unsigned char * )s1.c_str();
            size_t hashval = 0;

            while ( *p != '\0' ) {
                hashval = 31 * hashval + ( *p++ );    // or whatever
            }

            return hashval;
        }

        bool operator()( const std::string s1, const std::string s2 ) const {
            return s1 < s2;
        }

        ~irods_string_hash() {
        }

    }; // struct irods_string_hash

}; // namespace irods

#endif // __IRODS_HASH_HPP__


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

#include "boost_python.hpp"
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include "optional.hpp"
#include <boost/version.hpp>
#include "libtorrent/time.hpp"
#include <ctime>

using namespace boost::python;

object datetime_timedelta;
object datetime_datetime;

template <typename Duration>
struct chrono_duration_to_python
{
    static PyObject* convert(Duration const& d)
    {
        std::int64_t const us = lt::total_microseconds(d);
        object result = datetime_timedelta(
            0 // days
          , us / 1000000 // seconds
          , us % 1000000 // microseconds
        );

        return incref(result.ptr());
    }
};

struct time_duration_to_python
{
    static PyObject* convert(boost::posix_time::time_duration const& d)
    {
        object result = datetime_timedelta(
            0 // days
          , 0 // seconds
          , d.total_microseconds()
        );

        return incref(result.ptr());
    }
};

template <typename Tag> struct tag {};

lt::time_point now(::tag<lt::time_point>)
{ return lt::clock_type::now(); }

lt::time_point32 now(::tag<lt::time_point32>)
{ return lt::time_point_cast<lt::seconds32>(lt::clock_type::now()); }

template <typename T>
struct time_point_to_python
{
    static PyObject* convert(T const pt)
    {
        using std::chrono::system_clock;
        using std::chrono::duration_cast;
        object result;
        if (pt > T())
        {
           time_t const tm = system_clock::to_time_t(system_clock::now()
              + duration_cast<system_clock::duration>(pt - now(::tag<T>())));

           std::tm* date = std::localtime(&tm);
           result = datetime_datetime(
              (int)1900 + date->tm_year
              // tm use 0-11 and we need 1-12
              , (int)date->tm_mon + 1
              , (int)date->tm_mday
              , date->tm_hour
              , date->tm_min
              , date->tm_sec
              );
        }
        else
        {
            result = object();
        }
        return incref(result.ptr());
    }
};

struct ptime_to_python
{
    static PyObject* convert(boost::posix_time::ptime const& pt)
    {
        boost::gregorian::date date = pt.date();
        boost::posix_time::time_duration td = pt.time_of_day();

        object result = datetime_datetime(
            (int)date.year()
          , (int)date.month()
          , (int)date.day()
          , td.hours()
          , td.minutes()
          , td.seconds()
        );

        return incref(result.ptr());
    }
};

void bind_datetime()
{
    object datetime = import("datetime").attr("__dict__");

    datetime_timedelta = datetime["timedelta"];
    datetime_datetime = datetime["datetime"];

    to_python_converter<boost::posix_time::time_duration
      , time_duration_to_python>();

    to_python_converter<boost::posix_time::ptime
      , ptime_to_python>();

    to_python_converter<lt::time_point
      , time_point_to_python<lt::time_point>>();

    to_python_converter<lt::time_point32
      , time_point_to_python<lt::time_point32>>();

    to_python_converter<lt::time_duration
      , chrono_duration_to_python<lt::time_duration>>();

    to_python_converter<lt::seconds32
      , chrono_duration_to_python<lt::seconds32>>();

    to_python_converter<std::chrono::seconds
      , chrono_duration_to_python<std::chrono::seconds>>();

    optional_to_python<boost::posix_time::ptime>();
    optional_to_python<std::time_t>();
}