// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <fstream>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/dipole_pseudo_pathway.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/sites_to_states.hpp"
#include "heom/static_fluorescence_config.hpp"
#include "heom/thermal_state_search.hpp"
#include "heom/thermal_state_search_config.hpp"
#include "heom/thermal_state_search_solver.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;
using num::complex_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::static_fluorescence_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using tss_solver_t = heom::thermal_state_search_solver<heom::ode, heom::thermal_state_search, heom::hierarchy_norm>;
		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// linear absorption pseudo pathway with ground state
		const auto& pathway_spec = heom::dipole_pseudo_pathway_wgs_spec;
		const auto sts_mode = pathway_spec.sites_to_states_mode();

		// compute thermal state
		heom::instance heom_instance_tss(heom_config, sts_mode, complete_graph);

		// OpenCL range
		ocl::nd_range range {
			{}, // offset
			{1, static_cast<std::uint64_t>(heom_instance_tss.matrices())}, // global size
			{1, 1} // local size
		};

		// create thermal state search solver (tss) from configuration and instance
		tss_solver_t tss_solver(ocl_config, range, heom_config, heom_instance_tss);

		std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
		tss_solver.write_ocl_runtime_config(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// determine thermal state until converged or max_steps is reached
		std::cout << "Computing thermal state..." << std::endl;
		int_t i = 0;
		for (; i < heom_config.thermal_state_search_max_steps(); ++i) {
			tss_solver.step_forward(1);
			// abort if convergence is reached
			if (tss_solver.test_convergence(heom_config.thermal_state_search_delta()))
				break;
		}
		if (i < heom_config.thermal_state_search_max_steps())
			std::cout << "... thermal state search converged after " << i << " steps." << std::endl;
		else
			std::cout << "... WARNING: thermal state search stopped after max_steps were reached (maybe increase max_steps, or delta)." << std::endl;

		// synchronise instance with tss solver results, i.e. transfer compute device to host memory
		tss_solver.update_instance();

		std::cout << "-------- Thermal State Search Solver Runtime Summary -------" << std::endl;
		tss_solver.write_runtime_summary(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// run the tss solver, no observations
		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		// output data structure for traces
		const size_t num_dipole_matrices = heom_config.dipole_tensor_prefactors().size();
		const int_t num_observations = iterations + 1;
		std::vector<heom::matrix_trace_observation_view> observations(num_observations);

		// for progress estimate
		const size_t solver_runs = num_dipole_matrices;
		size_t solver_run = 0;

		// iterate over all dipoles and compute population dynamics
		for (size_t tensor_index = 0; tensor_index < num_dipole_matrices; ++tensor_index) {

			// copy thermal state instance
			heom::instance heom_instance { heom_instance_tss };
			// overwrite pseudo-hamiltonian of tss instance
			heom_instance.set_hamiltonian(state_hamiltonian(heom_config.system_hamiltonian(), sts_mode));

			// OpenCL range
			ocl::nd_range range {
				{}, // offset
				{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
				{1, 1} // local size
			};

			// create a solver from configuration and instance
			solver_t solver(ocl_config, range, heom_config, heom_instance);
			std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
			solver.write_ocl_runtime_config(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;
			++solver_run; // for progress estimate

			// compute dipole matrices
			auto get_dipole_matrix = [&](size_t spec_index) {
				return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
			};
			auto dipole_matrix_minus = get_dipole_matrix(0);
			auto dipole_matrix_plus = get_dipole_matrix(1);
			dipole_matrix_minus.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));

			// multiply D+ from right
			solver.hierarchy_mmult_right(dipole_matrix_plus.data());

			// setup observer with pre-scaled D-
			heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix_minus.data()); // D- from left via observer

			// generate first observation with initial value
			observations[0] += trace_observer.observe_trace(0.0);

			// write header once
			if (tensor_index == 0)
				trace_observer.write_header(observation_file, true);

			for (int_t i = 0; i < iterations; ++i) {
				// propagate
				solver.step_forward(steps_per_iteration);

				// after propagation, we are at:
				const auto current_step = (i + 1) * steps_per_iteration;
				const real_t current_time = current_step * heom_config.solver_step_size();

				// observe
				observations[i + 1] += trace_observer.observe_trace(current_time);

				// update status
				heom::write_progress(current_step - 1, total_steps, solver_run, solver_runs, "Calculation static fluorescence: ", std::cout);
			}

			// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
			std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
			solver.write_runtime_summary(std::cout);
			std::cout << "------------------------------------------------------------" << std::endl;

		} // for tensor_index

		// write averaged traces
		for (auto& obs : observations) {
			obs.avg(num_dipole_matrices);
			observation_file << obs << '\n';
		}
		observation_file << std::flush; // tell the OS to write to disk

		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
		std::cout << "local hierarchy buffer size:            "
		          << boost::format("%11.2f MiB\n") % (heom_instance_tss.size_hierarchy_byte() / 1024.0 / 1024.0);
		std::cout << "local instance size:                    "
		          << boost::format("%11.2f MiB\n") % (heom_instance_tss.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
		std::cout << "max. OpenCL buffer allocations:         "
		          << boost::format("%11.2f MiB") % (tss_solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % tss_solver.ocl_helper().allocations_max()
		          << ')' << std::endl;
//		std::cout << "max. OpenCL buffer allocations:         "
//		          << boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
//		          << " ("
//		          << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
//		          << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "time in main():\t" << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


#include <algorithm>
#include <boost/range/irange.hpp>
#include <cmath>
#include <map>
#include <memory>
#include <set>

#include "bounding_edge.hpp"
#include "kd_tree_node.hpp"
#include "rtc_log.hpp"
#include "scoped_timer.hpp"

namespace rtc
{
kd_tree::kd_tree(const rtc::scene_model& ss) : bbox{ss.points}
{
  SCOPE_TIME_COUNTER;
  const auto& p = ss.points;
  const auto& t = ss.triangles;
  const std::uint32_t max_depth = 1.3F * std::log2(t.size()) + 8;

  auto make_bbox = [](auto& p, auto& tr) noexcept {
    return rtc::bounding_box{p[tr.vertex_a()], p[tr.vertex_b()], p[tr.vertex_c()]};
  };

  rtc::bounding_box node_bbox{p};
  std::vector<std::uint32_t> v(t.size());
  std::vector<bounding_box> primitive_bboxes;
  edge_buffer_array_t edge_buffer{
      edge_buffer_t(2 * t.size()), edge_buffer_t(2 * t.size()), edge_buffer_t(2 * t.size())};

  primitive_bboxes.reserve(t.size());

  std::for_each(t.begin(), t.end(), [&](auto& t) { primitive_bboxes.push_back(make_bbox(p, t)); });
  std::generate(v.begin(), v.end(), [i = 0]() mutable { return i++; });

  build_tree(root, node_bbox, std::move(v), primitive_bboxes, edge_buffer, max_depth);
}

auto kd_tree::cbegin(const rtc::math_ray& ray) const noexcept -> kd_tree::const_iterator
{
  if (auto range = bbox.intersection_values_for(ray))
  {
    const auto [tmin, tmax] = range.value();
    return const_iterator{ray, {root.get(), tmin, tmax}};
  }

  return const_iterator{};
}

auto kd_tree::cend(const rtc::math_ray&) const noexcept -> kd_tree::const_iterator { return const_iterator{}; }

kd_tree::~kd_tree() = default;
kd_tree::kd_tree(kd_tree&&) noexcept = default;
auto kd_tree::operator=(kd_tree&&) noexcept -> kd_tree& = default;

auto kd_tree::compute_node_split_paramters(edge_buffer_array_t& edge_buffer,
                                           const std::vector<std::uint32_t>& tr,
                                           const rtc::bounding_box& node_bbox,
                                           const std::vector<rtc::bounding_box>& primitive_bboxes)
{
  constexpr int isect_cost{80};
  constexpr int traversal_cost{1};
  constexpr rtc_float empty_bonus{0.5F};
  const auto node_diagonal{node_bbox.diagonal()};
  const rtc_float invTotalSA{1.0F / node_bbox.surface_area()};
  const rtc_float old_cost{isect_cost * static_cast<rtc_float>(tr.size())};

  int retries{0};
  int best_axis{-1};
  int best_offset{-1};
  rtc::axis axis{node_bbox.maximum_extent()};
  rtc_float best_cost{std::numeric_limits<rtc_float>::max()};

  do
  {
    const rtc::axis axis2{next(axis)}, axis3{next(axis2)};
    int below{}, above(tr.size());

    for (const auto i : boost::irange<std::size_t>(0, tr.size()))
    {
      const rtc::bounding_box& box{primitive_bboxes[tr[i]]};
      edge_buffer[int(axis)][2 * i] = {box.min_boundary().axis(axis), tr[i], bounding_edge_point::start};
      edge_buffer[int(axis)][2 * i + 1] = {box.max_boundary().axis(axis), tr[i], bounding_edge_point::end};
    }

    std::sort(&edge_buffer[int(axis)][0], &edge_buffer[int(axis)][0] + 2 * tr.size());

    for (const auto i : boost::irange<std::size_t>(0, 2 * tr.size()))
    {
      const rtc::bounding_edge_point& bp{edge_buffer[int(axis)][i]};
      const auto edge_t = bp.value;

      if (bp.type == bounding_edge_point::end)
        --above;
      if (bp.type == bounding_edge_point::start)
        ++below;

      if (edge_t > node_bbox.min_boundary().axis(axis) && edge_t < node_bbox.max_boundary().axis(axis))
      {
        const rtc_float belowSA{2 * (node_diagonal.axis(axis2) * node_diagonal.axis(axis3) +
                                     (edge_t - node_bbox.min_boundary().axis(axis)) *
                                         (node_diagonal.axis(axis2) + node_diagonal.axis(axis3)))};

        const rtc_float aboveSA{2 * (node_diagonal.axis(axis2) * node_diagonal.axis(axis3) +
                                     (node_bbox.max_boundary().axis(axis) - edge_t) *
                                         (node_diagonal.axis(axis2) + node_diagonal.axis(axis3)))};

        const rtc_float pBelow{belowSA * invTotalSA};
        const rtc_float pAbove{aboveSA * invTotalSA};
        const rtc_float eb{(above == 0 || below == 0) ? empty_bonus : 0};
        const rtc_float cost{traversal_cost + isect_cost * (1 - eb) * (pBelow * below + pAbove * above)};

        if (cost < best_cost)
        {
          best_cost = cost;
          best_axis = int(axis);
          best_offset = i;
        }
      }
    }
  } while ((best_axis == -1) && ((axis = next(axis), retries++) < 2));

  return std::make_tuple(best_axis, best_offset, best_cost, old_cost);
}

auto kd_tree::split_triangles(std::vector<std::uint32_t>&& tr_init,
                              const edge_buffer_array_t& edge_buffer,
                              const std::uint32_t best_axis,
                              const std::uint32_t best_offset)
{
  std::vector<std::uint32_t> tr{std::move(tr_init)}, left_set{}, right_set{};

  left_set.reserve(best_offset);
  right_set.reserve(2 * tr.size() - best_offset);

  for (auto i = 0; i < best_offset; ++i)
  {
    if (edge_buffer[best_axis][i].type == bounding_edge_point::start)
    {
      left_set.emplace_back(edge_buffer[best_axis][i].tr_index);
    }
  }

  for (auto i = best_offset + 1; i < 2 * tr.size(); ++i)
  {
    if (edge_buffer[best_axis][i].type == bounding_edge_point::end)
    {
      right_set.emplace_back(edge_buffer[best_axis][i].tr_index);
    }
  }
  return std::make_tuple(std::move(left_set), std::move(right_set));
}

void kd_tree::build_tree(std::unique_ptr<tree_node>& node,
                         rtc::bounding_box node_bbox,
                         std::vector<std::uint32_t> tr,
                         const std::vector<rtc::bounding_box>& primitive_bboxes,
                         edge_buffer_array_t& edge_buffer,
                         const std::uint32_t depth,
                         std::uint32_t bad_refines)
{
  const auto create_leaf_node = [&node, &tr] {
    node = std::make_unique<tree_node>();
    node->triangles = std::make_unique<kd_tree::value_type>();
    node->triangles->insert(node->triangles->end(), tr.begin(), tr.end());
  };

  if (tr.size() <= 1 || !depth)
    return create_leaf_node();

  const auto [axis, offset, cost, old_cost] =
      compute_node_split_paramters(edge_buffer, tr, node_bbox, primitive_bboxes);

  bad_refines += (cost > old_cost) ? 1 : 0;

  auto no_use_of_spliting_node = [old_cost = old_cost, cost = cost, axis = axis, &bad_refines, &tr] {
    return ((cost > 4 * old_cost) && (tr.size() < 16)) || (axis == -1) || (bad_refines == 3);
  };

  if (no_use_of_spliting_node())
    return create_leaf_node();

  auto [left_set, right_set] = split_triangles(std::move(tr), edge_buffer, axis, offset);
  bounding_box left_bbox{node_bbox}, right_bbox{node_bbox};

  node = std::make_unique<tree_node>();
  node->axis = {edge_buffer[axis][offset].value, static_cast<rtc::axis>(axis)};

  left_bbox.max_boundary().axis(node->axis.split) = right_bbox.min_boundary().axis(node->axis.split) = node->axis.value;

  build_tree(node->left, left_bbox, std::move(left_set), primitive_bboxes, edge_buffer, depth - 1, bad_refines);
  build_tree(node->right, right_bbox, std::move(right_set), primitive_bboxes, edge_buffer, depth - 1, bad_refines);
}

}  // namespace rtc


#include <Eigen/Eigen>
#include <fmt/format.h>
#include <fstream>
#include <lmng/animation.h>
#include <lmng/hierarchy.h>
#include <lmng/name.h>
#include <lmng/transform.h>
#include <lmng/yaml_save_load.h>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/zip.hpp>
#include <yaml-cpp/yaml.h>

namespace lmng
{
void animation_system::update(entt::registry &registry, float dt)
{
    for (auto [entity, animation_state] :
         registry.view<animation_state>().proxy())
    {
        update_animation(registry, entity, animation_state, dt);
    }
}

void animation_system::animate(
  entt::registry &registry,
  entt::entity skeleton_root,
  animation animation,
  float progress,
  float rate,
  anim_loop_type loop_type)
{
    registry.emplace<animation_state>(
      skeleton_root, animation, rate, progress, loop_type);
}

void animation_system::update_animation(
  entt::registry &registry,
  entt::entity entity,
  animation_state &animation_state,
  float dt)
{
    auto &animation_data = *animation_state.animation;

    if (animation_state.rate == 0)
        return;

    auto [prev_keyframe, next_keyframe] =
      get_keyframes(animation_state, animation_data);

    float delta = animation_state.rate * dt;

    float transition_period = next_keyframe->time - prev_keyframe->time;

    float transition_progress = animation_state.progress - prev_keyframe->time;

    tween_poses(
      registry,
      entity,
      *prev_keyframe->pose,
      *next_keyframe->pose,
      (transition_progress + delta) / transition_period);

    registry.replace<lmng::animation_state>(
      entity,
      advance_animation(
        animation_state, animation_data, prev_keyframe, next_keyframe, delta));
}

animation_state animation_system::advance_animation(
  animation_state const animation_state,
  animation_data const &animation_data,
  keyframe_list::iterator const &prev_keyframe,
  keyframe_list::iterator const &next_keyframe,
  float delta)
{
    lmng::animation_state new_state = animation_state;
    new_state.progress += delta;

    bool forward = !std::signbit(delta);

    float to_next = forward ? next_keyframe->time - animation_state.progress
                            : animation_state.progress - prev_keyframe->time;

    if (to_next <= 0)
    {
        bool at_end = forward
                        ? next_keyframe + 1 == animation_data.keyframes.end()
                        : prev_keyframe == animation_data.keyframes.begin();

        if (at_end)
        {
            switch (animation_state.loop_type)
            {
            case anim_loop_type::pendulum:
                new_state.rate = -animation_state.rate;
            }
        }
    }

    return new_state;
}

auto animation_system::get_keyframes(
  animation_state &animation_state,
  animation_data &animation_data)
  -> std::pair<keyframe_list::iterator, keyframe_list::iterator>
{
    if (animation_state.progress < animation_data.keyframes.front().time)
        return {
          animation_data.keyframes.begin(),
          animation_data.keyframes.begin() + 1};

    for (auto keyframe_iterator = animation_data.keyframes.begin();
         keyframe_iterator != animation_data.keyframes.end();
         ++keyframe_iterator)
    {
        if (animation_state.progress > keyframe_iterator->time)
        {
            return {keyframe_iterator, keyframe_iterator + 1};
        }
    }

    return {
      animation_data.keyframes.end() - 1, animation_data.keyframes.end() - 2};
}

void animation_system::tween_poses(
  entt::registry &registry,
  entt::entity entity,
  const pose_data &first,
  const pose_data &second,
  float distance)
{
    for (auto const &[bone_name, from, target] :
         ranges::views::zip(second.bone_names, first.targets, second.targets))
    {
        auto bone_entity = lmng::find_child(registry, entity, bone_name);
        if (bone_entity == entt::null)
        {
            throw std::runtime_error{"Entity " + bone_name + " not found"};
        }
        auto transform = registry.get<lmng::transform>(bone_entity);

        transform.position =
          from.position + distance * (target.position - from.position);

        transform.rotation = from.rotation.slerp(distance, target.rotation);

        registry.replace<lmng::transform>(bone_entity, transform);
    }
}

yaml_pose_loader::yaml_pose_loader(const std::filesystem::path &project_dir)
    : project_dir{project_dir}
{
}

std::shared_ptr<pose_data>
  yaml_pose_loader::load(asset_cache &cache, std::string const &asset_path)
{
    auto yaml = YAML::LoadFile(
      (project_dir / "assets" / (asset_path + ".lpose")).string());

    pose_data data;

    for (auto const &yaml_entity : yaml)
    {
        transform target_transform;

        auto transform_yaml = yaml_entity.second["Transform"];

        auto pos_strstr =
          std::istringstream{transform_yaml["Position"].as<std::string>()};

        pos_strstr >> target_transform.position[0] >>
          target_transform.position[1] >> target_transform.position[2];

        auto rot_strstr =
          std::istringstream{transform_yaml["Rotation"].as<std::string>()};

        rot_strstr >> target_transform.rotation.coeffs()[0] >>
          target_transform.rotation.coeffs()[1] >>
          target_transform.rotation.coeffs()[2] >>
          target_transform.rotation.coeffs()[3];

        data.bone_names.emplace_back(yaml_entity.first.as<std::string>());
        data.targets.emplace_back(target_transform);
    }

    return std::make_shared<pose_data>(std::move(data));
}

yaml_animation_loader::yaml_animation_loader(
  const std::filesystem::path &project_dir)
    : project_dir{project_dir}
{
}

std::shared_ptr<animation_data>
  yaml_animation_loader::load(asset_cache &cache, const std::string &asset_path)
{
    auto yaml = YAML::LoadFile(
      (project_dir / "assets" / (asset_path + ".lmanim")).string());

    animation_data data;

    for (auto const &keyframe_yaml : yaml["Keyframes"])
    {
        data.keyframes.emplace_back(keyframe_data{
          cache.load<lmng::pose_data>(keyframe_yaml["Pose"].as<std::string>()),
          keyframe_yaml["Time"].as<float>(),
        });
    }

    return std::make_shared<animation_data>(std::move(data));
}
} // namespace lmng


// Boost.Geometry

// Copyright (c) 2019-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_CALCULATE_POINT_ORDER_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_CALCULATE_POINT_ORDER_HPP

#include <vector>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/core/point_order.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/strategies/geographic/point_order.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

namespace detail
{

template <typename Iter, typename CalcT>
struct clean_point
{
    explicit clean_point(Iter const& iter)
        : m_iter(iter), m_azi(0), m_razi(0), m_azi_diff(0)
        , m_is_azi_valid(false), m_is_azi_diff_valid(false)
    {}

    typename boost::iterators::iterator_reference<Iter>::type ref() const
    {
        return *m_iter;
    }

    CalcT const& azimuth() const
    {
        return m_azi;
    }

    CalcT const& reverse_azimuth() const
    {
        return m_razi;
    }

    CalcT const& azimuth_difference() const
    {
        return m_azi_diff;
    }

    void set_azimuths(CalcT const& azi, CalcT const& razi)
    {
        m_azi = azi;
        m_razi = razi;
        m_is_azi_valid = true;
    }

    void set_azimuth_invalid()
    {
        m_is_azi_valid = false;
    }

    bool is_azimuth_valid() const
    {
        return m_is_azi_valid;
    }

    void set_azimuth_difference(CalcT const& diff)
    {
        m_azi_diff = diff;
        m_is_azi_diff_valid = true;
    }

    void set_azimuth_difference_invalid()
    {
        m_is_azi_diff_valid = false;
    }

    bool is_azimuth_difference_valid() const
    {
        return m_is_azi_diff_valid;
    }

private:
    Iter m_iter;
    CalcT m_azi;
    CalcT m_razi;
    CalcT m_azi_diff;
    // NOTE: these flags could be removed and replaced with some magic number
    //       assigned to the above variables, e.g. CalcT(1000).
    bool m_is_azi_valid;
    bool m_is_azi_diff_valid;
};

struct calculate_point_order_by_azimuth
{
    template <typename Ring, typename Strategy>
    static geometry::order_selector apply(Ring const& ring, Strategy const& strategy)
    {
        typedef typename boost::range_iterator<Ring const>::type iter_t;
        typedef typename Strategy::template result_type<Ring>::type calc_t;
        typedef clean_point<iter_t, calc_t> clean_point_t;
        typedef std::vector<clean_point_t> cleaned_container_t;
        typedef typename cleaned_container_t::iterator cleaned_iter_t;

        calc_t const zero = 0;
        calc_t const pi = math::pi<calc_t>();

        std::size_t const count = boost::size(ring);
        if (count < 3)
        {
            return geometry::order_undetermined;
        }

        // non-duplicated, non-spike points
        cleaned_container_t cleaned;
        cleaned.reserve(count);

        for (iter_t it = boost::begin(ring); it != boost::end(ring); ++it)
        {
            // Add point
            cleaned.push_back(clean_point_t(it));

            while (cleaned.size() >= 3)
            {
                cleaned_iter_t it0 = cleaned.end() - 3;
                cleaned_iter_t it1 = cleaned.end() - 2;
                cleaned_iter_t it2 = cleaned.end() - 1;

                calc_t diff;
                if (get_or_calculate_azimuths_difference(*it0, *it1, *it2, diff, strategy)
                    && ! math::equals(math::abs(diff), pi))
                {
                    // neither duplicate nor a spike - difference already stored
                    break;
                }
                else
                {
                    // spike detected
                    // TODO: angles have to be invalidated only if spike is detected
                    // for duplicates it'd be ok to leave them
                    it0->set_azimuth_invalid();
                    it0->set_azimuth_difference_invalid();
                    it2->set_azimuth_difference_invalid();
                    cleaned.erase(it1);
                }
            }
        }

        // filter-out duplicates and spikes at the front and back of cleaned
        cleaned_iter_t cleaned_b = cleaned.begin();
        cleaned_iter_t cleaned_e = cleaned.end();
        std::size_t cleaned_count = cleaned.size();
        bool found = false;
        do
        {
            found = false;
            while(cleaned_count >= 3)
            {
                cleaned_iter_t it0 = cleaned_e - 2;
                cleaned_iter_t it1 = cleaned_e - 1;
                cleaned_iter_t it2 = cleaned_b;
                cleaned_iter_t it3 = cleaned_b + 1;

                calc_t diff = 0;
                if (! get_or_calculate_azimuths_difference(*it0, *it1, *it2, diff, strategy)
                    || math::equals(math::abs(diff), pi))
                {
                    // spike at the back
                    // TODO: angles have to be invalidated only if spike is detected
                    // for duplicates it'd be ok to leave them
                    it0->set_azimuth_invalid();
                    it0->set_azimuth_difference_invalid();
                    it2->set_azimuth_difference_invalid();
                    --cleaned_e;
                    --cleaned_count;
                    found = true;
                }
                else if (! get_or_calculate_azimuths_difference(*it1, *it2, *it3, diff, strategy)
                         || math::equals(math::abs(diff), pi))
                {
                    // spike at the front
                    // TODO: angles have to be invalidated only if spike is detected
                    // for duplicates it'd be ok to leave them
                    it1->set_azimuth_invalid();
                    it1->set_azimuth_difference_invalid();
                    it3->set_azimuth_difference_invalid();
                    ++cleaned_b;
                    --cleaned_count;
                    found = true;
                }
                else
                {
                    break;
                }
            }
        }
        while (found);

        if (cleaned_count < 3)
        {
            return geometry::order_undetermined;
        }

        // calculate the sum of external angles
        calc_t angles_sum = zero;
        for (cleaned_iter_t it = cleaned_b; it != cleaned_e; ++it)
        {
            cleaned_iter_t it0 = (it == cleaned_b ? cleaned_e - 1 : it - 1);
            cleaned_iter_t it2 = (it == cleaned_e - 1 ? cleaned_b : it + 1);

            calc_t diff = 0;
            get_or_calculate_azimuths_difference(*it0, *it, *it2, diff, strategy);

            angles_sum += diff;
        }

#ifdef BOOST_GEOMETRY_DEBUG_POINT_ORDER
        std::cout << angles_sum  << " for " << geometry::wkt(ring) << std::endl;
#endif

        return angles_sum == zero ? geometry::order_undetermined
             : angles_sum > zero  ? geometry::clockwise
                                  : geometry::counterclockwise;
    }

private:
    template <typename Iter, typename T, typename Strategy>
    static bool get_or_calculate_azimuths_difference(clean_point<Iter, T> & p0,
                                                     clean_point<Iter, T> & p1,
                                                     clean_point<Iter, T> const& p2,
                                                     T & diff,
                                                     Strategy const& strategy)
    {
        if (p1.is_azimuth_difference_valid())
        {
            diff = p1.azimuth_difference();
            return true;
        }

        T azi1, razi1, azi2, razi2;
        if (get_or_calculate_azimuths(p0, p1, azi1, razi1, strategy)
            && get_or_calculate_azimuths(p1, p2, azi2, razi2, strategy))
        {
            diff = strategy.apply(p0.ref(), p1.ref(), p2.ref(), razi1, azi2);
            p1.set_azimuth_difference(diff);
            return true;
        }
        return false;
    }

    template <typename Iter, typename T, typename Strategy>
    static bool get_or_calculate_azimuths(clean_point<Iter, T> & p0,
                                          clean_point<Iter, T> const& p1,
                                          T & azi, T & razi,
                                          Strategy const& strategy)
    {
        if (p0.is_azimuth_valid())
        {
            azi = p0.azimuth();
            razi = p0.reverse_azimuth();
            return true;
        }

        if (strategy.apply(p0.ref(), p1.ref(), azi, razi))
        {
            p0.set_azimuths(azi, razi);
            return true;
        }

        return false;
    }
};

struct calculate_point_order_by_area
{
    template <typename Ring, typename Strategy>
    static geometry::order_selector apply(Ring const& ring, Strategy const& strategy)
    {
        typedef detail::area::ring_area
            <
                geometry::order_as_direction<geometry::point_order<Ring>::value>::value,
                geometry::closure<Ring>::value
            > ring_area_type;

        typedef typename area_result
            <
                Ring, Strategy
            >::type result_type;

        result_type const result = ring_area_type::apply(ring,
                                                         // TEMP - in the future (umbrella) strategy will be passed
                                                         geometry::strategies::area::services::strategy_converter
                                                            <
                                                                decltype(strategy.get_area_strategy())
                                                            >::get(strategy.get_area_strategy()));

        result_type const zero = 0;
        return result == zero ? geometry::order_undetermined
             : result > zero  ? geometry::clockwise
                              : geometry::counterclockwise;
    }
};

} // namespace detail

namespace dispatch
{

template
<
    typename Strategy,
    typename VersionTag = typename Strategy::version_tag
>
struct calculate_point_order
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this VersionTag.",
        VersionTag);
};

template <typename Strategy>
struct calculate_point_order<Strategy, strategy::point_order::area_tag>
    : geometry::detail::calculate_point_order_by_area
{};

template <typename Strategy>
struct calculate_point_order<Strategy, strategy::point_order::azimuth_tag>
    : geometry::detail::calculate_point_order_by_azimuth
{};

} // namespace dispatch

namespace detail
{

template <typename Ring, typename Strategy>
inline geometry::order_selector calculate_point_order(Ring const& ring, Strategy const& strategy)
{
    concepts::check<Ring>();

    return dispatch::calculate_point_order<Strategy>::apply(ring, strategy);
}

template <typename Ring>
inline geometry::order_selector calculate_point_order(Ring const& ring)
{
    typedef typename strategy::point_order::services::default_strategy
        <
            typename geometry::cs_tag<Ring>::type
        >::type strategy_type;

    concepts::check<Ring>();

    return dispatch::calculate_point_order<strategy_type>::apply(ring, strategy_type());
}

} // namespace detail

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_CALCULATE_POINT_ORDER_HPP


/* fasttext_classifier.cc                                                -*- C++ -*-
   Mathieu Marquis Bolduc, 27 fevrier 2017
   This file is part of MLDB. Copyright 2017 mldb.ai inc. All rights reserved.
*/

#include "fasttext_classifier.h"
#include "classifier_persist_impl.h"
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <functional>
#include "mldb/utils/vector_utils.h"
#include "mldb/plugins/jml/jml/registry.h"

#include "mldb/base/exc_assert.h"
#include "mldb/utils/smart_ptr_utils.h"

#include "mldb/ext/fasttext/src/fasttext.h"
#include "mldb/ext/fasttext/src/args.h"
#include "mldb/ext/fasttext/src/model.h"

using namespace std;
using namespace MLDB::DB;

namespace ML {

/*****************************************************************************/
/* DECISION_TREE                                                             */
/*****************************************************************************/

FastTest_Classifier::FastTest_Classifier()
{

}

FastTest_Classifier::
FastTest_Classifier(DB::Store_Reader & store,
              const std::shared_ptr<const Feature_Space> & fs)
{
    throw Exception("FastTest_Classifier constructor(reconst): not implemented");
}

FastTest_Classifier::
FastTest_Classifier(std::shared_ptr<const Feature_Space> feature_space,
              const Feature & predicted)
 : Classifier_Impl(feature_space, predicted)
{
    ExcAssert(Classifier_Impl::feature_space());
}

FastTest_Classifier::
~FastTest_Classifier()
{
}

void
FastTest_Classifier::
swap(FastTest_Classifier & other)
{
    Classifier_Impl::swap(other);

    std::swap(encoding, other.encoding);  ///< How the outputs are represented
    std::swap(fastText_, other.fastText_);
    std::swap(features, other.features);
    std::swap(featureMap, other.featureMap);
}

float
FastTest_Classifier::
predict(int label, const Feature_Set & infeatures,
        PredictionContext * context) const
{
    Label_Dist results = predict(infeatures, context);
    return results[label];
}

Label_Dist
FastTest_Classifier::
predict(const Feature_Set & infeatures,
        PredictionContext * context) const
{
    ExcAssert(fastText_);
    ExcAssert(fastText_->model_);
    Label_Dist results;
    results.resize(label_count());

    std::vector<int32_t> words;

    for (const auto& feature : infeatures) {
        auto it = featureMap.find(feature.first);
        if (it != featureMap.end()) {
            size_t f = (*it);
            for (int i = 0; i < feature.second; ++i)
                words.push_back(f);
        }
    }

    if (!words.empty()) {
        fasttext::Vector hidden(fastText_->args_->dim);
        fasttext::Vector output(label_count());
        std::vector<std::pair<fasttext::real,int32_t>> modelPredictions;
        fastText_->model_->predict(words, label_count(), modelPredictions, hidden, output);
        for (auto it = modelPredictions.cbegin(); it != modelPredictions.cend(); it++) {
            results[it->second] = it->first;
        }
    }

    return results;
}

Explanation
FastTest_Classifier::
explain(const Feature_Set & feature_set,
        const ML::Label & label,
        double weight,
        PredictionContext * context) const
{
    if (label < 0 || label > fastText_->output_->n_)
        throw Exception("FastTest_Classifier explain : label not in model");

    Explanation result;
    auto dims = fastText_->args_->dim;

    auto getEl = [] (const fasttext::Matrix& mat, size_t i, size_t j) -> float {
        return mat.data_[i * mat.n_ + j];
    };

    fasttext::Vector outputVector(dims);
    outputVector.zero();
    outputVector.addRow(*(fastText_->output_), label);

    for (const auto&f : feature_set) {
        auto feature = f.first;
        auto it = featureMap.find(feature);
        if (it != featureMap.end()) {
            size_t f = (*it);
            float sum = 0.0f;
            for (int dim = 0; dim < fastText_->args_->dim; ++dim) {
                sum += getEl(*fastText_->input_, f, dim)*outputVector[dim];
            }

            result.feature_weights[feature] = sum;
        }
    }

    result.bias = 0.f;

    return result;
}

bool
FastTest_Classifier::
optimization_supported() const
{
    return false;
}

bool
FastTest_Classifier::
predict_is_optimized() const
{
    return false;
}

bool
FastTest_Classifier::
optimize_impl(Optimization_Info & info)
{
    return false;
}

std::string
FastTest_Classifier::
print() const
{
    string result = "Fast Text Summary: ";
    result += std::to_string(fastText_->args_->dim) + " dims ";
    return result;
}

std::string
FastTest_Classifier::
summary() const
{
    return print();
}

std::vector<ML::Feature>
FastTest_Classifier::
all_features() const
{
   return features;
}

Output_Encoding
FastTest_Classifier::
output_encoding() const
{
    throw Exception("FastTest_Classifier output_encoding: not implemented");
    return encoding;
}

void
FastTest_Classifier::
serialize(DB::Store_Writer & store) const
{
    auto serializeFasttextMatrix = [&] (std::shared_ptr<fasttext::Matrix> matrix) {
        ExcAssert(matrix);
        store << matrix->m_;
        store << matrix->n_;
        fasttext::real* p = matrix->data_;
        for (int i = 0; i < matrix->m_*matrix->n_; ++i, p++) {
            store << *p;
            ExcAssert(!std::isnan(*p));
        }
    };

    store << class_id();
    store << compact_size_t(0);  // version
    store << compact_size_t(label_count());
    feature_space_->serialize(store, predicted_);

    size_t numFeatures = features.size();
    store << numFeatures;
    for (auto& f : features)
        serialize(store, f);

    //serialize input and output matrices
    ExcAssert(fastText_);
    serializeFasttextMatrix(fastText_->input_);
    serializeFasttextMatrix(fastText_->output_);

    size_t dims = fastText_->args_->dim;
    store << dims;

    store << encoding;
    store << compact_size_t(12345);  // end marker*/
}

void
FastTest_Classifier::
serialize(DB::Store_Writer & store, const Feature & feature) const
{
    store << DB::compact_size_t(feature.type())
          << DB::compact_size_t(feature.arg1())
          << DB::compact_size_t(feature.arg2());
}

void
FastTest_Classifier::
reconstitute(DB::Store_Reader & store, Feature & feature) const
{
    DB::compact_size_t type, arg1, arg2;
    store >> type >> arg1 >> arg2;
    feature = Feature(type, arg1, arg2);
}

void
FastTest_Classifier::
reconstitute(DB::Store_Reader & store,
             const std::shared_ptr<const Feature_Space> & feature_space)
{
    auto reconstituteFasttextMatrix = [&] () {
        int64_t m,n;

        store >> m;
        store >> n;
        auto matrix = std::make_shared<fasttext::Matrix>(m,n);
        fasttext::real* p = matrix->data_;
        for (int i = 0; i < m*n; ++i, p++) {
            store >> *p;
            ExcAssert(!isnan(*p));
        }

        return matrix;
    };

    string id;
    store >> id;

    if (id != class_id())
        throw Exception("FastTest_Classifier::reconstitute: read bad ID '"
                        + id + "'");

    compact_size_t version(store);

    switch (version) {
    case 0: {
        compact_size_t label_count(store);
        feature_space->reconstitute(store, predicted_);
        Classifier_Impl::init(feature_space, predicted_);

        size_t numFeatures;
        store >> numFeatures;
        features.resize(numFeatures);
        for (auto& f : features)
            reconstitute(store, f);

        fastText_ = make_shared<fasttext::FastText>();

        fastText_->input_ = reconstituteFasttextMatrix();
        fastText_->output_ = reconstituteFasttextMatrix();
        fastText_->args_ = make_shared<fasttext::Args>();

        fastText_->model_ = std::make_shared<fasttext::Model>(fastText_->input_, fastText_->output_, fastText_->args_, 0);

        size_t dims = 0;
        store >> dims;
        fastText_->args_->dim = dims;

        store >> encoding;
        break;
    }
    default:
        throw Exception("FastTest_Classifier: Attempt to reconstitute model of "
                        "unknown version " + ostream_format(version.size_));
    }

    compact_size_t marker(store);
    if (marker != 12345)
        throw Exception("FastTest_Classifier::reconstitute: read bad marker at end");

    for (size_t f = 0; f < features.size(); ++f) {
        featureMap[features[f]] = f;
    }
}

void
FastTest_Classifier::
reconstitute(DB::Store_Reader & store)
{
    reconstitute(store, nullptr);
}

std::string
FastTest_Classifier::
class_id() const
{
    return "FASTTEXT";
}

FastTest_Classifier *
FastTest_Classifier::
make_copy() const
{
    //as far as I could tell make_copy is only used for bagging
    throw Exception("FastTest_Classifier: bagging is not yet supported");
    return new FastTest_Classifier(*this);
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Impl, FastTest_Classifier> REGISTER("FASTTEXT");

} // file scope

} // namespace ML


#ifndef STAN_MATH_TORSTEN_PKMODEL_MODELPARAMETERS_HPP
#define STAN_MATH_TORSTEN_PKMODEL_MODELPARAMETERS_HPP

#include <stan/math/prim/fun/to_array_1d.hpp>
#include <stan/math/torsten/ev_history.hpp>
#include <stan/math/torsten/PKModel/ExtractVector.hpp>
#include <stan/math/torsten/PKModel/SearchReal.hpp>
#include <Eigen/Dense>
#include <algorithm>
#include <vector>

namespace torsten {

/**
 * The ModelParameters class defines objects that contain the parameters of
 * a model at a given time.
 */
template<typename T_time, typename T_parameters, typename T_biovar, typename T_tlag>
struct ModelParameters {
  double time_;
  std::vector<T_parameters> theta_;
  int nrow, ncol;
  std::vector<T_biovar> biovar_;
  std::vector<T_tlag> tlag_;

  ModelParameters() {}

  ModelParameters(const T_time& time,
                  const Eigen::Matrix<T_parameters, Eigen::Dynamic, Eigen::Dynamic>& K,
                  const std::vector<T_biovar>& biovar,
                  const std::vector<T_tlag>& tlag)
    : time_(stan::math::value_of(time)), theta_(K.size()), nrow(K.rows()), ncol(K.cols()), biovar_(biovar), tlag_(tlag)
  {
    Eigen::Matrix<T_parameters, -1, -1>::Map(theta_.data(), nrow, ncol) = K;
  }

  ModelParameters(const T_time& time,
                  const std::vector<T_parameters>& theta,
                  const std::vector<T_biovar>& biovar,
                  const std::vector<T_tlag>& tlag)
    : time_(stan::math::value_of(time)), theta_(theta), biovar_(biovar), tlag_(tlag) {}

  /**
   * Adds parameters. Useful for the mixed solver, where
   * we want to augment the parameters with the intial PK
   * states when calling the numerical integrator.
   */
  template <typename T>
  ModelParameters<T_time, T, T_biovar, T_tlag>
  augment(const std::vector<T>& thetaAdd) const {
    std::vector<T> theta(theta_.size());
    for (size_t i = 0; i < theta.size(); i++) theta[i] = theta_[i];
    for (size_t i = 0; i < thetaAdd.size(); i++) theta.push_back(thetaAdd[i]);
    return
      ModelParameters<T_time, T, T_biovar, T_tlag>
        (time_, theta, biovar_, tlag_);
  }

  template <typename T>
  ModelParameters<T_time, T, T_biovar, T_tlag>
  augment(const Eigen::Matrix<T, Eigen::Dynamic, 1>& thetaAdd)
  const {
    return augment(stan::math::to_array_1d(thetaAdd));
  }

  /**
   * Edit time stored in parameter object.
   */
  void time(double time) {
    time_ = time;
  }

  int CountParameters() const {
    return theta_.size();
  }

  // access functions   // FIX ME - name should be get_theta.
  double get_time() const { return time_; }
  std::vector<T_parameters> get_RealParameters(bool return_matrix) const {
    if (return_matrix) {
      auto k = get_K();
      std::vector<T_parameters> par(k.size());
      for (size_t j = 0; j < par.size(); ++j) par[j] = k(j);
      return par;
    } else {
      return theta_;
    }
  }
  std::vector<T_biovar> get_biovar() const {
    return biovar_;
  }
  std::vector<T_tlag> get_tlag() const {
    return tlag_;
  }
  Eigen::Matrix<T_parameters, Eigen::Dynamic, Eigen::Dynamic> get_K() const {
    Eigen::Matrix<T_parameters, -1, -1> res(nrow, ncol);
    res = Eigen::Matrix<T_parameters, -1, -1>::Map(theta_.data(), nrow, ncol);
    return res;
  }

  inline ModelParameters<double, double, double, double> to_value() const {
    return ModelParameters<double, double, double, double>
      (time_,
       stan::math::value_of(theta_),
       stan::math::value_of(biovar_),
       stan::math::value_of(tlag_));
  }
};

  template<typename T>
  struct is_matrix : std::false_type {};

  template<typename T, int R, int C>
  struct is_matrix<Eigen::Matrix<T, R, C> > : std::true_type {};

/**
 * The ModelParameterHistory class defines objects that contain a vector
 * of ModelParameters, along with a series of functions that operate on
 * them.
 */
template<typename T_time, typename T4_container, typename T5, typename T6>
struct ModelParameterHistory {
  using T4 = typename stan::value_type<T4_container>::type;
  using Param = std::pair<double, std::array<int, 3> >;

  static const bool has_matrix_param;

  std::vector<Param> index;
  const std::vector<T4_container>& theta_;
  const std::vector<std::vector<T5> >& biovar_;
  const std::vector<std::vector<T6> >& tlag_;

  template<typename T0>
   ModelParameterHistory(const std::vector<T0>& time,
                        const std::vector<T4_container>& theta,
                        const std::vector<std::vector<T5> >& biovar,
                        const std::vector<std::vector<T6> >& tlag) :
     ModelParameterHistory(0, time.size(), time,
                           0, theta.size(), theta,
                           0, biovar.size(), biovar,
                           0, tlag.size(), tlag)
  {}

  /*
   * For population data in form of ragged array, we need to
   * generate individual parameter history given the entire
   * population data and the location of the
   * inidividual. However, @c theta, @c biovar and @c tlag
   * could have different lengths, so for each variable we
   * need an index that points to the range that belongs to the individual.
   * Note that if all three variables are of size 1, their
   * time is set to be the first entry of the @c time vector
   */
  template<typename T0>
  ModelParameterHistory(int ibegin, int isize,
                        const std::vector<T0>& time,
                        int ibegin_theta, int isize_theta,
                        const std::vector<T4_container>& theta,
                        int ibegin_biovar, int isize_biovar,
                        const std::vector<std::vector<T5> >& biovar,
                        int ibegin_tlag, int isize_tlag,
                        const std::vector<std::vector<T6> >& tlag) :
    index(isize),
    theta_(theta),
    biovar_(biovar),
    tlag_(tlag)
  {
    for (int i = 0; i < isize; ++i) {
      int j = isize_theta   > 1 ? ibegin_theta  + i : ibegin_theta;
      int k = isize_biovar  > 1 ? ibegin_biovar + i : ibegin_biovar;
      int l = isize_tlag    > 1 ? ibegin_tlag   + i : ibegin_tlag;
      index[i] = std::make_pair<double, std::array<int, 3> >(stan::math::value_of(time[ibegin + i]), {j, k, l });
    }
    Sort();
  }

  const T4_container& model_param(int i) const {
    return theta_[index[i].second[0]];
  }

  ModelParameters<T_time, T4, T5, T6> GetModelParameters(int i) const {
    return ModelParameters<T_time, T4, T5, T6>(std::get<0>(index[i]), theta_[std::get<1>(index[i])[0]], biovar_[std::get<1>(index[i])[1]], tlag_[std::get<1>(index[i])[2]]);
  }

  /**
   * MPV.size gives us the number of events.
   * MPV[i].RealParameters.size gives us the number of
   * ODE parameters for the ith event.
   *
   * FIX ME - rename this GetValueTheta
   */
  inline const T4& GetValue(int iEvent, int iParameter) const {
    return theta_[std::get<1>(index[iEvent])[0]][iParameter];
  }

  inline const T5& GetValueBio(int iEvent, int iParameter) const {
    return biovar_[std::get<1>(index[iEvent])[1]][iParameter];
  }

  inline const T6& GetValueTlag(int iEvent, int iParameter) const {
    return tlag_[std::get<1>(index[iEvent])[2]][iParameter];
  }

  inline int get_size() const {
    return index.size();
  }

  void Sort() {
    std::sort(index.begin(), index.end(),
              [](const Param& a, const Param& b)
              { return std::get<0>(a) < std::get<0>(b); });
  }

  bool Check() {
  // check that elements are in chronological order.
    int i = index.size() - 1;
    bool ordered = true;

    while (i > 0 && ordered) {
      ordered = (std::get<0>(index[i]) >= std::get<0>(index[i-1]));
      i--;
    }
    return ordered;
  }

  /**
   * COMPLETE MODEL PARAMETERS
   *
   * Completes parameters so that it contains model parameters for each event
   * in events. If parameters contains only one set of parameters (case where
   * the parameters are constant), this set is replicated for each event in
   * events. Otherwise a new parameter vector is added for each new event
   * (isnew = true). This parameter vector is identical to the parameter vector
   * at the subsequent event. If the new event occurs at a time posterior to
   * the time of the last event, than the new vector parameter equals the
   * parameter vector of the last event. This amounts to doing an LOCF
   * (Last Observation Carried Forward):
   * Three cases:
   * (a) The time of the new event is higher than the time of the last
   *     parameter vector in parameters (k = len_parameters).
   *     Create a parameter vector at the the time of the new event,
   *     with the parameters of the last parameter vector.
   *     (Last Observation Carried Forward)
   * (b) The time of the new event matches the time of a parameter vector
   *     in parameters. This parameter vector gets replicated.
   * (c) (a) is not verified and no parameter vector occurs at the time
   *     of the new event. A new parameter vector is created at the time
   *     of the new event, and its parameters are equal to the parameters
   *     of the subsequent parameter vector in parameters.
   *
   * Since both @c index and events @c index are sorted
   * in time, we always move paramtter pointer to events
   *
   * Events and Parameters are sorted at the end of the procedure.
   *
   * @param[in] parameters at each event
   * @param[in] events elements (following NONMEM convention) at each event
   * @return - modified parameters and events.
   */
  template<typename T0, typename T_p1, typename T_p2, typename T_p3>
  void CompleteParameterHistory(torsten::EventHistory<T0, T_p1, T_p2, T_p3, T6>& events) {
    int nEvent = events.size();
    assert(nEvent > 0);
    int len_Parameters = index.size();  // numbers of events for which parameters are determined
    assert(len_Parameters > 0);

    if (!Check()) Sort();
    if (!events.Check()) events.Sort();
    index.resize(nEvent);

    int iEvent = 0;
    for (int i = 0; i < len_Parameters - 1; i++) {
      while (events.isnew(iEvent)) iEvent++;  // skip new events
      assert(std::get<0>(index[i]) == events.time(iEvent));  // compare time of "old' events to time of parameters.
      iEvent++;
    }

    if (len_Parameters == 1)  {
      for (int i = 0; i < nEvent; i++) {
        index[i] = std::make_pair<double, std::array<int, 3> >(stan::math::value_of(events.time(i)) , std::array<int,3>(std::get<1>(index[0])));
        events.index[i][3] = 0;
      }
    } else {  // parameters are event dependent.
      std::vector<double> times(nEvent, 0);
      for (int i = 0; i < nEvent; i++) times[i] = index[i].first;
      iEvent = 0;

      Param newParameter;
      int j = 0;
      std::vector<Param>::const_iterator lower = index.begin();
      std::vector<Param>::const_iterator it_param_end = index.begin() + len_Parameters;
      for (int iEvent = 0; iEvent < nEvent; ++iEvent) {
        if (events.isnew(iEvent)) {
          // Find the index corresponding to the time of the new event in the
          // times vector.
          const double t = stan::math::value_of(events.time(iEvent));
          lower = std::lower_bound(lower, it_param_end, t,
                                   [](const Param& t1, const double& t2) {return t1.first < t2;});
          newParameter = lower == (it_param_end) ? index[len_Parameters-1] : *lower;
          newParameter.first = t;
          index[len_Parameters + j] = newParameter;
          events.index[iEvent][3] = 0;
          j++;
        }
      }
    }
    Sort();
  }
};

template<typename T_time, typename T4_container, typename T5, typename T6>
const bool ModelParameterHistory<T_time, T4_container, T5, T6>::has_matrix_param = is_matrix<T4_container>::value;

}

#endif


//          Copyright Gavin Band 2008 - 2012.
// 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 METRO_rBF_HPP
#define METRO_rBF_HPP

#include <Eigen/Core>

namespace metro {
	// compute natural logarithm of rBF, the retrospective Bayes factor from
	// Stephens and Balding, Nature Genetics 2009.
	// counts should be 2xn
	double compute_rBF( Eigen::MatrixXd const& counts, double const lambda ) ;
}

#endif


#include <type_traits>
#include <utility>
#include <boost/hana.hpp>

struct Cat
{
    Cat() = delete;
    void meow();
};

struct Dog
{
    Dog() = delete;
    void bark();
};

int main()
{
    auto has_meow = boost::hana::is_valid([](auto x) -> decltype(x.meow())
        {
        });

    auto has_bark = boost::hana::is_valid([](auto x) -> decltype(x.bark())
        {
        });

    static_assert(has_meow(Cat{}));
    static_assert(has_bark(Dog{}));
}


#include <opencv2/core/eigen.hpp>
#include <Eigen/StdVector>

namespace aslam {

namespace cameras {

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T>::PinholeProjection()
    : _fu(0.0),
      _fv(0.0),
      _cu(0.0),
      _cv(0.0),
      _ru(0),
      _rv(0) {
  updateTemporaries();
}

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T>::PinholeProjection(
    const sm::PropertyTree & config)
    : _distortion(sm::PropertyTree(config, "distortion")) {
  _fu = config.getDouble("fu");
  _fv = config.getDouble("fv");
  _cu = config.getDouble("cu");
  _cv = config.getDouble("cv");
  _ru = config.getInt("ru");
  _rv = config.getInt("rv");

  updateTemporaries();
}

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T>::PinholeProjection(double focalLengthU,
                                                   double focalLengthV,
                                                   double imageCenterU,
                                                   double imageCenterV,
                                                   int resolutionU,
                                                   int resolutionV,
                                                   distortion_t distortion)
    : _fu(focalLengthU),
      _fv(focalLengthV),
      _cu(imageCenterU),
      _cv(imageCenterV),
      _ru(resolutionU),
      _rv(resolutionV),
      _distortion(distortion) {
  updateTemporaries();
}

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T>::PinholeProjection(double focalLengthU,
                                                   double focalLengthV,
                                                   double imageCenterU,
                                                   double imageCenterV,
                                                   int resolutionU,
                                                   int resolutionV)
    : _fu(focalLengthU),
      _fv(focalLengthV),
      _cu(imageCenterU),
      _cv(imageCenterV),
      _ru(resolutionU),
      _rv(resolutionV) {
  updateTemporaries();
}

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T>::~PinholeProjection() {
}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_K>
bool PinholeProjection<DISTORTION_T>::euclideanToKeypoint(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_K> & outKeypointConst) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);
  Eigen::MatrixBase<DERIVED_K> & outKeypoint = const_cast<Eigen::MatrixBase<
      DERIVED_K> &>(outKeypointConst);

  outKeypoint.derived().resize(2);
  double rz = 1.0 / p[2];
  outKeypoint[0] = p[0] * rz;
  outKeypoint[1] = p[1] * rz;

  _distortion.distort(outKeypoint);

  outKeypoint[0] = _fu * outKeypoint[0] + _cu;
  outKeypoint[1] = _fv * outKeypoint[1] + _cv;

  return isValid(outKeypoint) && p[2] > 0;
}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
bool PinholeProjection<DISTORTION_T>::euclideanToKeypoint(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_K> & outKeypointConst,
    const Eigen::MatrixBase<DERIVED_JP> & outJp) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JP>, 2, 3);

  Eigen::MatrixBase<DERIVED_K> & outKeypoint = const_cast<Eigen::MatrixBase<
      DERIVED_K> &>(outKeypointConst);
  outKeypoint.derived().resize(2);

  // Jacobian:
  Eigen::MatrixBase<DERIVED_JP> & J =
      const_cast<Eigen::MatrixBase<DERIVED_JP> &>(outJp);
  J.derived().resize(KeypointDimension, 3);
  J.setZero();

  double rz = 1.0 / p[2];
  double rz2 = rz * rz;

  outKeypoint[0] = p[0] * rz;
  outKeypoint[1] = p[1] * rz;

  Eigen::MatrixXd Jd;
  _distortion.distort(outKeypoint, Jd);  // distort and Jacobian wrt. keypoint

  // Jacobian including distortion
  J(0, 0) = _fu * Jd(0, 0) * rz;
  J(0, 1) = _fu * Jd(0, 1) * rz;
  J(0, 2) = -_fu * (p[0] * Jd(0, 0) + p[1] * Jd(0, 1)) * rz2;
  J(1, 0) = _fv * Jd(1, 0) * rz;
  J(1, 1) = _fv * Jd(1, 1) * rz;
  J(1, 2) = -_fv * (p[0] * Jd(1, 0) + p[1] * Jd(1, 1)) * rz2;

  outKeypoint[0] = _fu * outKeypoint[0] + _cu;
  outKeypoint[1] = _fv * outKeypoint[1] + _cv;

  return isValid(outKeypoint) && p[2] > 0;

}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_K>
bool PinholeProjection<DISTORTION_T>::homogeneousToKeypoint(
    const Eigen::MatrixBase<DERIVED_P> & ph,
    const Eigen::MatrixBase<DERIVED_K> & outKeypoint) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);

  // hope this works... (required to have valid static asserts)
  if (ph[3] < 0)
    return euclideanToKeypoint(-ph.derived().template head<3>(), outKeypoint);
  else
    return euclideanToKeypoint(ph.derived().template head<3>(), outKeypoint);
}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_K, typename DERIVED_JP>
bool PinholeProjection<DISTORTION_T>::homogeneousToKeypoint(
    const Eigen::MatrixBase<DERIVED_P> & ph,
    const Eigen::MatrixBase<DERIVED_K> & outKeypoint,
    const Eigen::MatrixBase<DERIVED_JP> & outJp) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JP>, 2, 4);

  Eigen::MatrixBase<DERIVED_JP> & J =
      const_cast<Eigen::MatrixBase<DERIVED_JP> &>(outJp);
  J.derived().resize(KeypointDimension, 4);
  J.setZero();

  // hope this works... (required to have valid static asserts)
  return euclideanToKeypoint(ph.derived().template head<3>(), outKeypoint,
                             J.derived().template topLeftCorner<2, 3>());

  if (ph[3] < 0) {
    bool success = euclideanToKeypoint(
        -ph.derived().template head<3>(), outKeypoint,
        J.derived().template topLeftCorner<2, 3>());
    J = -J;
    return success;
  } else {
    return euclideanToKeypoint(ph.derived().template head<3>(), outKeypoint,
                               J.derived().template topLeftCorner<2, 3>());
  }
}

template<typename DISTORTION_T>
template<typename DERIVED_K, typename DERIVED_P>
bool PinholeProjection<DISTORTION_T>::keypointToEuclidean(
    const Eigen::MatrixBase<DERIVED_K> & keypoint,
    const Eigen::MatrixBase<DERIVED_P> & outPointConst) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);

  keypoint_t kp = keypoint;

  kp[0] = (kp[0] - _cu) / _fu;
  kp[1] = (kp[1] - _cv) / _fv;
  _distortion.undistort(kp);  // revert distortion

  Eigen::MatrixBase<DERIVED_P> & outPoint = const_cast<Eigen::MatrixBase<
      DERIVED_P> &>(outPointConst);
  outPoint.derived().resize(3);

  outPoint[0] = kp[0];
  outPoint[1] = kp[1];
  outPoint[2] = 1;

  return isValid(keypoint);

}

template<typename DISTORTION_T>
template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
bool PinholeProjection<DISTORTION_T>::keypointToEuclidean(
    const Eigen::MatrixBase<DERIVED_K> & keypoint,
    const Eigen::MatrixBase<DERIVED_P> & outPointConst,
    const Eigen::MatrixBase<DERIVED_JK> & outJk) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JK>, 3, 2);

  keypoint_t kp = keypoint;

  kp[0] = (kp[0] - _cu) / _fu;
  kp[1] = (kp[1] - _cv) / _fv;

  Eigen::MatrixXd Jd(2, 2);

  _distortion.undistort(kp, Jd);  // revert distortion

  Eigen::MatrixBase<DERIVED_P> & outPoint = const_cast<Eigen::MatrixBase<
      DERIVED_P> &>(outPointConst);
  outPoint.derived().resize(3);

  outPoint[0] = kp[0];
  outPoint[1] = kp[1];
  outPoint[2] = 1;

  Eigen::MatrixBase<DERIVED_JK> & J =
      const_cast<Eigen::MatrixBase<DERIVED_JK> &>(outJk);
  J.derived().resize(3, KeypointDimension);
  J.setZero();

  //J.derived()(0,0) = 1.0;
  //J.derived()(1,1) = _fu_over_fv;

  J.derived()(0, 0) = _recip_fu;
  J.derived()(1, 1) = _recip_fv;

  J *= Jd;

  return isValid(keypoint);

}

template<typename DISTORTION_T>
template<typename DERIVED_K, typename DERIVED_P>
bool PinholeProjection<DISTORTION_T>::keypointToHomogeneous(
    const Eigen::MatrixBase<DERIVED_K> & keypoint,
    const Eigen::MatrixBase<DERIVED_P> & outPoint) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);

  Eigen::MatrixBase<DERIVED_P> & p =
      const_cast<Eigen::MatrixBase<DERIVED_P> &>(outPoint);
  p.derived().resize(4);
  p[3] = 0.0;
  return keypointToEuclidean(keypoint, p.derived().template head<3>());

}

template<typename DISTORTION_T>
template<typename DERIVED_K, typename DERIVED_P, typename DERIVED_JK>
bool PinholeProjection<DISTORTION_T>::keypointToHomogeneous(
    const Eigen::MatrixBase<DERIVED_K> & keypoint,
    const Eigen::MatrixBase<DERIVED_P> & outPoint,
    const Eigen::MatrixBase<DERIVED_JK> & outJk) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_K>, 2);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JK>, 4, 2);

  Eigen::MatrixBase<DERIVED_JK> & Jk = const_cast<Eigen::MatrixBase<DERIVED_JK> &>(outJk);

  Jk.derived().resize(4, 2);
  Jk.setZero();

  Eigen::MatrixBase<DERIVED_P> & p =
      const_cast<Eigen::MatrixBase<DERIVED_P> &>(outPoint);
  p[3] = 0.0;

  return keypointToEuclidean(keypoint, p.template head<3>(),
                             Jk.template topLeftCorner<3, 2>());

}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_JI>
void PinholeProjection<DISTORTION_T>::euclideanToKeypointIntrinsicsJacobian(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_JI> & outJi) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JI>, 2, 4);

  Eigen::MatrixBase<DERIVED_JI> & J =
      const_cast<Eigen::MatrixBase<DERIVED_JI> &>(outJi);
  J.derived().resize(KeypointDimension, 4);
  J.setZero();

  double rz = 1.0 / p[2];

  keypoint_t kp;
  kp[0] = p[0] * rz;
  kp[1] = p[1] * rz;
  _distortion.distort(kp);

  J(0, 0) = kp[0];
  J(0, 2) = 1;

  J(1, 1) = kp[1];
  J(1, 3) = 1;

}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_JD>
void PinholeProjection<DISTORTION_T>::euclideanToKeypointDistortionJacobian(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_JD> & outJd) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 3);

  double rz = 1.0 / p[2];
  keypoint_t kp;
  kp[0] = p[0] * rz;
  kp[1] = p[1] * rz;

  _distortion.distortParameterJacobian(kp, outJd);

  Eigen::MatrixBase<DERIVED_JD> & J =
      const_cast<Eigen::MatrixBase<DERIVED_JD> &>(outJd);
  J.derived().resize(KeypointDimension, _distortion.minimalDimensions());

  J.row(0) *= _fu;
  J.row(1) *= _fv;

}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_JI>
void PinholeProjection<DISTORTION_T>::homogeneousToKeypointIntrinsicsJacobian(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_JI> & outJi) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_JI>, 2, 4);

  if (p[3] < 0.0) {
    euclideanToKeypointIntrinsicsJacobian(-p.derived().template head<3>(),
                                          outJi);
  } else {
    euclideanToKeypointIntrinsicsJacobian(p.derived().template head<3>(),
                                          outJi);
  }

}

template<typename DISTORTION_T>
template<typename DERIVED_P, typename DERIVED_JD>
void PinholeProjection<DISTORTION_T>::homogeneousToKeypointDistortionJacobian(
    const Eigen::MatrixBase<DERIVED_P> & p,
    const Eigen::MatrixBase<DERIVED_JD> & outJd) const {

  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED_P>, 4);

  if (p[3] < 0.0) {
    euclideanToKeypointDistortionJacobian(-p.derived().template head<3>(),
                                          outJd);
  } else {
    euclideanToKeypointDistortionJacobian(p.derived().template head<3>(),
                                          outJd);
  }

}

template<typename DISTORTION_T>
template<class Archive>
void PinholeProjection<DISTORTION_T>::save(Archive & ar,
                                           const unsigned int /* version */) const {
  ar << BOOST_SERIALIZATION_NVP(_fu);
  ar << BOOST_SERIALIZATION_NVP(_fv);
  ar << BOOST_SERIALIZATION_NVP(_cu);
  ar << BOOST_SERIALIZATION_NVP(_cv);
  ar << BOOST_SERIALIZATION_NVP(_ru);
  ar << BOOST_SERIALIZATION_NVP(_rv);
  ar << BOOST_SERIALIZATION_NVP(_distortion);

}

template<typename DISTORTION_T>
template<class Archive>
void PinholeProjection<DISTORTION_T>::load(Archive & ar,
                                           const unsigned int version) {
  SM_ASSERT_LE(std::runtime_error, version,
               (unsigned int) CLASS_SERIALIZATION_VERSION,
               "Unsupported serialization version");

  ar >> BOOST_SERIALIZATION_NVP(_fu);
  ar >> BOOST_SERIALIZATION_NVP(_fv);
  ar >> BOOST_SERIALIZATION_NVP(_cu);
  ar >> BOOST_SERIALIZATION_NVP(_cv);
  ar >> BOOST_SERIALIZATION_NVP(_ru);
  ar >> BOOST_SERIALIZATION_NVP(_rv);
  ar >> BOOST_SERIALIZATION_NVP(_distortion);

  updateTemporaries();
}

// \brief creates a random valid keypoint.
template<typename DISTORTION_T>
Eigen::VectorXd PinholeProjection<DISTORTION_T>::createRandomKeypoint() const {
  Eigen::VectorXd out(2, 1);
  out.setRandom();
  out(0) = fabs(out(0)) * _ru;
  out(1) = fabs(out(1)) * _rv;
  return out;
}

// \brief creates a random visible point. Negative depth means random between 0 and 100 meters.
template<typename DISTORTION_T>
Eigen::Vector3d PinholeProjection<DISTORTION_T>::createRandomVisiblePoint(
    double depth) const {
  Eigen::VectorXd y = createRandomKeypoint();
  Eigen::Vector3d p;
  keypointToEuclidean(y, p);

  if (depth < 0.0) {
    depth = ((double) rand() / (double) RAND_MAX) * 100.0;
  }

  p /= p.norm();

  // Muck with the depth. This doesn't change the pointing direction.
  p *= depth;
  return p;

}

template<typename DISTORTION_T>
template<typename DERIVED_K>
bool PinholeProjection<DISTORTION_T>::isValid(
    const Eigen::MatrixBase<DERIVED_K> & keypoint) const {
  return keypoint[0] >= 0 && keypoint[1] >= 0 && keypoint[0] < (double) _ru
      && keypoint[1] < (double) _rv;

}

template<typename DISTORTION_T>
template<typename DERIVED_P>
bool PinholeProjection<DISTORTION_T>::isEuclideanVisible(
    const Eigen::MatrixBase<DERIVED_P> & p) const {
  keypoint_t k;
  return euclideanToKeypoint(p, k);

}

template<typename DISTORTION_T>
template<typename DERIVED_P>
bool PinholeProjection<DISTORTION_T>::isHomogeneousVisible(
    const Eigen::MatrixBase<DERIVED_P> & ph) const {
  keypoint_t k;
  return homogeneousToKeypoint(ph, k);
}

template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::update(const double * v) {
  _fu += v[0];
  _fv += v[1];
  _cu += v[2];
  _cv += v[3];
  _recip_fu = 1.0 / _fu;
  _recip_fv = 1.0 / _fv;
  _fu_over_fv = _fu / _fv;
}

template<typename DISTORTION_T>
int PinholeProjection<DISTORTION_T>::minimalDimensions() const {
  return IntrinsicsDimension;
}

template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::getParameters(Eigen::MatrixXd & P) const {
  P.resize(4, 1);
  P(0, 0) = _fu;
  P(1, 0) = _fv;
  P(2, 0) = _cu;
  P(3, 0) = _cv;
}

template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::setParameters(const Eigen::MatrixXd & P) {
  _fu = P(0, 0);
  _fv = P(1, 0);
  _cu = P(2, 0);
  _cv = P(3, 0);
  updateTemporaries();
}

template<typename DISTORTION_T>
Eigen::Vector2i PinholeProjection<DISTORTION_T>::parameterSize() const {
  return Eigen::Vector2i(4, 1);
}

template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::updateTemporaries() {
  _recip_fu = 1.0 / _fu;
  _recip_fv = 1.0 / _fv;
  _fu_over_fv = _fu / _fv;

}

template<typename DISTORTION_T>
bool PinholeProjection<DISTORTION_T>::isBinaryEqual(
    const PinholeProjection<DISTORTION_T> & rhs) const {
  return _fu == rhs._fu && _fv == rhs._fv && _cu == rhs._cu && _cv == rhs._cv
      && _ru == rhs._ru && _rv == rhs._rv && _recip_fu == rhs._recip_fu
      && _recip_fv == rhs._recip_fv && _fu_over_fv == rhs._fu_over_fv
      && _distortion.isBinaryEqual(rhs._distortion);
}

template<typename DISTORTION_T>
PinholeProjection<DISTORTION_T> PinholeProjection<DISTORTION_T>::getTestProjection() {
  return PinholeProjection<DISTORTION_T>(400, 400, 320, 240, 640, 480,
                                         DISTORTION_T::getTestDistortion());
}

template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::resizeIntrinsics(double scale) {
  _fu *= scale;
  _fv *= scale;
  _cu *= scale;
  _cv *= scale;
  _ru = _ru * scale;
  _rv = _rv * scale;

  updateTemporaries();
}

/// \brief Get a set of border rays
template<typename DISTORTION_T>
void PinholeProjection<DISTORTION_T>::getBorderRays(Eigen::MatrixXd & rays) {
  rays.resize(4, 8);
  keypointToHomogeneous(Eigen::Vector2d(0.0, 0.0), rays.col(0));
  keypointToHomogeneous(Eigen::Vector2d(0.0, _rv * 0.5), rays.col(1));
  keypointToHomogeneous(Eigen::Vector2d(0.0, _rv - 1.0), rays.col(2));
  keypointToHomogeneous(Eigen::Vector2d(_ru - 1.0, 0.0), rays.col(3));
  keypointToHomogeneous(Eigen::Vector2d(_ru - 1.0, _rv * 0.5), rays.col(4));
  keypointToHomogeneous(Eigen::Vector2d(_ru - 1.0, _rv - 1.0), rays.col(5));
  keypointToHomogeneous(Eigen::Vector2d(_ru * 0.5, 0.0), rays.col(6));
  keypointToHomogeneous(Eigen::Vector2d(_ru * 0.5, _rv - 1.0), rays.col(7));

}

class PinholeHelpers {
public:

  static inline double square(double x) {
    return x*x;
  }
  static inline float square(float x) {
    return x*x;
  }
  static inline double hypot(double a, double b) {
    return sqrt(square(a) + square(b));
  }

  static std::vector<cv::Point2d> intersectCircles(double x1, double y1, double r1,
                                            double x2, double y2, double r2) {
    std::vector<cv::Point2d> ipts;

    double d = hypot(x1-x2, y1-y2);
    if (d > r1 + r2) {
      // circles are separate
      return ipts;
    }
    if (d < fabs(r1 - r2)) {
      // one circle is contained within the other
      return ipts;
    }

    double a = (square(r1) - square(r2) + square(d)) / (2.0 * d);
    double h = sqrt(square(r1) - square(a));

    double x3 = x1 + a * (x2 - x1) / d;
    double y3 = y1 + a * (y2 - y1) / d;

    if (h < 1e-10) {
      // two circles touch at one point
      ipts.push_back(cv::Point2d(x3, y3));
      return ipts;
    }

    ipts.push_back(cv::Point2d(x3 + h * (y2 - y1) / d, y3 - h * (x2 - x1) / d));
    ipts.push_back(cv::Point2d(x3 - h * (y2 - y1) / d, y3 + h * (x2 - x1) / d));
    return ipts;
  }

  static void fitCircle(const std::vector<cv::Point2d>& points, double& centerX,
                 double& centerY, double& radius) {
    // D. Umbach, and K. Jones, A Few Methods for Fitting Circles to Data,
    // IEEE Transactions on Instrumentation and Measurement, 2000
    // We use the modified least squares method.
    double sum_x = 0.0;
    double sum_y = 0.0;
    double sum_xx = 0.0;
    double sum_xy = 0.0;
    double sum_yy = 0.0;
    double sum_xxx = 0.0;
    double sum_xxy = 0.0;
    double sum_xyy = 0.0;
    double sum_yyy = 0.0;

    int n = points.size();
    for (int i = 0; i < n; ++i) {
      double x = points.at(i).x;
      double y = points.at(i).y;

      sum_x += x;
      sum_y += y;
      sum_xx += x * x;
      sum_xy += x * y;
      sum_yy += y * y;
      sum_xxx += x * x * x;
      sum_xxy += x * x * y;
      sum_xyy += x * y * y;
      sum_yyy += y * y * y;
    }

    double A = n * sum_xx - square(sum_x);
    double B = n * sum_xy - sum_x * sum_y;
    double C = n * sum_yy - square(sum_y);
    double D = 0.5
        * (n * sum_xyy - sum_x * sum_yy + n * sum_xxx - sum_x * sum_xx);
    double E = 0.5
        * (n * sum_xxy - sum_y * sum_xx + n * sum_yyy - sum_y * sum_yy);

    centerX = (D * C - B * E) / (A * C - square(B));
    centerY = (A * E - B * D) / (A * C - square(B));

    double sum_r = 0.0;
    for (int i = 0; i < n; ++i) {
      double x = points.at(i).x;
      double y = points.at(i).y;
      sum_r += hypot(x - centerX, y - centerY);
    }
    radius = sum_r / n;
  }

  static double medianOfVectorElements(std::vector<double> values)
  {
    double median;
    size_t size = values.size();
    std::sort(values.begin(), values.end());
    if (size%2 == 0)
        median = (values[size / 2 - 1] + values[size / 2]) / 2;
    else
        median = values[size / 2];
    return median;
  }

}; // class PinholeHelpers

/// \brief initialize the intrinsics based on one view of a gridded calibration target
/// These functions are based on functions from Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
//this algorithm can be used with high distortion lenses
template<typename DISTORTION_T>
bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<GridCalibrationTargetObservation> &observations) {

  SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
  SM_ASSERT_TRUE(Exception, observations.size() != 0, "Need min. one observation");

  // First, initialize the image center at the center of the image.
  _cu = (observations[0].imCols() - 1.0) / 2.0;
  _cv = (observations[0].imRows() - 1.0) / 2.0;
  _ru = observations[0].imCols();
  _rv = observations[0].imRows();
  _distortion.clear();

  //process all images
  size_t nImages = observations.size();

  // Initialize focal length
  // C. Hughes, P. Denny, M. Glavin, and E. Jones,
  // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
  // Extraction, PAMI 2010
  // Find circles from rows of chessboard corners, and for each pair
  // of circles, find vanishing points: v1 and v2.
  // f = ||v1 - v2|| / PI;
  std::vector<double> f_guesses;

  for (size_t i=0; i<nImages; ++i) {
    const GridCalibrationTargetObservation& obs = observations.at(i);
    SM_ASSERT_TRUE(Exception, obs.target(), "The GridCalibrationTargetObservation has no target object");
    const GridCalibrationTargetBase & target = *obs.target();

    std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> center(target.rows());
    double radius[target.rows()];
    bool skipImage=false;

    for (size_t r=0; r<target.rows(); ++r) {
      std::vector<cv::Point2d> circle;
      for (size_t c=0; c<target.cols(); ++c) {
        Eigen::Vector2d imagePoint;
        Eigen::Vector3d gridPoint;

        if (obs.imageGridPoint(r, c, imagePoint))
          circle.push_back(cv::Point2f(imagePoint[0], imagePoint[1]));
        else
          //skip this image if the board view is not complete
          skipImage=true;
      }
      PinholeHelpers::fitCircle(circle, center[r](0), center[r](1), radius[r]);
    }

    if(skipImage)
      continue;

    for (size_t j=0; j<target.rows(); ++j)
    {
      for (size_t k=j+1; k<target.cols(); ++k)
      {
        // find distance between pair of vanishing points which
        // correspond to intersection points of 2 circles
        std::vector < cv::Point2d > ipts;
        ipts = PinholeHelpers::intersectCircles(center[j](0), center[j](1),
                                                radius[j], center[k](0), center[k](1), radius[k]);
        if (ipts.size()<2)
          continue;

         double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
         f_guesses.push_back(f_guess);
      }
    }
  }

  if(f_guesses.empty()){
    //return false;
    // https://github.com/ethz-asl/kalibr/issues/99
    // https://github.com/tu-darmstadt-ros-pkg/kalibr/commit/2822f6a6f47b92b600de42fd1e82915f1b0b2fd7
    // https://www.mouser.com/pdfdocs/Intel_D400_Series_Datasheet.pdf
    // D435 focal length is 1.93 mm... but we'll use 400 (pixels?)
    f_guesses.push_back(400.0);
  }
  //get the median of the guesses
  double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);

  //set the estimate
  _fu = f0;
  _fv = f0;
  updateTemporaries();

  return true;
}

template<typename DISTORTION_T>
size_t PinholeProjection<DISTORTION_T>::computeReprojectionError(
    const GridCalibrationTargetObservation & obs,
    const sm::kinematics::Transformation & T_target_camera,
    double & outErr) const {
  outErr = 0.0;
  size_t count = 0;
  sm::kinematics::Transformation T_camera_target = T_target_camera.inverse();

  for (size_t i = 0; i < obs.target()->size(); ++i) {
    Eigen::Vector2d y, yhat;
    if (obs.imagePoint(i, y)
        && euclideanToKeypoint(T_camera_target * obs.target()->point(i),
                               yhat)) {
      outErr += (y - yhat).norm();
      ++count;
    }
  }
  return count;
}

/// \brief estimate the transformation of the camera with respect to the calibration target
///        On success out_T_t_c is filled in with the transformation that takes points from
///        the camera frame to the target frame
/// \return true on success
template<typename DISTORTION_T>
bool PinholeProjection<DISTORTION_T>::estimateTransformation(
    const GridCalibrationTargetObservation & obs,
    sm::kinematics::Transformation & out_T_t_c) const {

  std::vector<cv::Point2f> Ms;
  std::vector<cv::Point3f> Ps;

  // Get the observed corners in the image and target frame
  obs.getCornersImageFrame(Ms);
  obs.getCornersTargetFrame(Ps);

  // Convert all target corners to a fakey pinhole view.
  size_t count = 0;
  for (size_t i = 0; i < Ms.size(); ++i) {
    Eigen::Vector3d targetPoint(Ps[i].x, Ps[i].y, Ps[i].z);
    Eigen::Vector2d imagePoint(Ms[i].x, Ms[i].y);
    Eigen::Vector3d backProjection;

    if (keypointToEuclidean(imagePoint, backProjection)
        && backProjection.normalized()[2] > std::cos(80.0*M_PI/180.0)) {
      double x = backProjection[0];
      double y = backProjection[1];
      double z = backProjection[2];
      Ps.at(count).x = targetPoint[0];
      Ps.at(count).y = targetPoint[1];
      Ps.at(count).z = targetPoint[2];

      Ms.at(count).x = x / z;
      Ms.at(count).y = y / z;
      ++count;
    }
  }

  Ps.resize(count);
  Ms.resize(count);

  std::vector<double> distCoeffs(4, 0.0);

  cv::Mat rvec(3, 1, CV_64F);
  cv::Mat tvec(3, 1, CV_64F);

  if (Ps.size() < 4)
    return false;

  // Call the OpenCV pnp function.
  cv::solvePnP(Ps, Ms, cv::Mat::eye(3, 3, CV_64F), distCoeffs, rvec, tvec);

  // convert the rvec/tvec to a transformation
  cv::Mat C_camera_model = cv::Mat::eye(3, 3, CV_64F);
  Eigen::Matrix4d T_camera_model = Eigen::Matrix4d::Identity();
  cv::Rodrigues(rvec, C_camera_model);
  for (int r = 0; r < 3; ++r) {
    T_camera_model(r, 3) = tvec.at<double>(r, 0);
    for (int c = 0; c < 3; ++c) {
      T_camera_model(r, c) = C_camera_model.at<double>(r, c);
    }
  }

  out_T_t_c.set(T_camera_model.inverse());
  return true;
}

}  // namespace cameras

}  // namespace aslam


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SDK_BENCH_STATS_MIN_HPP_INCLUDED
#define NT2_SDK_BENCH_STATS_MIN_HPP_INCLUDED

#include <boost/accumulators/statistics/min.hpp>
#include <string>

namespace nt2 { namespace bench { namespace stats
{
  /*!
    @brief Miminum statistics

    Used as a Metric parameter, min_ make the metric computes the
    minimum value of its samples
  **/
  struct min_
  {
    /// @brief Samples processing function
    template<typename Samples> static inline double evaluate(Samples const& d)
    {
      return boost::accumulators::min(d);
    }

    /// @brief Statistics display
    static inline std::string unit() { return " (min)"; }
  };
} } }

#endif


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

#include <boost/simd/swar/functions/all_reduce.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/sdk/simd/category.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( all_reduce_
                                    , boost::simd::tag::avx_
                                    , (Tag)(A0)
                                    , ((simd_ < type64_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                      ((target_< unspecified_<Tag> >))
                                    )
  {
    typedef typename Tag::type                              base;
    typedef typename dispatch::meta::call<base( A0 const&
                                              , A0 const&
                                              )>::type      result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, Tag const&) const
    {
      typedef typename dispatch::meta::as_floating<A0>::type ftype;
      dispatch::functor<base> op;

      ftype a = _mm256_permute_pd(bitwise_cast<ftype>(a0), _MM_SHUFFLE2(2,1));
      ftype b = _mm256_permute2f128_pd(a,a,1);
      result_type that = op( bitwise_cast<A0>(a)
                           , bitwise_cast<A0>(b)
                           );
      ftype pthat = _mm256_permute_pd(bitwise_cast<ftype>(that), _MM_SHUFFLE2(2,1));

      return op(that, bitwise_cast<result_type>(pthat));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( all_reduce_
                                    , boost::simd::tag::avx_
                                    , (Tag)(A0)
                                    , ((simd_ < type32_<A0>
                                              , boost::simd::tag::avx_
                                              >
                                      ))
                                      ((target_< unspecified_<Tag> >))
                                    )
  {
    typedef typename Tag::type                              base;
    typedef typename dispatch::meta::call<base( A0 const&
                                              , A0 const&
                                              )>::type      result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, Tag const&) const
    {
      typedef typename dispatch::meta::as_floating<A0>::type ftype;
      dispatch::functor<base> op;

      ftype a = _mm256_permute_ps(bitwise_cast<ftype>(a0), _MM_SHUFFLE( 0,3,2,1 ));
      ftype b = _mm256_permute2f128_ps(a,a,1);
      result_type that  = op( bitwise_cast<A0>(a)
                            , bitwise_cast<A0>(b)
                            );
      ftype pthat = _mm256_permute_ps(bitwise_cast<ftype>(that), _MM_SHUFFLE( 1,0,3,2 ));

      that  = op(that, bitwise_cast<result_type>(pthat));
      pthat = _mm256_permute_ps(bitwise_cast<ftype>(that), _MM_SHUFFLE(2,3,0,1));

      return op(that, bitwise_cast<result_type>(pthat));
    }
  };
} } }

#endif
#endif