#include <rysq/fock.hpp>

// #include "fock-impl.hpp"
// #include "fock-eval.hpp"

#include <map>

#include <boost/preprocessor/seq/for_each_product.hpp>
#include <boost/preprocessor/seq/to_tuple.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/lambda/lambda.hpp>
#include "externals/boost/numeric/ublas/storage_adaptors.hpp"

#include "externals/cxx/foreach.hpp"
#include "externals/cxx/sugar/unpack.hpp"

#include "kernel/new.hpp"
#include "fock-transform.hpp"
#include "transpose.hpp"

using namespace rysq;

typedef kernel::Eri<> kernel_type;

Fock::Fock(const Quartet<Shell> &quartet) {

    transpose_ = Transpose(quartet[0].size < quartet[1].size,
 			   quartet[2].size < quartet[3].size,
 			   quartet.bra().size() < quartet.ket().size());
    kernel_ = kernel::new_<fock::Transform>(transpose_(quartet));
    // std::cout << transpose_(quartet) << std::endl;
}

Fock::~Fock() { delete static_cast<kernel_type*>(kernel_); }

void Fock::operator()(const Quartet<Center> &centers,
		      density_set D, fock_set F,
		      const Parameters &parameters) {
    // (*pImpl)(centers, D, F, parameters);
}

template<typename T, size_t N, typename U, class F>
boost::array<T,N> make_array(U (&input)[N], F unary) {
    boost::array<T,N> array;
    std::transform(input, input + N, array.begin(), unary);
    return array;
}

template<class I, class O, class F>
void transform2(I begin,I end, O output, F binary) {
    std::transform(begin, end, output, output, binary);
}

namespace ublas =  boost::numeric::ublas;

template<typename T>
struct kernel_data {
    typedef std::pair<int,int> index_type;
    // matrix types
    typedef ublas::column_major layout;
    typedef ublas::readonly_array_adaptor<double> adaptor;
    typedef ublas::matrix<double, layout, adaptor> matrix;
    struct block : matrix {
	void swap(size_t size1, size_t size2, const double *data) {
	    matrix tmp(size1, size2, adaptor(size1*size2, data));
	    matrix::swap(tmp);
	}
	template<class F>
	void for_each(const F &f) {
	    double *__restrict__ data_ = const_cast<double*>(&data()[0]);
    	    double *__restrict__ buffer_ = const_cast<double*>(buffer);
	    size_t m = size1(), n = size2();
	    if (is_trans) {
		for (size_t j = 0; j < n; ++j) {
		    for (size_t i = 0; i < m; ++i) {
			f(*data_++, buffer_[j+i*n]);
		    }
		}
	    }
	    else {
		for (size_t i = 0; i < m*n; ++i) {
		    f(*data_++, *buffer_++);
		}
	    }
	}
	T buffer;
	bool is_trans;
        index_type index, index_;
    };
    typedef block* iterator;
    typedef const block* const_iterator;
    kernel_data() {
	end_ = data_;
	for (int i = 0; i < 4; ++i) {
	    std::fill(map_[i], map_[i] + 4, (block*)NULL);
	}
    }
    iterator begin() { return data_; }
    iterator end() { return end_; }
    block& operator[](index_type index) {
	block* &b = map_[index.first][index.second];
	if (!b) {
	    b = end_++;
	    b->index = index;
	}
	return *b;
    }
    size_t size(){return data_.size(); }
    operator boost::array<T,6>() {
	boost::array<T,6> array;
	for (size_t i = 0; i < array.size(); ++i) {
	    index_type index(index_list::at(i)[0], index_list::at(i)[1]);
	    array[i] = (*this)[index].buffer;
	}
	return array;
    }
private:
    block data_[16];
    block* map_[4][4];
    block *end_;
};

void Fock::operator()(const std::vector<Center> &centers,
		      const std::vector<Int4> &quartets,
		      density_matrix_set D, fock_matrix_set F,
		      const Parameters &parameters) {

    typedef kernel_data<const double*> density_data;
    typedef kernel_data<double*> fock_data;

    density_data density_;
    fock_data fock_;

    for (size_t k = 0; k < index_list::size(); ++k) {
	UNPACK((const size_t i, j), index_list::at(k));
	std::pair<int,int> ij(transpose_.index[i], transpose_.index[j]);
	bool trans = (ij.first > ij.second);
	if (trans) std::swap(ij.first, ij.second);

	density_data::block &density_block = density_[ij];
	fock_data::block &fock_block = fock_[ij];

	density_block.index_ = std::make_pair(i,j);
	fock_block.index_ = std::make_pair(i,j);
	density_block.is_trans = trans;
	fock_block.is_trans = trans;
    }

    std::vector<double> buffer;

    foreach (const Int4 &q, quartets) {

	UNPACK((const int i, j, k, l), q);
	Quartet<Center> C(centers[i], centers[j], centers[k], centers[l]);
	double scale = 1.0/Quartet<Shell>::symmetry(i, j, k, l);

#define BLOCK_SIZE(A,m) (A).get(i,j)->block ## m()
#define BLOCK(A) (A).get(i,j)->block(q[i] - (A).base_index[i],	\
				     q[j] - (A).base_index[j])

	size_t size = 0;
	foreach (density_data::block &block, density_) {
	    const size_t i = block.index_.first, j = block.index_.second;
	    block.swap(BLOCK_SIZE(D,1), BLOCK_SIZE(D,2), BLOCK(D));
	    size += block.data().size();
	}
	foreach (fock_data::block &block, fock_) {
	    const size_t i = block.index_.first, j = block.index_.second;
	    block.swap(BLOCK_SIZE(F,1), BLOCK_SIZE(F,2), BLOCK(F));
	    size += block.data().size();
	}

#undef BLOCK_SIZE
#undef BLOCK

	buffer.clear();
	buffer.resize(size, 0);

	size_t pos = 0;
	foreach (density_data::block &block, density_) {
	    block.buffer = &buffer[pos];
	    pos += block.data().size();
	    using namespace boost::lambda;
	    block.for_each(_2 = _1);
	}

	foreach (fock_data::block &block, fock_) {
	    block.buffer = &buffer[pos];
	    pos += block.data().size();
	}

	Parameters p = parameters;
	fock::Transform<>::Data data(density_, fock_);
	(*static_cast<kernel_type*>(kernel_))(transpose_(C), data, p);

	foreach (fock_data::block &block, fock_) {
	    fock_data::index_type &index = block.index;
	    int ij = index.first + index.second;
	    double f = ((ij == 1 || ij == 5) ? parameters.scale2 : parameters.scale);
	    using namespace boost::lambda;
	    block.for_each(_1 = _1 + f*scale*_2);
	}

    }
}

void rysq::Fock::eval(const Quartet<type> &quartet,
		      const std::vector<Int4> &quartets,
		      density_matrix_set D, fock_matrix_set F,
		      const double *eri, const Parameters &parameters) {

    foreach (const Int4 &q, quartets) {
	UNPACK((const int i, j, k, l), q);

	density_set D6;
	fock_set F6;
	size_t s = 0;

	// submatrix D_, F_;
	foreach (const size_t (&index)[2], index_list()) {
	    UNPACK((const size_t i, j), index);
	    const double *dij = D.get(i,j)->block(q[i] - D.base_index[i],
						  q[j] - D.base_index[j]);
	    double *fij = F.get(i,j)->block(q[i] - F.base_index[i],
					    q[j] - F.base_index[j]);

	    F6[s] =  fij;
	    D6[s] = const_cast<double*>(dij);
	    ++s;
	}

	Parameters p = parameters;
	p.scale = 1.0/Quartet<Shell>::symmetry(i, j, k, l);
	eval(quartet, D6, F6, eri, p);

	eri += quartet.size();
    }

}

void rysq::Fock::eval(const Quartet<type> &quartet,
		      density_set D, fock_set F,
		      const double *Eri, const Parameters &p) {
    UNPACK((type a,b,c,d), quartet);

#define _TYPES	(rysq::SP)(rysq::S)(rysq::P)(rysq::D)(rysq::F)

#define _FOCK(r, types) if (a == BOOST_PP_SEQ_ELEM(0, types) &&	\
			    b == BOOST_PP_SEQ_ELEM(1, types)) {	\
	rysq::fock::eval<BOOST_PP_SEQ_ELEM(0, types),		\
	    BOOST_PP_SEQ_ELEM(1, types)>(c, d, D, F, p.scale, Eri);	\
    } else

    BOOST_PP_SEQ_FOR_EACH_PRODUCT(_FOCK, (_TYPES)(_TYPES)) {
	std::cout <<  quartet << std::endl;
	throw std::exception();
    }

#undef _FOCK
#undef _TYPES

}


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <functional>
#include <random>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include "arc_utilities/eigen_helpers.hpp"
#include "arc_utilities/eigen_helpers_conversions.hpp"
#include "arc_utilities/pretty_print.hpp"
#include "arc_utilities/voxel_grid.hpp"
#include "arc_utilities/simple_rrt_planner.hpp"
#include "uncertainty_planning_core/simple_pid_controller.hpp"
#include "uncertainty_planning_core/simple_uncertainty_models.hpp"
#include "uncertainty_planning_core/uncertainty_contact_planning.hpp"
#include "uncertainty_planning_core/simple_robot_models.hpp"
#include "uncertainty_planning_core/simple_samplers.hpp"
#include "uncertainty_planning_core/uncertainty_planning_core.hpp"
#include "fast_kinematic_simulator/fast_kinematic_simulator.hpp"
#include "fast_kinematic_simulator/simulator_environment_builder.hpp"
#include "uncertainty_planning_examples/config_common.hpp"

#ifndef SE2_COMMON_CONFIG_HPP
#define SE2_COMMON_CONFIG_HPP

namespace se2_common_config
{
    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetDefaultOptions()
    {
        uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS options;
        options.clustering_type = uncertainty_contact_planning::CONVEX_REGION_SIGNATURE;
        options.planner_time_limit = 120.0;
        options.goal_bias = 0.1;
        options.step_size = 15.0 * 0.125;
        options.step_duration = 10.0;
        options.goal_probability_threshold = 0.51;
        options.goal_distance_threshold = 2.0 * 0.125;
        options.connect_after_first_solution = 0.0;
        options.signature_matching_threshold = 0.75;
        options.distance_clustering_threshold = 15.0 * 0.125;
        options.feasibility_alpha = 0.75;
        options.variance_alpha = 0.75;
        options.edge_attempt_count = 50u;
        options.num_particles = 24u;
        options.use_contact = true;
        options.use_reverse = true;
        options.use_spur_actions = true;
        options.max_exec_actions = 1000u;
        options.max_policy_exec_time = 300.0;
        options.num_policy_simulations = 10u;
        options.num_policy_executions = 0u;
        options.policy_action_attempt_count = 100u;
        options.debug_level = 0;
        options.planner_log_file = "/tmp/se2_planner_log.txt";
        options.policy_log_file = "/tmp/se2_policy_log.txt";
        options.planned_policy_file = "/tmp/se2_planned_policy.policy";
        options.executed_policy_file = "/dev/null";
        return options;
    }

    inline config_common::TASK_CONFIG_PARAMS GetDefaultExtraOptions()
    {
        return config_common::TASK_CONFIG_PARAMS(0.125, 10.0, 0.0, 0.125, "se2_maze");
    }

    inline config_common::TASK_CONFIG_PARAMS GetExtraOptions()
    {
        return config_common::GetOptions(GetDefaultExtraOptions());
    }

    inline uncertainty_planning_core::PLANNING_AND_EXECUTION_OPTIONS GetOptions()
    {
        return uncertainty_planning_core::GetOptions(GetDefaultOptions());
    }

    inline simple_robot_models::SE2_ROBOT_CONFIG GetDefaultRobotConfig(const config_common::TASK_CONFIG_PARAMS& options)
    {
        const double kp = 0.5;//1.0; //0.1;
        const double ki = 0.0;
        const double kd = 0.0; //0.01;
        const double i_clamp = 0.0;
        const double velocity_limit = 1.0;
        const double angular_velocity_limit = velocity_limit * 0.125;
        const double max_sensor_noise = options.sensor_error;
        const double max_angular_sensor_noise = max_sensor_noise * 0.125;
        const double max_actuator_noise = options.actuator_error;
        const double max_angular_actuator_noise = max_actuator_noise * 0.125;
        const simple_robot_models::SE2_ROBOT_CONFIG robot_config(kp, ki, kd, i_clamp, velocity_limit, max_sensor_noise, max_actuator_noise, kp, ki, kd, i_clamp, angular_velocity_limit, max_angular_sensor_noise, max_angular_actuator_noise);
        return robot_config;
    }

    inline Eigen::Matrix<double, 3, 1> MakeConfig(const double x, const double y, const double zr)
    {
        Eigen::Matrix<double, 3, 1> config;
        config << x, y, zr;
        return config;
    }

    inline std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Matrix<double, 3, 1>> GetStartAndGoal()
    {
        // Define the goals of the plan
        const Eigen::Matrix<double, 3, 1> start = MakeConfig(7.75, 7.75, 0.0);
        const Eigen::Matrix<double, 3, 1> goal = MakeConfig(0.75, 0.75, 0.0);
        return std::make_pair(start, goal);
    }

    inline std::shared_ptr<EigenHelpers::VectorVector4d> GetRobotPoints()
    {
        std::shared_ptr<EigenHelpers::VectorVector4d> robot_points(new EigenHelpers::VectorVector4d());
        const std::vector<double> x_pos = {-0.1875, -0.0625, 0.0625, 0.1875, 0.3125, 0.4375, 0.5625, 0.6875, 0.8125, 0.9375, 1.0625, 1.1875, 1.3125, 1.4375};
        const std::vector<double> y_pos = {-0.1875, -0.0625, 0.0625, 0.1875, 0.3125, 0.4375, 0.5625, 0.6875, 0.8125, 0.9375, 1.0625, 1.1875, 1.3125, 1.4375};
        const std::vector<double> z_pos = {-0.4375, -0.3125, -0.1875, -0.0625, 0.0625, 0.1875, 0.3125, 0.4375};
        for (size_t xpdx = 0; xpdx < x_pos.size(); xpdx++)
        {
            for (size_t ypdx = 0; ypdx < y_pos.size(); ypdx++)
            {
                if (xpdx <= 3 || ypdx <= 3)
                {
                    for (size_t zpdx = 0; zpdx < z_pos.size(); zpdx++)
                    {
                        robot_points->push_back(Eigen::Vector4d(x_pos[xpdx], y_pos[ypdx], z_pos[zpdx], 1.0));
                    }
                }
            }
        }
        return robot_points;
    }

    inline simple_robot_models::SimpleSE2Robot GetRobot(const simple_robot_models::SE2_ROBOT_CONFIG& robot_config)
    {
        // Make the actual robot
        const Eigen::Matrix<double, 3, 1> initial_config = Eigen::Matrix<double, 3, 1>::Zero();
        const simple_robot_models::SimpleSE2Robot robot(GetRobotPoints(), initial_config, robot_config);
        return robot;
    }

    inline uncertainty_planning_core::SE2SamplerPtr GetSampler()
    {
        const double env_resolution = 0.125;
        const double env_min_x = 0.0 + (env_resolution);
        const double env_max_x = 10.0 - (env_resolution);
        const double env_min_y = 0.0 + (env_resolution);
        const double env_max_y = 10.0 - (env_resolution);
        // Make the sampler
        return uncertainty_planning_core::SE2SamplerPtr(new simple_samplers::SimpleSE2BaseSampler<uncertainty_planning_core::PRNG>(std::pair<double, double>(env_min_x, env_max_x), std::pair<double, double>(env_min_y, env_max_y)));
    }

    inline uncertainty_planning_core::SE2SimulatorPtr GetSimulator(const config_common::TASK_CONFIG_PARAMS& options, const int32_t debug_level)
    {
        const int32_t real_debug_level = std::max(0, debug_level - 10);
        const simulator_environment_builder::EnvironmentComponents environment_components = simulator_environment_builder::BuildCompleteEnvironment(options.environment_id, options.environment_resolution);
        const fast_kinematic_simulator::SolverParameters solver_params = fast_kinematic_simulator::GetDefaultSolverParameters();
        return fast_kinematic_simulator::MakeSE2Simulator(environment_components.GetEnvironment(), environment_components.GetEnvironmentSDF(), environment_components.GetSurfaceNormalsGrid(), solver_params, options.simulation_controller_frequency, real_debug_level);
    }
}

#endif // SE2_COMMON_CONFIG_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFER_POLICIES_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFER_POLICIES_HPP

#include <cstddef>

#include <boost/range.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/extensions/strategies/buffer_side.hpp>

#include <boost/geometry/algorithms/detail/overlay/backtrack_check_si.hpp>
#include <boost/geometry/algorithms/detail/overlay/calculate_distance_policy.hpp>
#include <boost/geometry/algorithms/detail/overlay/turn_info.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

enum intersection_location_type
{
    location_ok, inside_buffer, inside_original
};

class backtrack_for_buffer
{
public :
    typedef detail::overlay::backtrack_state state_type;

    template <typename Operation, typename Rings, typename Turns, typename Geometry>
    static inline void apply(std::size_t size_at_start,
                Rings& rings, typename boost::range_value<Rings>::type& ring,
                Turns& turns, Operation& operation,
                std::string const& reason,
                Geometry const& ,
                Geometry const& ,
                state_type& state
                )
    {
#if defined(BOOST_GEOMETRY_COUNT_BACKTRACK_WARNINGS)
extern int g_backtrack_warning_count;
g_backtrack_warning_count++;
#endif
//std::cout << "!";
//std::cout << "WARNING " << reason << std::endl;

        // TODO this is a copy of dissolve, check this for buffer
        state.m_good = false;

        // Make bad output clean
        rings.resize(size_at_start);
        ring.clear();

        // Reject this as a starting point
        operation.visited.set_rejected();

        // And clear all visit info
        clear_visit_info(turns);
    }
};

struct turn_assign_for_buffer
{
    static bool const include_no_turn = false;
    static bool const include_degenerate = false;
    static bool const include_opposite = true;

    template <typename Point1, typename Point2, typename Turn, typename IntersectionInfo, typename DirInfo>
    static inline void apply(Turn& turn, Point1 const& p1, Point2 const& p2, IntersectionInfo const& intersection_info, DirInfo const& dir_info)
    {
        detail::overlay::calculate_distance_policy::apply(turn, p1, p2,
                        intersection_info, dir_info);
        if (dir_info.opposite && intersection_info.count == 2)
        {
            turn.is_opposite = true;
        }
    }
};

// Should follow traversal-turn-concept (enrichment, visit structure)
// and adds index in piece vector to find it back
template <typename Point>
struct buffer_turn_operation : public detail::overlay::traversal_turn_operation<Point>
{
    int piece_index;
    bool include_in_occupation_map;

    inline buffer_turn_operation()
        : piece_index(-1)
        , include_in_occupation_map(false)
    {}
};

// Version for buffer including type of location, is_opposite, and helper variables
template <typename Point>
struct buffer_turn_info : public detail::overlay::turn_info<Point, buffer_turn_operation<Point> >
{
    bool is_opposite;

    intersection_location_type location;

    int priority;
    int count_within, count_on_helper, count_on_offsetted, count_on_corner;
    int count_on_occupied;
    int count_on_multi;
#if defined(BOOST_GEOMETRY_COUNT_DOUBLE_UU)
    int count_on_uu;
#endif

    std::set<int> piece_indices_to_skip;

#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
    std::string debug_string;
#endif

    inline buffer_turn_info()
        : is_opposite(false)
        , location(location_ok)
        , priority(0)
        , count_within(0)
        , count_on_helper(0)
        , count_on_offsetted(0)
        , count_on_corner(0)
        , count_on_occupied(0)
        , count_on_multi(0)
#if defined(BOOST_GEOMETRY_COUNT_DOUBLE_UU)
        , count_on_uu(0)
#endif
    {}
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

class si
{
private :
    segment_identifier m_id;

public :
    inline si(segment_identifier const& id)
        : m_id(id)
    {}

    template <typename Char, typename Traits>
    inline friend std::basic_ostream<Char, Traits>& operator<<(
            std::basic_ostream<Char, Traits>& os,
            si const& s)
    {
        os << s.m_id.multi_index << "." << s.m_id.segment_index;
        return os;
    }
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFER_POLICIES_HPP


#include <cstdint>
#include <thread>
#include <random>
#include <boost/signals2.hpp>

inline bool IsPrime(uint64_t number)
{
	if (((!(number & 1)) && number != 2) || (number < 2) || (number % 3 == 0 && number != 3))
	{
		return false;
	}

	for (uint64_t k = 1; 36*k*k-12*k < number; ++k)
	{
		if ((number % (6*k+1) == 0) || (number % (6*k-1) == 0))
		{
			return false;
		}
	}

	return true;
}

class ThreadStart
{
	public:
		explicit ThreadStart()
		{
		}

		virtual ~ThreadStart()
		{
		}

	public:
		void doWork(long long number)
		{

		}
};

const size_t n = 100000;

const size_t max = std::numeric_limits<uint64_t>::max();
const size_t min = max / 2;

std::random_device rd;   // non-deterministic generator
std::mt19937 gen(rd());  // to seed mersenne twister.
std::uniform_int_distribution<> dist(min, max); // distribute results between 1 and 10 inclusive.

 std::vector<uint64_t>	&& createLargeVector()
 {
	return dist(gen);
 }

int test6_threads()
{

	// Typedefs for beautiful code
	typedef boost::signals2::signal<bool(uint64_t)>	registration_manager;
	typedef registration_manager::slot_type			registration_request;

	// Our commander
	registration_manager signalHandler;

	// Create work for threads
	size_t numThreads = std::thread::hardware_concurrency();
	std::vector<

	// Create n-threads and connect them

	std::thread t(&threadstart, 20);
	t.join();

	return 0;
}


#include "drake/manipulation/kinova_jaco/jaco_status_sender.h"

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"

namespace drake {
namespace manipulation {
namespace kinova_jaco {
namespace {

using Eigen::VectorXd;

class JacoStatusSenderTest : public testing::Test {
 public:
  JacoStatusSenderTest()
      : dut_(),
        context_ptr_(dut_.CreateDefaultContext()),
        context_(*context_ptr_) {}

  const lcmt_jaco_status& output() const {
    const lcmt_jaco_status& result =
        dut_.get_output_port().Eval<lcmt_jaco_status>(context_);
    DRAKE_DEMAND(result.num_joints == kJacoDefaultArmNumJoints);
    DRAKE_DEMAND(result.joint_position.size() == kJacoDefaultArmNumJoints);
    DRAKE_DEMAND(result.joint_velocity.size() == kJacoDefaultArmNumJoints);
    DRAKE_DEMAND(result.joint_torque.size() == kJacoDefaultArmNumJoints);
    DRAKE_DEMAND(
        result.joint_torque_external.size() == kJacoDefaultArmNumJoints);
    DRAKE_DEMAND(result.num_fingers == kJacoDefaultArmNumFingers);
    DRAKE_DEMAND(result.finger_position.size() == kJacoDefaultArmNumFingers);
    DRAKE_DEMAND(result.finger_velocity.size() == kJacoDefaultArmNumFingers);
    DRAKE_DEMAND(result.finger_torque.size() == kJacoDefaultArmNumFingers);
    DRAKE_DEMAND(
        result.finger_torque_external.size() == kJacoDefaultArmNumFingers);
    return result;
  }

  static std::vector<double> as_vector(const Eigen::VectorXd& v) {
    return {v.data(), v.data() + v.size()};
  }

  void Fix(const systems::InputPort<double>& port, const Eigen::VectorXd& v) {
    port.FixValue(&context_, v);
  }

 protected:
  JacoStatusSender dut_;
  std::unique_ptr<systems::Context<double>> context_ptr_;
  systems::Context<double>& context_;
};

TEST_F(JacoStatusSenderTest, AcceptanceTest) {
  constexpr int total_dof =
      kJacoDefaultArmNumJoints + kJacoDefaultArmNumFingers;

  const VectorXd state = VectorXd::LinSpaced(total_dof * 2, 0.0, 1.0);
  const VectorXd torque = VectorXd::LinSpaced(total_dof, 2.0, 3.0);
  const VectorXd torque_external = VectorXd::LinSpaced(total_dof, 4.0, 5.0);
  const VectorXd current = VectorXd::LinSpaced(total_dof, 6.0, 7.0);

  // Fix only the required inputs ...
  Fix(dut_.get_state_input_port(), state);

  // ... so that some outputs have passthrough values ...
  EXPECT_EQ(output().joint_position,
            as_vector(state.head(kJacoDefaultArmNumJoints)));
  EXPECT_EQ(output().joint_velocity,
            as_vector(state.segment(total_dof, kJacoDefaultArmNumJoints) / 2));
  EXPECT_EQ(output().finger_position,
            as_vector(
                state.segment(kJacoDefaultArmNumJoints,
                              kJacoDefaultArmNumFingers) * kFingerUrdfToSdk));
  EXPECT_EQ(output().finger_velocity,
            as_vector(
                state.tail(kJacoDefaultArmNumFingers) * kFingerUrdfToSdk));
  // ... and some outputs have default values.
  EXPECT_EQ(output().joint_torque,
            std::vector<double>(kJacoDefaultArmNumJoints, 0.0));
  EXPECT_EQ(output().joint_torque_external,
            std::vector<double>(kJacoDefaultArmNumJoints, 0.0));
  EXPECT_EQ(output().joint_current,
            std::vector<double>(kJacoDefaultArmNumJoints, 0.0));
  EXPECT_EQ(output().finger_torque,
            std::vector<double>(kJacoDefaultArmNumFingers, 0.0));
  EXPECT_EQ(output().finger_torque_external,
            std::vector<double>(kJacoDefaultArmNumFingers, 0.0));
  EXPECT_EQ(output().finger_current,
            std::vector<double>(kJacoDefaultArmNumFingers, 0.0));

  // Fix all of the inputs ...
  Fix(dut_.get_torque_input_port(), torque);
  Fix(dut_.get_torque_external_input_port(), torque_external);
  Fix(dut_.get_current_input_port(), current);

  // ... so all ouputs have values.
  EXPECT_EQ(output().joint_torque,
            as_vector(torque.head(kJacoDefaultArmNumJoints)));
  EXPECT_EQ(output().joint_torque_external,
            as_vector(torque_external.head(kJacoDefaultArmNumJoints)));
  EXPECT_EQ(output().joint_current,
            as_vector(current.head(kJacoDefaultArmNumJoints)));
  EXPECT_EQ(output().finger_torque,
            as_vector(torque.tail(kJacoDefaultArmNumFingers)));
  EXPECT_EQ(output().finger_torque_external,
            as_vector(torque_external.tail(kJacoDefaultArmNumFingers)));
  EXPECT_EQ(output().finger_current,
            as_vector(current.tail(kJacoDefaultArmNumFingers)));
}

}  // namespace
}  // namespace kinova_jaco
}  // namespace manipulation
}  // namespace drake


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018, 2019.
// Modifications copyright (c) 2017-2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose:  Implementation of the aitoff (Aitoff) and wintri (Winkel Tripel)
//           projections.
// Author:   Gerald Evenden (1995)
//           Drazen Tutic, Lovro Gradiser (2015) - add inverse
//           Thomas Knudsen (2016) - revise/add regression tests
// Copyright (c) 1995, Gerald Evenden

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aitoff
    {
            enum mode_type {
                mode_aitoff = 0,
                mode_winkel_tripel = 1
            };

            template <typename T>
            struct par_aitoff
            {
                T    cosphi1;
                mode_type mode;
            };

            template <typename T, typename Parameters>
            struct base_aitoff_spheroid
            {
                par_aitoff<T> m_proj_parm;

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(Parameters const& , T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T c, d;

                    if((d = acos(cos(lp_lat) * cos(c = 0.5 * lp_lon)))) {/* basic Aitoff */
                        xy_x = 2. * d * cos(lp_lat) * sin(c) * (xy_y = 1. / sin(d));
                        xy_y *= d * sin(lp_lat);
                    } else
                        xy_x = xy_y = 0.;
                    if (this->m_proj_parm.mode == mode_winkel_tripel) { /* Winkel Tripel */
                        xy_x = (xy_x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                        xy_y = (xy_y + lp_lat) * 0.5;
                    }
                }
                /***********************************************************************************
                *
                * Inverse functions added by Drazen Tutic and Lovro Gradiser based on paper:
                *
                * I.Özbug Biklirici and Cengizhan Ipbüker. A General Algorithm for the Inverse
                * Transformation of Map Projections Using Jacobian Matrices. In Proceedings of the
                * Third International Symposium Mathematical & Computational Applications,
                * pages 175{182, Turkey, September 2002.
                *
                * Expected accuracy is defined by epsilon = 1e-12. Should be appropriate for
                * most applications of Aitoff and Winkel Tripel projections.
                *
                * Longitudes of 180W and 180E can be mixed in solution obtained.
                *
                * Inverse for Aitoff projection in poles is undefined, longitude value of 0 is assumed.
                *
                * Contact : dtutic@geof.hr
                * Date: 2015-02-16
                *
                ************************************************************************************/

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(Parameters const& , T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T pi = detail::pi<T>();
                    static const T two_pi = detail::two_pi<T>();
                    static const T epsilon = 1e-12;

                    int iter, max_iter = 10, round = 0, max_round = 20;
                    T D, C, f1, f2, f1p, f1l, f2p, f2l, dp, dl, sl, sp, cp, cl, x, y;

                    if ((fabs(xy_x) < epsilon) && (fabs(xy_y) < epsilon )) {
                        lp_lat = 0.; lp_lon = 0.;
                        return;
                    }

                    /* intial values for Newton-Raphson method */
                    lp_lat = xy_y; lp_lon = xy_x;
                    do {
                        iter = 0;
                        do {
                            sl = sin(lp_lon * 0.5); cl = cos(lp_lon * 0.5);
                            sp = sin(lp_lat); cp = cos(lp_lat);
                            D = cp * cl;
                            C = 1. - D * D;
                            D = acos(D) / math::pow(C, T(1.5));
                            f1 = 2. * D * C * cp * sl;
                            f2 = D * C * sp;
                            f1p = 2.* (sl * cl * sp * cp / C - D * sp * sl);
                            f1l = cp * cp * sl * sl / C + D * cp * cl * sp * sp;
                            f2p = sp * sp * cl / C + D * sl * sl * cp;
                            f2l = 0.5 * (sp * cp * sl / C - D * sp * cp * cp * sl * cl);
                            if (this->m_proj_parm.mode == mode_winkel_tripel) { /* Winkel Tripel */
                                f1 = 0.5 * (f1 + lp_lon * this->m_proj_parm.cosphi1);
                                f2 = 0.5 * (f2 + lp_lat);
                                f1p *= 0.5;
                                f1l = 0.5 * (f1l + this->m_proj_parm.cosphi1);
                                f2p = 0.5 * (f2p + 1.);
                                f2l *= 0.5;
                            }
                            f1 -= xy_x; f2 -= xy_y;
                            dl = (f2 * f1p - f1 * f2p) / (dp = f1p * f2l - f2p * f1l);
                            dp = (f1 * f2l - f2 * f1l) / dp;
                            dl = fmod(dl, pi); /* set to interval [-M_PI, M_PI] */
                            lp_lat -= dp;    lp_lon -= dl;
                        } while ((fabs(dp) > epsilon || fabs(dl) > epsilon) && (iter++ < max_iter));
                        if (lp_lat > two_pi) lp_lat -= 2.*(lp_lat-two_pi); /* correct if symmetrical solution for Aitoff */
                        if (lp_lat < -two_pi) lp_lat -= 2.*(lp_lat+two_pi); /* correct if symmetrical solution for Aitoff */
                        if ((fabs(fabs(lp_lat) - two_pi) < epsilon) && (!this->m_proj_parm.mode)) lp_lon = 0.; /* if pole in Aitoff, return longitude of 0 */

                        /* calculate x,y coordinates with solution obtained */
                        if((D = acos(cos(lp_lat) * cos(C = 0.5 * lp_lon))) != 0.0) {/* Aitoff */
                            x = 2. * D * cos(lp_lat) * sin(C) * (y = 1. / sin(D));
                            y *= D * sin(lp_lat);
                        } else
                            x = y = 0.;
                        if (this->m_proj_parm.mode == mode_winkel_tripel) { /* Winkel Tripel */
                            x = (x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                            y = (y + lp_lat) * 0.5;
                        }
                    /* if too far from given values of x,y, repeat with better approximation of phi,lam */
                    } while (((fabs(xy_x-x) > epsilon) || (fabs(xy_y-y) > epsilon)) && (round++ < max_round));

                    if (iter == max_iter && round == max_round)
                    {
                        BOOST_THROW_EXCEPTION( projection_exception(error_non_convergent) );
                        //fprintf(stderr, "Warning: Accuracy of 1e-12 not reached. Last increments: dlat=%e and dlon=%e\n", dp, dl);
                    }
                }

                static inline std::string get_name()
                {
                    return "aitoff_spheroid";
                }

            };

            template <typename Parameters>
            inline void setup(Parameters& par)
            {
                par.es = 0.;
            }

            // Aitoff
            template <typename Parameters, typename T>
            inline void setup_aitoff(Parameters& par, par_aitoff<T>& proj_parm)
            {
                proj_parm.mode = mode_aitoff;
                setup(par);
            }

            // Winkel Tripel
            template <typename Params, typename Parameters, typename T>
            inline void setup_wintri(Params& params, Parameters& par, par_aitoff<T>& proj_parm)
            {
                static const T two_div_pi = detail::two_div_pi<T>();

                T phi1;

                proj_parm.mode = mode_winkel_tripel;
                if (pj_param_r<srs::spar::lat_1>(params, "lat_1", srs::dpar::lat_1, phi1)) {
                    if ((proj_parm.cosphi1 = cos(phi1)) == 0.)
                        BOOST_THROW_EXCEPTION( projection_exception(error_lat_larger_than_90) );
                } else /* 50d28' or phi1=acos(2/pi) */
                    proj_parm.cosphi1 = two_div_pi;
                setup(par);
            }

    }} // namespace detail::aitoff
    #endif // doxygen

    /*!
        \brief Aitoff projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_aitoff.gif
    */
    template <typename T, typename Parameters>
    struct aitoff_spheroid : public detail::aitoff::base_aitoff_spheroid<T, Parameters>
    {
        template <typename Params>
        inline aitoff_spheroid(Params const& , Parameters & par)
        {
            detail::aitoff::setup_aitoff(par, this->m_proj_parm);
        }
    };

    /*!
        \brief Winkel Tripel projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_wintri.gif
    */
    template <typename T, typename Parameters>
    struct wintri_spheroid : public detail::aitoff::base_aitoff_spheroid<T, Parameters>
    {
        template <typename Params>
        inline wintri_spheroid(Params const& params, Parameters & par)
        {
            detail::aitoff::setup_wintri(params, par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_aitoff, aitoff_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_wintri, wintri_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(aitoff_entry, aitoff_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(wintri_entry, wintri_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(aitoff_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(aitoff, aitoff_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(wintri, wintri_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP


#include <exchange/market_state.hpp>
#include <boost/math/special_functions/relative_difference.hpp>

namespace eosio {

   market_state::market_state( account_name this_contract, symbol_type market_symbol, exchange_accounts& acnts )
   :marketid( market_symbol.name() ),
    market_table( this_contract, marketid ),
    base_margins( this_contract,  (marketid<<4) + 1),
    quote_margins( this_contract, (marketid<<4) + 2),
    base_loans( this_contract,    (marketid<<4) + 1),
    quote_loans( this_contract,   (marketid<<4) + 2),
    _accounts(acnts),
    market_state_itr( market_table.find(marketid) )
   {
      eosio_assert( market_state_itr != market_table.end(), "unknown market" );
      exstate = *market_state_itr;
   }

   void market_state::margin_call( extended_symbol debt_type ) {
      if( debt_type == exstate.base.balance.get_extended_symbol() )
         margin_call( exstate.base, base_margins );
      else
         margin_call( exstate.quote, quote_margins );
   }

   void market_state::margin_call( exchange_state::connector& c, margins& marginstable ) {
      auto price_idx = marginstable.get_index<N(callprice)>();
      auto pos = price_idx.begin();
      if( pos == price_idx.end() )
         return;

      auto receipt = exstate.convert( pos->collateral, pos->borrowed.get_extended_symbol() );
      eosio_assert( receipt.amount >= pos->borrowed.amount, "programmer error: insufficient collateral to cover" );/// VERY BAD, SHOULD NOT HAPPEN
      auto change_debt = receipt - pos->borrowed;

      auto change_collat = exstate.convert( change_debt, pos->collateral.get_extended_symbol() );

      _accounts.adjust_balance( pos->owner, change_collat );

      c.peer_margin.total_lent.amount -= pos->borrowed.amount;
      price_idx.erase(pos);

      pos = price_idx.begin();
      if( pos != price_idx.end() )
         c.peer_margin.least_collateralized = pos->call_price;
      else
         c.peer_margin.least_collateralized = double(uint64_t(-1));
   }

   const exchange_state& market_state::initial_state()const {
      return *market_state_itr;
   }

   void market_state::lend( account_name lender, const extended_asset& quantity ) {
      auto sym = quantity.get_extended_symbol();
      _accounts.adjust_balance( lender, -quantity );

      if( sym == exstate.base.balance.get_extended_symbol() ) {
         double new_shares = exstate.base.peer_margin.lend( quantity.amount );
         adjust_lend_shares( lender, base_loans, new_shares );
      }
      else if( sym == exstate.quote.balance.get_extended_symbol() ) {
         double new_shares = exstate.quote.peer_margin.lend( quantity.amount );
         adjust_lend_shares( lender, quote_loans, new_shares );
      }
      else eosio_assert( false, "unable to lend to this market" );
   }

   void market_state::unlend( account_name lender, double ishares, const extended_symbol& sym ) {
      eosio_assert( ishares > 0, "cannot unlend negative balance" );
      adjust_lend_shares( lender, base_loans, -ishares );

      print( "sym: ", sym );

      if( sym == exstate.base.balance.get_extended_symbol() ) {
         extended_asset unlent  = exstate.base.peer_margin.unlend( ishares );
         _accounts.adjust_balance( lender, unlent );
      }
      else if( sym == exstate.quote.balance.get_extended_symbol() ) {
         extended_asset unlent  = exstate.quote.peer_margin.unlend( ishares );
         _accounts.adjust_balance( lender, unlent );
      }
      else eosio_assert( false, "unable to lend to this market" );
   }

   void market_state::adjust_lend_shares( account_name lender, loans& l, double delta ) {
      auto existing = l.find( lender );
      if( existing == l.end() ) {
         l.emplace( lender, [&]( auto& obj ) {
            obj.owner = lender;
            obj.interest_shares = delta;
            eosio_assert( delta >= 0, "underflow" );
         });
      } else {
         l.modify( existing, 0, [&]( auto& obj ) {
            obj.interest_shares += delta;
            eosio_assert( obj.interest_shares >= 0, "underflow" );
         });
      }
   }

   void market_state::cover_margin( account_name borrower, const extended_asset& cover_amount ) {
      if( cover_amount.get_extended_symbol() == exstate.base.balance.get_extended_symbol() ) {
         cover_margin( borrower, base_margins, exstate.base, cover_amount );
      } else if( cover_amount.get_extended_symbol() == exstate.quote.balance.get_extended_symbol() ) {
         cover_margin( borrower, quote_margins, exstate.quote, cover_amount );
      } else {
         eosio_assert( false, "invalid debt asset" );
      }
   }

   /**
    *  This method will use the collateral to buy the borrowed asset from the market
    *  with collateral to cancel the debt.
    */
   void market_state::cover_margin( account_name borrower, margins& m, exchange_state::connector& c,
                                    const extended_asset& cover_amount )
   {
      auto existing = m.find( borrower );
      eosio_assert( existing != m.end(), "no known margin position" );
      eosio_assert( existing->borrowed.amount >= cover_amount.amount, "attempt to cover more than user has" );

      auto tmp = exstate;
      auto estcol  = tmp.convert( cover_amount, existing->collateral.get_extended_symbol() );
      auto debpaid = exstate.convert( estcol, cover_amount.get_extended_symbol() );
      eosio_assert( debpaid.amount >= cover_amount.amount, "unable to cover debt" );

      auto refundcover = debpaid - cover_amount;

      auto refundcol = exstate.convert( refundcover, existing->collateral.get_extended_symbol() );
      estcol.amount -= refundcol.amount;

      if( existing->borrowed.amount == cover_amount.amount ) {
         auto freedcollateral = existing->collateral - estcol;
         m.erase( existing );
         existing = m.begin();
         _accounts.adjust_balance( borrower, freedcollateral );
      }
      else {
         m.modify( existing, 0, [&]( auto& obj ) {
             obj.collateral.amount -= estcol.amount;
             obj.borrowed.amount -= cover_amount.amount;
             obj.call_price = double(obj.borrowed.amount) / obj.collateral.amount;
         });
      }
      c.peer_margin.total_lent.amount -= cover_amount.amount;

      if( existing != m.end() ) {
         if( existing->call_price < c.peer_margin.least_collateralized )
            c.peer_margin.least_collateralized = existing->call_price;
      } else {
         c.peer_margin.least_collateralized = std::numeric_limits<double>::max();
      }
   }

   void market_state::update_margin( account_name borrower, const extended_asset& delta_debt, const extended_asset& delta_col )
   {
      if( delta_debt.get_extended_symbol() == exstate.base.balance.get_extended_symbol() ) {
         adjust_margin( borrower, base_margins, exstate.base, delta_debt, delta_col );
      } else if( delta_debt.get_extended_symbol() == exstate.quote.balance.get_extended_symbol() ) {
         adjust_margin( borrower, quote_margins, exstate.quote, delta_debt, delta_col );
      } else {
         eosio_assert( false, "invalid debt asset" );
      }
   }

   void market_state::adjust_margin( account_name borrower, margins& m, exchange_state::connector& c,
                                     const extended_asset& delta_debt, const extended_asset& delta_col )
   {
      auto existing = m.find( borrower );
      if( existing == m.end() ) {
         eosio_assert( delta_debt.amount > 0, "cannot borrow neg" );
         eosio_assert( delta_col.amount > 0, "cannot have neg collat" );

         existing = m.emplace( borrower, [&]( auto& obj ) {
            obj.owner      = borrower;
            obj.borrowed   = delta_debt;
            obj.collateral = delta_col;
            obj.call_price = double(obj.borrowed.amount) / obj.collateral.amount;
         });
      } else {
         if( existing->borrowed.amount == -delta_debt.amount ) {
            eosio_assert( existing->collateral.amount == -delta_col.amount, "user failed to claim all collateral" );

            m.erase( existing );
            existing = m.begin();
         } else {
            m.modify( existing, 0, [&]( auto& obj ) {
               obj.borrowed   += delta_debt;
               obj.collateral += delta_col;
               obj.call_price = double(obj.borrowed.amount) / obj.collateral.amount;
            });
         }
      }

      c.peer_margin.total_lent += delta_debt;
      eosio_assert( c.peer_margin.total_lent.amount <= c.peer_margin.total_lendable.amount, "insufficient funds availalbe to borrow" );

      if( existing != m.end() ) {
         if( existing->call_price < c.peer_margin.least_collateralized )
            c.peer_margin.least_collateralized = existing->call_price;

         eosio_assert( !exstate.requires_margin_call( c ), "this update would trigger a margin call" );
      } else {
         c.peer_margin.least_collateralized = std::numeric_limits<double>::max();
      }

   }

   void market_state::save() {
      market_table.modify( market_state_itr, 0, [&]( auto& s ) {
         s = exstate;
      });
   }

}


#pragma once

#include <openssl/bio.h>
#include <openssl/dh.h>
#include <openssl/dsa.h>
#include <openssl/err.h>
#include <openssl/evp.h>
#include <openssl/pem.h>
#include <openssl/rand.h>
#include <openssl/rsa.h>
#include <openssl/ssl.h>

#include <boost/asio/ssl/context.hpp>
#include <random.hpp>

#include <random>

namespace ensuressl
{
constexpr char const* trustStorePath = "/etc/ssl/certs/authority";
constexpr char const* x509Comment = "Generated from OpenBMC service";
static void initOpenssl();
static EVP_PKEY* createEcKey();

// Trust chain related errors.`
inline bool isTrustChainError(int errnum)
{
    return (errnum == X509_V_ERR_DEPTH_ZERO_SELF_SIGNED_CERT) ||
           (errnum == X509_V_ERR_SELF_SIGNED_CERT_IN_CHAIN) ||
           (errnum == X509_V_ERR_UNABLE_TO_GET_ISSUER_CERT_LOCALLY) ||
           (errnum == X509_V_ERR_CERT_UNTRUSTED) ||
           (errnum == X509_V_ERR_UNABLE_TO_VERIFY_LEAF_SIGNATURE);
}

inline bool validateCertificate(X509* const cert)
{
    // Create an empty X509_STORE structure for certificate validation.
    X509_STORE* x509Store = X509_STORE_new();
    if (x509Store == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error occurred during X509_STORE_new call";
        return false;
    }

    // Load Certificate file into the X509 structure.
    X509_STORE_CTX* storeCtx = X509_STORE_CTX_new();
    if (storeCtx == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error occurred during X509_STORE_CTX_new call";
        X509_STORE_free(x509Store);
        return false;
    }

    int errCode = X509_STORE_CTX_init(storeCtx, x509Store, cert, nullptr);
    if (errCode != 1)
    {
        BMCWEB_LOG_ERROR << "Error occurred during X509_STORE_CTX_init call";
        X509_STORE_CTX_free(storeCtx);
        X509_STORE_free(x509Store);
        return false;
    }

    errCode = X509_verify_cert(storeCtx);
    if (errCode == 1)
    {
        BMCWEB_LOG_INFO << "Certificate verification is success";
        X509_STORE_CTX_free(storeCtx);
        X509_STORE_free(x509Store);
        return true;
    }
    if (errCode == 0)
    {
        errCode = X509_STORE_CTX_get_error(storeCtx);
        X509_STORE_CTX_free(storeCtx);
        X509_STORE_free(x509Store);
        if (isTrustChainError(errCode))
        {
            BMCWEB_LOG_DEBUG << "Ignoring Trust Chain error. Reason: "
                             << X509_verify_cert_error_string(errCode);
            return true;
        }
        BMCWEB_LOG_ERROR << "Certificate verification failed. Reason: "
                         << X509_verify_cert_error_string(errCode);
        return false;
    }

    BMCWEB_LOG_ERROR
        << "Error occurred during X509_verify_cert call. ErrorCode: "
        << errCode;
    X509_STORE_CTX_free(storeCtx);
    X509_STORE_free(x509Store);
    return false;
}

inline bool verifyOpensslKeyCert(const std::string& filepath)
{
    bool privateKeyValid = false;
    bool certValid = false;

    std::cout << "Checking certs in file " << filepath << "\n";

    FILE* file = fopen(filepath.c_str(), "r");
    if (file != nullptr)
    {
        EVP_PKEY* pkey = PEM_read_PrivateKey(file, nullptr, nullptr, nullptr);
        if (pkey != nullptr)
        {
#if (OPENSSL_VERSION_NUMBER < 0x30000000L)
            RSA* rsa = EVP_PKEY_get1_RSA(pkey);
            if (rsa != nullptr)
            {
                std::cout << "Found an RSA key\n";
                if (RSA_check_key(rsa) == 1)
                {
                    privateKeyValid = true;
                }
                else
                {
                    std::cerr << "Key not valid error number "
                              << ERR_get_error() << "\n";
                }
                RSA_free(rsa);
            }
            else
            {
                EC_KEY* ec = EVP_PKEY_get1_EC_KEY(pkey);
                if (ec != nullptr)
                {
                    std::cout << "Found an EC key\n";
                    if (EC_KEY_check_key(ec) == 1)
                    {
                        privateKeyValid = true;
                    }
                    else
                    {
                        std::cerr << "Key not valid error number "
                                  << ERR_get_error() << "\n";
                    }
                    EC_KEY_free(ec);
                }
            }
#else
            EVP_PKEY_CTX* pkeyCtx =
                EVP_PKEY_CTX_new_from_pkey(nullptr, pkey, nullptr);

            if (pkeyCtx == nullptr)
            {
                std::cerr << "Unable to allocate pkeyCtx " << ERR_get_error()
                          << "\n";
            }
            else if (EVP_PKEY_check(pkeyCtx) == 1)
            {
                privateKeyValid = true;
            }
            else
            {

                std::cerr << "Key not valid error number " << ERR_get_error()
                          << "\n";
            }
#endif

            if (privateKeyValid)
            {
                // If the order is certificate followed by key in input file
                // then, certificate read will fail. So, setting the file
                // pointer to point beginning of file to avoid certificate and
                // key order issue.
                fseek(file, 0, SEEK_SET);

                X509* x509 = PEM_read_X509(file, nullptr, nullptr, nullptr);
                if (x509 == nullptr)
                {
                    std::cout << "error getting x509 cert " << ERR_get_error()
                              << "\n";
                }
                else
                {
                    certValid = validateCertificate(x509);
                    X509_free(x509);
                }
            }

#if (OPENSSL_VERSION_NUMBER > 0x30000000L)
            EVP_PKEY_CTX_free(pkeyCtx);
#endif
            EVP_PKEY_free(pkey);
        }
        fclose(file);
    }
    return certValid;
}

inline X509* loadCert(const std::string& filePath)
{
    BIO* certFileBio = BIO_new_file(filePath.c_str(), "rb");
    if (certFileBio == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error occured during BIO_new_file call, "
                         << "FILE= " << filePath;
        return nullptr;
    }

    X509* cert = X509_new();
    if (cert == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error occured during X509_new call, "
                         << ERR_get_error();
        BIO_free(certFileBio);
        return nullptr;
    }

    if (PEM_read_bio_X509(certFileBio, &cert, nullptr, nullptr) == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error occured during PEM_read_bio_X509 call, "
                         << "FILE= " << filePath;

        BIO_free(certFileBio);
        X509_free(cert);
        return nullptr;
    }
    BIO_free(certFileBio);
    return cert;
}

inline int addExt(X509* cert, int nid, const char* value)
{
    X509_EXTENSION* ex = nullptr;
    X509V3_CTX ctx{};
    X509V3_set_ctx(&ctx, cert, cert, nullptr, nullptr, 0);

    // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast)
    ex = X509V3_EXT_conf_nid(nullptr, &ctx, nid, const_cast<char*>(value));
    if (ex == nullptr)
    {
        BMCWEB_LOG_ERROR << "Error: In X509V3_EXT_conf_nidn: " << value;
        return -1;
    }
    X509_add_ext(cert, ex, -1);
    X509_EXTENSION_free(ex);
    return 0;
}

inline void generateSslCertificate(const std::string& filepath,
                                   const std::string& cn)
{
    FILE* pFile = nullptr;
    std::cout << "Generating new keys\n";
    initOpenssl();

    std::cerr << "Generating EC key\n";
    EVP_PKEY* pPrivKey = createEcKey();
    if (pPrivKey != nullptr)
    {
        std::cerr << "Generating x509 Certificate\n";
        // Use this code to directly generate a certificate
        X509* x509 = X509_new();
        if (x509 != nullptr)
        {
            // get a random number from the RNG for the certificate serial
            // number If this is not random, regenerating certs throws broswer
            // errors
            bmcweb::OpenSSLGenerator gen;
            std::uniform_int_distribution<int> dis(
                1, std::numeric_limits<int>::max());
            int serial = dis(gen);

            ASN1_INTEGER_set(X509_get_serialNumber(x509), serial);

            // not before this moment
            X509_gmtime_adj(X509_get_notBefore(x509), 0);
            // Cert is valid for 10 years
            X509_gmtime_adj(X509_get_notAfter(x509),
                            60L * 60L * 24L * 365L * 10L);

            // set the public key to the key we just generated
            X509_set_pubkey(x509, pPrivKey);

            // get the subject name
            X509_NAME* name = X509_get_subject_name(x509);

            using x509String = const unsigned char;
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            x509String* country = reinterpret_cast<x509String*>("US");
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            x509String* company = reinterpret_cast<x509String*>("OpenBMC");
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            x509String* cnStr = reinterpret_cast<x509String*>(cn.c_str());

            X509_NAME_add_entry_by_txt(name, "C", MBSTRING_ASC, country, -1, -1,
                                       0);
            X509_NAME_add_entry_by_txt(name, "O", MBSTRING_ASC, company, -1, -1,
                                       0);
            X509_NAME_add_entry_by_txt(name, "CN", MBSTRING_ASC, cnStr, -1, -1,
                                       0);
            // set the CSR options
            X509_set_issuer_name(x509, name);

            X509_set_version(x509, 2);
            addExt(x509, NID_basic_constraints, ("critical,CA:TRUE"));
            addExt(x509, NID_subject_alt_name, ("DNS:" + cn).c_str());
            addExt(x509, NID_subject_key_identifier, ("hash"));
            addExt(x509, NID_authority_key_identifier, ("keyid"));
            addExt(x509, NID_key_usage, ("digitalSignature, keyEncipherment"));
            addExt(x509, NID_ext_key_usage, ("serverAuth"));
            addExt(x509, NID_netscape_comment, (x509Comment));

            // Sign the certificate with our private key
            X509_sign(x509, pPrivKey, EVP_sha256());

            pFile = fopen(filepath.c_str(), "wt");

            if (pFile != nullptr)
            {
                PEM_write_PrivateKey(pFile, pPrivKey, nullptr, nullptr, 0,
                                     nullptr, nullptr);

                PEM_write_X509(pFile, x509);
                fclose(pFile);
                pFile = nullptr;
            }

            X509_free(x509);
        }

        EVP_PKEY_free(pPrivKey);
        pPrivKey = nullptr;
    }

    // cleanup_openssl();
}

EVP_PKEY* createEcKey()
{
    EVP_PKEY* pKey = nullptr;

#if (OPENSSL_VERSION_NUMBER < 0x30000000L)
    int eccgrp = 0;
    eccgrp = OBJ_txt2nid("secp384r1");

    EC_KEY* myecc = EC_KEY_new_by_curve_name(eccgrp);
    if (myecc != nullptr)
    {
        EC_KEY_set_asn1_flag(myecc, OPENSSL_EC_NAMED_CURVE);
        EC_KEY_generate_key(myecc);
        pKey = EVP_PKEY_new();
        if (pKey != nullptr)
        {
            if (EVP_PKEY_assign(pKey, EVP_PKEY_EC, myecc) != 0)
            {
                /* pKey owns myecc from now */
                if (EC_KEY_check_key(myecc) <= 0)
                {
                    std::cerr << "EC_check_key failed.\n";
                }
            }
        }
    }
#else
    // Create context for curve parameter generation.
    std::unique_ptr<EVP_PKEY_CTX, decltype(&::EVP_PKEY_CTX_free)> ctx{
        EVP_PKEY_CTX_new_id(EVP_PKEY_EC, nullptr), &::EVP_PKEY_CTX_free};
    if (!ctx)
    {
        return nullptr;
    }

    // Set up curve parameters.
    EVP_PKEY* params = nullptr;
    if ((EVP_PKEY_paramgen_init(ctx.get()) <= 0) ||
        (EVP_PKEY_CTX_set_ec_param_enc(ctx.get(), OPENSSL_EC_NAMED_CURVE) <=
         0) ||
        (EVP_PKEY_CTX_set_ec_paramgen_curve_nid(ctx.get(), NID_secp384r1) <=
         0) ||
        (EVP_PKEY_paramgen(ctx.get(), &params) <= 0))
    {
        return nullptr;
    }

    // Set up RAII holder for params.
    std::unique_ptr<EVP_PKEY, decltype(&::EVP_PKEY_free)> pparams{
        params, &::EVP_PKEY_free};

    // Set new context for key generation, using curve parameters.
    ctx.reset(EVP_PKEY_CTX_new_from_pkey(nullptr, params, nullptr));
    if (!ctx || (EVP_PKEY_keygen_init(ctx.get()) <= 0))
    {
        return nullptr;
    }

    // Generate key.
    if (EVP_PKEY_keygen(ctx.get(), &pKey) <= 0)
    {
        return nullptr;
    }
#endif

    return pKey;
}

void initOpenssl()
{
#if OPENSSL_VERSION_NUMBER < 0x10100000L
    SSL_load_error_strings();
    OpenSSL_add_all_algorithms();
    RAND_load_file("/dev/urandom", 1024);
#endif
}

inline void ensureOpensslKeyPresentAndValid(const std::string& filepath)
{
    bool pemFileValid = false;

    pemFileValid = verifyOpensslKeyCert(filepath);

    if (!pemFileValid)
    {
        std::cerr << "Error in verifying signature, regenerating\n";
        generateSslCertificate(filepath, "testhost");
    }
}

inline std::shared_ptr<boost::asio::ssl::context>
    getSslContext(const std::string& sslPemFile)
{
    std::shared_ptr<boost::asio::ssl::context> mSslContext =
        std::make_shared<boost::asio::ssl::context>(
            boost::asio::ssl::context::tls_server);
    mSslContext->set_options(boost::asio::ssl::context::default_workarounds |
                             boost::asio::ssl::context::no_sslv2 |
                             boost::asio::ssl::context::no_sslv3 |
                             boost::asio::ssl::context::single_dh_use |
                             boost::asio::ssl::context::no_tlsv1 |
                             boost::asio::ssl::context::no_tlsv1_1);

    // BIG WARNING: This needs to stay disabled, as there will always be
    // unauthenticated endpoints
    // mSslContext->set_verify_mode(boost::asio::ssl::verify_peer);

    SSL_CTX_set_options(mSslContext->native_handle(), SSL_OP_NO_RENEGOTIATION);

    BMCWEB_LOG_DEBUG << "Using default TrustStore location: " << trustStorePath;
    mSslContext->add_verify_path(trustStorePath);

    mSslContext->use_certificate_file(sslPemFile,
                                      boost::asio::ssl::context::pem);
    mSslContext->use_private_key_file(sslPemFile,
                                      boost::asio::ssl::context::pem);

    // Set up EC curves to auto (boost asio doesn't have a method for this)
    // There is a pull request to add this.  Once this is included in an asio
    // drop, use the right way
    // http://stackoverflow.com/questions/18929049/boost-asio-with-ecdsa-certificate-issue
    if (SSL_CTX_set_ecdh_auto(mSslContext->native_handle(), 1) != 1)
    {
        BMCWEB_LOG_ERROR << "Error setting tmp ecdh list\n";
    }

    std::string mozillaModern = "ECDHE-ECDSA-AES256-GCM-SHA384:"
                                "ECDHE-RSA-AES256-GCM-SHA384:"
                                "ECDHE-ECDSA-CHACHA20-POLY1305:"
                                "ECDHE-RSA-CHACHA20-POLY1305:"
                                "ECDHE-ECDSA-AES128-GCM-SHA256:"
                                "ECDHE-RSA-AES128-GCM-SHA256:"
                                "ECDHE-ECDSA-AES256-SHA384:"
                                "ECDHE-RSA-AES256-SHA384:"
                                "ECDHE-ECDSA-AES128-SHA256:"
                                "ECDHE-RSA-AES128-SHA256";

    if (SSL_CTX_set_cipher_list(mSslContext->native_handle(),
                                mozillaModern.c_str()) != 1)
    {
        BMCWEB_LOG_ERROR << "Error setting cipher list\n";
    }
    return mSslContext;
}
} // namespace ensuressl


#include <boost/test/unit_test.hpp>
#include <moreorg/OrganizationModel.hpp>
#include <moreorg/OrganizationModelAsk.hpp>
#include <moreorg/facades/Robot.hpp>
#include <moreorg/PropertyConstraintSolver.hpp>
#include <moreorg/vocabularies/OM.hpp>
#include "test_utils.hpp"

using namespace moreorg;
using namespace owlapi::model;

BOOST_AUTO_TEST_SUITE(property_constraint_solver)

BOOST_AUTO_TEST_CASE(value_bound)
{
    owlapi::model::IRI propertyA("http://test/propertyA");
    PropertyConstraint pcA0(propertyA, PropertyConstraint::GREATER_EQUAL, 3.0);
    PropertyConstraint pcA1(propertyA, PropertyConstraint::LESS_THAN, 10.0);
    PropertyConstraint pcA2(propertyA, PropertyConstraint::LESS_THAN, 8.5);
    PropertyConstraint pcA3(propertyA, PropertyConstraint::LESS_THAN, 2.5);

    {
        PropertyConstraint::List constraints =  { pcA0, pcA1, pcA2 };
        ValueBound vb = PropertyConstraintSolver::merge(constraints);

        BOOST_REQUIRE_MESSAGE(vb.getMin() == 3.0, "Min is set to " << vb.getMin() << " expected " << 3.0);
        BOOST_REQUIRE_MESSAGE(vb.getMax() == 8.5, "Max is set to " << vb.getMax() << " expected " << 8.5);
    }
    {
        PropertyConstraint::List constraints =  { pcA0, pcA1, pcA2, pcA3 };
        BOOST_REQUIRE_THROW(PropertyConstraintSolver::merge(constraints), std::invalid_argument);
    }
}

BOOST_AUTO_TEST_CASE(fulfillment)
{
    //std::string filename = "" + getRootDir() +
    //    "/test/data/om-robot-sherpa-tt.ttl";
    std::string filename = "" + getRootDir() + "/test/data/test-load-area.ttl";
    OrganizationModel::Ptr organizationModel = make_shared<OrganizationModel>(filename);

    owlapi::model::IRI sherpaTT = vocabulary::OM::resolve("SherpaTT");

    ModelPool pool;
    pool[sherpaTT] = 1;

    OrganizationModelAsk ask(organizationModel, pool, true);
    facades::Robot robot = facades::Robot::getInstance(pool, ask);

    BOOST_REQUIRE_MESSAGE( robot.getPowerSourceCapacity() != 0, "SherpaTT has "
            " power source capacity");

    BOOST_REQUIRE_MESSAGE( robot.getLoadArea() == 0.15*0.15, "SherpaTT has load area");
    {
        PropertyConstraint massC0(vocabulary::OM::mass(), PropertyConstraint::GREATER_EQUAL, 100.0);
        BOOST_REQUIRE_MESSAGE(!massC0.usesPropertyReference(), "Is not a self referencing constraint");
        BOOST_REQUIRE_MESSAGE(massC0.getReferenceValue(robot) == 100.0, "Property reference mass");

        Fulfillment f = PropertyConstraintSolver::fulfills(robot, PropertyConstraint::Set{ massC0 });
        BOOST_REQUIRE_MESSAGE(f.isMet(), "SherpaTT has a mass greater than 100.0 kg");
    }

    {
        PropertyConstraint massC1(vocabulary::OM::mass(), PropertyConstraint::LESS_THAN, 50.0);
        Fulfillment f = PropertyConstraintSolver::fulfills(robot, PropertyConstraint::List{ massC1 });
        BOOST_REQUIRE_MESSAGE(!f.isMet(), "SherpaTT has not a mass less than 50.0 kg");
    }

    {
        PropertyConstraint massC1(vocabulary::OM::resolve("loadAreaSize"), PropertyConstraint::GREATER_THAN, 0.1);
        Fulfillment f = PropertyConstraintSolver::fulfills(robot, PropertyConstraint::List{ massC1 });
        BOOST_REQUIRE_MESSAGE(!f.isMet(), "SherpaTT has a load area > 0.1 cm^2");
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef BASECORRESPONDENCEFILTER_HPP
#define BASECORRESPONDENCEFILTER_HPP

#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "../global.hpp"
#include <iostream>

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< float, 1, registration::NUM_FEATURES> FeatureVec; //matrix Mx6 of type float
typedef Eigen::Vector3f Vec3Float;
typedef Eigen::SparseMatrix<float, 0, int> SparseMat;
typedef Eigen::Triplet<float> Triplet;
typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic> MatDynInt; //matrix MxN of type unsigned int
typedef Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic> MatDynFloat;

namespace registration {

class BaseCorrespondenceFilter
{
    /*
    # GOAL
    This class serves as the base class for the correspondence filter classes.

    */

    public:
        BaseCorrespondenceFilter();
        virtual ~BaseCorrespondenceFilter();

        virtual void set_floating_input(const FeatureMat * const inFloatingFeatures,
                                const VecDynFloat * const inFloatingFlags){}
        virtual void set_target_input(const FeatureMat * const inTargetFeatures,
                            const VecDynFloat * const inTargetFlags){}
        void set_output(FeatureMat * const ioCorrespondingFeatures,
                        VecDynFloat * const ioCorrespondingFlags);
        SparseMat get_affinity() const {return _affinity;}
        virtual void set_parameters(const size_t numNeighbours,
                                    const float flagThreshold){}
        virtual void set_parameters(const size_t numNeighbours,
                                    const float flagThreshold,
                                    const bool equalizePushPull){}
        virtual void update(){}

    protected:

        //# Inputs
        const FeatureMat * _inFloatingFeatures = NULL;
        const VecDynFloat * _inFloatingFlags = NULL; //currently never used (only in the symmetric version)
        const FeatureMat * _inTargetFeatures = NULL;
        const VecDynFloat * _inTargetFlags = NULL;

        //# Outputs
        FeatureMat * _ioCorrespondingFeatures = NULL;
        VecDynFloat * _ioCorrespondingFlags = NULL;

        //# User Parameters
        size_t _numNeighbours = 3;
        float _flagThreshold = 0.99f;

        //# Internal Data structures
        SparseMat _affinity;

        //# Internal Parameters
        size_t _numFloatingElements = 0;
        size_t _numTargetElements = 0;

        //# Internal functions
        //## Function to update the sparse affinity matrix
        virtual void _update_affinity(){};
        //## Function to convert the sparse affinity weights into corresponding
        //## features and flags
        void _affinity_to_correspondences();

    private:

};

}//namespace registration
#endif // BASECORRESPONDENCEFILTER_HPP


#include <iostream>
#include <ros/ros.h>
#include "humanoid_league_msgs/GoalRelative.h"
#include "humanoid_league_msgs/LineInformationRelative.h"
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>//quick walking node
#include "humanoid_league_msgs/TeamData.h"
#include "humanoid_league_msgs/GameState.h"
#include "humanoid_league_msgs/ObstaclesRelative.h"
#include <Eigen/Eigen>
#include <Eigen/Dense>

using namespace Eigen;

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

	/*通过"testData"Node仿真发送下列messages，其中"visual_odometry" "visualCompass"在bitbots_meta中尚未更新。*/
	ros::Publisher pubTeamData = n.advertise<humanoid_league_msgs::TeamData>("teamdata_topic",10);
	ros::Publisher pubGoalRelative = n.advertise<humanoid_league_msgs::GoalRelative>("goal_relative",10);
	ros::Publisher pubObsRelative = n.advertise<humanoid_league_msgs::ObstaclesRelative>("obstacles_relative",10);
	ros::Publisher pubLineInformationRelative = n.advertise<humanoid_league_msgs::LineInformationRelative>("line_relative",10);
	ros::Publisher pubWlakOdometry = n.advertise<nav_msgs::Odometry>("walk_odometry",10);
	ros::Publisher pubVisualOdometry = n.advertise<nav_msgs::Odometry>("visual_odometry",10);
	ros::Publisher pubPointCloud2 = n.advertise<sensor_msgs::PointCloud2>("line_relative_pc",10);
	ros::Publisher pubGameState = n.advertise<humanoid_league_msgs::GameState>("gamestate",10);
//	ros::Publisher pubVisualCompass = n.advertise<>();
//	ros::Publisher pubBallRelative = n.advertise<humanoid_league_msgs::BallRelative>("ball_relative",10);

	Matrix<float, 24, 7> MatrixGoal;
	Matrix<float,  9, 6> MatrixObs;
	Matrix<float,  6, 2> MatrixSegstart;
	Matrix<float,  6, 2> MatrixSegend;
	Matrix<float,  6, 6> MatrixCircle;
	//lx, ly, rx, ry, cx, cy, c
	MatrixGoal << //fake data:七个一组，分别对应上述值。
		4, 2, 4, 4, 4, 3, 1, 	      4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1,
            	4, 2, 4, 4, 4, 3, 1, 	      4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1,
            	4, 2, 4, 4, 4, 3, 1, 	      4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1,
            	4, 2, 4, 4, 4, 3, 1, 	      4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1, 		4, 2, 4, 4, 4, 3, 1,
            	3.9, 2, 3.9, 3.9, 4, 3, 1,    3.7, 2, 3.7, 4, 3.7, 3, 1,        3.5, 2, 3.5, 4, 3.5, 3, 1,
            	3.3, 2, 3.3, 4, 3.3, 3, 1,    3.1, 2, 3.1, 4, 3.1, 3, 1,        2.9, 2, 2.9, 4, 2.9, 3, 1,
            	2.7, 2, 2.7, 4, 2.7, 3, 1,    2.5, 2, 2.5, 4, 2.5, 3, 1;

	//x, y, w, h, t, c
	MatrixObs <<    0.5, -2,   0.5, 0.5, 3, 0.7, 	0.8, -2,   0.5, 0.5, 3, 0.7,
            		1,   -2,   0.5, 0.5, 3, 0.7, 	1.2, -2,   0.5, 0.5, 3, 0.7,
            		1.2, -2.2, 0.5, 0.5, 3, 0.7, 	1.2, -2.5, 0.5, 0.5, 3, 0.7,
            		1.2, -2.7, 0.5, 0.5, 3, 0.7, 	1.0, -2.5, 0.5, 0.5, 3, 0.7,
        		0.8, -2.3, 0.5, 0.5, 3, 0.7 ;

	//线的起点坐标，两个值一组
	MatrixSegstart <<  0,0,   0,0,   0,0,   0,0,   0,0,   0,0;//TODO:1.make data,2.declear it
	//线的终点，两个值一组
	MatrixSegend   <<  0,0,   0,0,   0,0,   0,0,   0,0,   0,0;
	//三个点确定一个圆，六个值一组，分别是左点，中点，右点
	MatrixCircle <<    0,0,0,0,0,0,   0,0,0,0,0,0,   0,0,0,0,0,0,   0,0,0,0,0,0,   0,0,0,0,0,0,   0,0,0,0,0,0;

	humanoid_league_msgs::GoalRelative goal;
	humanoid_league_msgs::TeamData team;
	humanoid_league_msgs::ObstacleRelative obs;
	std::vector<humanoid_league_msgs::ObstacleRelative> Obs;
	humanoid_league_msgs::ObstaclesRelative obstacle;
	humanoid_league_msgs::LineSegmentRelative linesegment;
	std::vector<humanoid_league_msgs::LineSegmentRelative> lineSegment;
	humanoid_league_msgs::LineIntersectionRelative lineinter;
	std::vector<humanoid_league_msgs::LineIntersectionRelative> lineInter;
	humanoid_league_msgs::LineCircleRelative linecircle;
	std::vector<humanoid_league_msgs::LineCircleRelative> lineCircle;
	humanoid_league_msgs::LineInformationRelative line;
	humanoid_league_msgs::GameState gamestate;
	int timeCounter = 30;
	bool firsthalf = true;
	int durationHalfGame = 60;

	ros::Rate loop_rate(10);
	while(ros::ok())
	{
		for(int i = 0; i < MatrixGoal.rows(); i++)
		{
			goal.header.seq = i;
			goal.header.stamp = ros::Time::now();
			goal.header.frame_id = "base_footprint";
			goal.left_post.x = MatrixGoal(i,0);
			goal.left_post.y = MatrixGoal(i,1);
			goal.right_post.x = MatrixGoal(i,2);
			goal.right_post.y = MatrixGoal(i,3);
			goal.center_direction.x = MatrixGoal(i,4);
			goal.center_direction.y = MatrixGoal(i,5);
			goal.confidence = MatrixGoal(i,6);

			if(i<MatrixObs.rows())
			{
				obs.playerNumber = -1;
				obs.position.x = MatrixObs(i,0);//Point
				obs.position.y = MatrixObs(i,1);
				obs.width = MatrixObs(i,2);
				obs.height = MatrixObs(i,3);
				obs.color = MatrixObs(i,4);
				obs.confidence = MatrixObs(i,5);
				Obs.push_back(obs);
				obstacle.header.stamp = ros::Time::now();
				obstacle.header.frame_id = "base_footprint";
				obstacle.obstacles = Obs;
			}

			if(i<6)
			{
				for(int j = 0; j < 6; j++ )
				{
					for(int k = 0; k < 6; k++ )
					{
						linesegment.start.x = MatrixSegstart(k,0);
						linesegment.start.y = MatrixSegstart(k,1);
						linesegment.end.x = MatrixSegend(k,0);
						linesegment.end.y = MatrixSegend(k,1);
						linesegment.confidence = 1;
						lineSegment.push_back(linesegment);
					}
					lineinter.segments = lineSegment;
					lineinter.type = 0;
					lineinter.confidence = 1;
				}
				lineInter.push_back(lineinter);
			}

			if(i<6)
			{
				linecircle.left.x = MatrixCircle(i,0);
				linecircle.left.y = MatrixCircle(i,1);
				linecircle.middle.x = MatrixCircle(i,2);
				linecircle.middle.y = MatrixCircle(i,3);
				linecircle.right.x = MatrixCircle(i,4);
				linecircle.right.y = MatrixCircle(i,5);
				linecircle.confidence = 1;
				lineCircle.push_back(linecircle);
			}

			gamestate.header.frame_id = i;
			gamestate.header.stamp = ros::Time::now();
			gamestate.secondaryState = 0;
			gamestate.secondsTillUnpenalized = timeCounter;
        		// Penalty boolean
        		gamestate.penalized = timeCounter > 0	;
        		// Sets halftime and rest secs
        		gamestate.firstHalf = firsthalf;
        		gamestate.secondsRemaining = durationHalfGame;
        		// Sets Score
        		gamestate.ownScore = 7;
        		gamestate.rivalScore = 1;
        		// team colors
        		gamestate.teamColor = 1;// magenta
			timeCounter -= 1;
       			if(timeCounter < 0)
			{
				timeCounter = 0;
			}
      	 	 	durationHalfGame -= 1;
       		 	if(durationHalfGame == 0)
			{
				durationHalfGame = 60;
            			firsthalf = false;
			}
			line.intersections = lineInter;
			line.segments = lineSegment;
			line.circles = lineCircle;
			obstacle.obstacles = Obs;
			pubGoalRelative.publish(goal);
			pubObsRelative.publish(obstacle);
			pubLineInformationRelative.publish(line);
			pubGameState.publish(gamestate);
			loop_rate.sleep();
		}
	}
}
                        /*
                        //bitbots_meta TODO;
                        iteam.header.seq = ;
                        team.header.stamp = ros::Time::now();
                        team.header.frame_id = ;
                        team.robot_ids[3];
                        team.role[2];
                        team.action[1];
                        team.state[1];
                        team.robot_positions = ;//数组
                        team.oppgoal_relative = ;//数组
                        team.avg_walking_speed = ;
                        team.time_to_position_at_ball = ;
                        team.max_kicking_distance = ;
                        pubTeamData.publish(team);

                        nav_msgs::Odometry walkodom;
                        walkodom.header.seq = ;
                        walkodom.header.stamp = ros::Time::now();
                        walkodomm.header.frame_id = ;
                        walkodom.string_child_frame_id = ;
                        walkodom.pose = ;//geometry_msgs/PoseWithCovariance
                        walkodom.twist = ;//geometry_msgs/TwistWithCovariance
                        pubWlakOdometry.publish(walkodom);

                        sensor_msgs::PointCloud2 line;
                        line.header.seq = ;
                        line.header.stamp = ros::Time::now();
                        line.header.frame_id = ;
                        line.height = ;
                        line.width = ;
                        line.fields = ;//sensor_msgs/PointField[]
                        line.is_bigendian = ;//bool
                        line.point_step = ;
                        line.row_step = ;
                        line.data = ;
                        line.is_dense = ;//bool
                        */


//
// Created by James on 10/31/2021.
//

#include <stakepool.h>

#include <validator.h>
#include <stakeparams.h>
#include <consensus/amount.h>
#include <chain.h>
#include <cmath>
#include <crypto/sha256.h>
#include <script/script.h>
#include <boost/crc.hpp>
#include <float.h>
#include <vector>
#include <unordered_map>
#include <script/script.h>

StakePool::StakePool() {
    totalStake = 0;
    lastValidationTime = 0;
}

bool StakePool::addValidator(Validator* validator, int nHeight, std::vector<std::string>* rterror) {
    if (validatorExists(validator)) {
        rterror->push_back("Validator exists");
        return false;
    }

    validatorPool.push_back(validator);
    validatorPoolScript[validator->scriptPubKey.ToString()] = validator;
    return recalculateProbabilities(nHeight, rterror);
}

bool StakePool::removeValidator(Validator* validator, int nHeight, std::vector<std::string>* rterror) {
    if (validator->suspended) {
        rterror->push_back("Validator suspended");
        return false;
    }

    if (!validatorExists(validator)) {
        rterror->push_back("Validator does not exist");
        return false;
    }

    auto it = validatorPoolScript.find(validator->scriptPubKey.ToString());
    for (size_t i = 0; i < validatorPool.size(); i++) {
        if (it->second == validatorPool.at(i)) {
            validatorPool.erase(validatorPool.begin()+i);
            break;
        }
    }
    validatorPoolScript.erase(it);
    return recalculateProbabilities(nHeight, rterror);
}

bool StakePool::recalculateProbabilities(int nHeight, std::vector<std::string>* rterror) {
    totalStake = 0;
    for (Validator* v : validatorPool) {
        if (v->suspended) {
            int elapsedTime = nHeight - v->suspendedBlock;
            if (elapsedTime >= VALIDATOR_SUSPENSION_DURATION) {
                unsuspendValidator(v, nHeight, rterror);

            }
        }

        if (!v->suspended){
            v->adjustStake(nHeight);
            totalStake += v->adjustedStake;
        }
    }

    for (Validator* v : validatorPool) {
        v->calculateProbability(totalStake);
    }
    sort(nHeight);
    return true;
}

Validator* StakePool::retrieveNextValidator(int nHeight, std::vector<std::string>* rterror) {
    recalculateProbabilities(nHeight, rterror);

    vData.clear();
    serialize();

    boost::crc_32_type hasher;
    std::string s(vData.begin(), vData.end());
    hasher.process_bytes(s.data(), s.length());
    double result = abs((double) hasher.checksum());

    double selection = result / DBL_MAX;
    Validator* selectedValidator = nullptr;

    double probabilitiesSummed = 0.0;
    for (Validator* v : validatorPool) {
        probabilitiesSummed += v->probability;
        if (selection <= probabilitiesSummed) {
            selectedValidator = v;
            break;
        }
    }

    if (selectedValidator == nullptr) {
        rterror->push_back("No validator found");
    }

    return selectedValidator;
}

void StakePool::serialize() {
    vData.clear();
    for (Validator* v : validatorPool) {
        std::vector<unsigned char> s = ToByteVector(v->scriptPubKey);
        vData.insert(vData.end(), s.front(), s.back());
    }
}

void StakePool::sort(int nHeight) {
    std::sort(validatorPool.begin(), validatorPool.end(), std::greater<Validator*>());
}

bool StakePool::validatorExists(Validator* v) {
    return validatorPoolScript.count(v->scriptPubKey.ToString()) == 1;
}

bool StakePool::suspendValidator(Validator *v, int nHeight, std::vector <std::string>* rterror) {
    if(!validatorExists(v)) {
        rterror->push_back("Validator does not exist");
        return false;
    }

    v->suspended = true;
    v->suspendedBlock = nHeight;
    v->adjustedStake = 0;
    recalculateProbabilities(nHeight, rterror);
}

bool StakePool::unsuspendValidator(Validator *v, int nHeight, std::vector <std::string>* rterror) {
    v->suspended = false;
    v->suspendedBlock = 0;
    v->lastBlockHeight = nHeight;
    return true;
}

bool StakePool::viable() {
    bool viable = false;

    for (Validator* v : validatorPool) {
        if (!v->suspended) {
            viable = true;
        }
    }

    return viable;
}


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

// Author: David Alexander

#include <ConsensusCore/Quiver/Diploid.hpp>

#include <ConsensusCore/Mutation.hpp>

#include <algorithm>
#include <cassert>
#include <cfloat>
#include <cmath>
#include <iostream>
#include <numeric>
#include <utility>
#include <vector>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

using std::vector;
using std::accumulate;
using std::pair;
using std::make_pair;
using std::cout;
using std::endl;

typedef boost::numeric::ublas::vector<float> fvec;
typedef boost::numeric::ublas::matrix<float> fmat;
typedef boost::numeric::ublas::matrix_column<const fmat> fmat_column;

#if defined(_MSC_VER) && _MSC_VER < 1800

// compute log(1+x) without losing precision for small values of x
float log1p(float x)
{
    if (x <= -1.0)
    {
        std::stringstream os;
        os << "Invalid input argument (" << x
           << "); must be greater than -1.0";
        throw std::invalid_argument(os.str());
    }

    if (fabs(x) > 1e-4)
    {
        // x is large enough that the obvious evaluation is OK
        return log(1.0 + x);
    }

    // Use Taylor approx. log(1 + x) = x - x^2/2 with error roughly x^3/3
    // Since |x| < 10^-4, |x|^3 < 10^-12, relative error less than 10^-8

    return (-0.5*x + 1.0)*x;
}

#endif

namespace ConsensusCore {

    // This needs to be configurable.
    DEBUG_ONLY( const int MUTATIONS_PER_SITE = 9; ) // NOLINT
    const int LENGTH_DIFFS[] = { 0, 0, 0, 0, 1, 1, 1, 1, -1 };

    DiploidSite::DiploidSite(int allele0, int allele1,
                             float logBayesFactor,
                             std::vector<int> alleleForRead)
        : Allele0(allele0),
          Allele1(allele1),
          LogBayesFactor(logBayesFactor),
          AlleleForRead(alleleForRead)
    {}

    static inline float logaddexp(float x, float y)
    {
         float diff = x - y;
         if (diff > 0)  {
             return x + log1p(exp(-diff));
         } else {
             return y + log1p(exp(diff));
         }
    }

    //
    // Computes Pr(R | hom)
    //
    static float HomozygousLogLikelihood(const fmat& siteScores)
    {
        int G = siteScores.size2();
        fvec gScores(G);
        for (int g = 0; g < G; g++)
        {
            gScores(g) = sum(column(siteScores, g));
        }
       return accumulate(gScores.begin(),  gScores.end(), -FLT_MAX, logaddexp);
    }

    //
    // Computes: Pr(R | het)
    //
    static float HeterozygousLogLikelihood(const fmat& siteScores,
                                            int* allele0, int* allele1)
    {
        assert (siteScores.size2() == MUTATIONS_PER_SITE);

        int I = siteScores.size1();
        int G = siteScores.size2();
        float log2 = log(2);

        vector<float> varScores;
        float runningMax = -FLT_MAX;
        int runningAllele0 = -1;
        int runningAllele1 = -1;
        for (int g0 = 0; g0 < G; g0++)
        {
            for (int g1 = g0 + 1; g1 < G; g1++)
            {
                if (LENGTH_DIFFS[g0] == LENGTH_DIFFS[g1])
                {
                    float total = -I * log2;
                    for (int i = 0; i < I; i++)
                    {
                        total += logaddexp(siteScores(i, g0),
                                           siteScores(i, g1));
                    }
                    varScores.push_back(total);
                    if (total > runningMax)
                    {
                        runningMax = total;
                        runningAllele0 = g0;
                        runningAllele1 = g1;
                    }
                }
            }
        }
        if (allele0 != NULL && allele1 != NULL)
        {
            *allele0 = runningAllele0;
            *allele1 = runningAllele1;
        }
        return accumulate(varScores.begin(),  varScores.end(), -FLT_MAX, logaddexp);
    }

    static inline fmat ToMatrix(const float *siteScores, int dim1, int dim2)
    {
        // Kind of kludgy.  I blame ublas--a pretty crummy matrix library.
        fmat M(dim1, dim2);
        std::copy(siteScores, siteScores + dim1*dim2, M.data().begin());
        return M;
    }

#if 0
    static void PrintMatrix(const fmat& siteScores)
    {
        for (int i = 0; i < (int)siteScores.size1(); i++)
        {
            for (int j = 0; j < (int)siteScores.size2(); j++)
            {
                cout << siteScores(i, j) << " ";
            }
            cout << endl;
        }
        cout << endl;
    }
#endif  // 0

    vector<int> AssignReadsToAlleles(const fmat& siteScores, int allele0, int allele1)
    {
        int I = siteScores.size1();
        vector<int> assignment(I, -1);
        for (int i = 0; i < I; i++)
        {
            assignment[i] = (siteScores(i, allele0) > siteScores(i, allele1) ? 0 : 1);
        }
        return assignment;
    }

    // Is the site detected as a heterozygote?
    //  - If not, return NULL.
    //  - If so, return a pointer to a new DiploidSite object
    // logPriorRatio >= 0 is log {Pr(hom)/Pr(het)}
    DiploidSite* IsSiteHeterozygous(const float *siteScores, int dim1, int dim2,
                                    float logPriorRatio)
    {
        // First column of siteScores must correspond to no-op mutation.
        int allele0, allele1;

        fmat M = ToMatrix(siteScores, dim1, dim2);
        float homScore = HomozygousLogLikelihood(M);
        float hetScore = HeterozygousLogLikelihood(M, &allele0, &allele1);
        float logBF = hetScore - homScore;

        if (logBF - logPriorRatio > 0)
        {
            return new DiploidSite(allele0, allele1, logBF,
                                   AssignReadsToAlleles(M, allele0, allele1));
        }
        else
        {
            return NULL;
        }
    }
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <float.h>
#include <string.h>
#include <stdarg.h>

#include <sys/stat.h>
#include <sys/types.h>

#include <algorithm>
#include <cmath>
using namespace std;

#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

#include "search.h"

#ifndef __S_IFMT
#define __S_IFMT 0170000
#endif

#ifndef __S_IFDIR
#define __S_IFDIR 0040000
#endif

inline bool operator<(const search_result &a, const search_result &b)
{
    return a.value < b.value;
}

/*
search_model *load_model(const char *model_file_name, const UInt64 dim, const UInt64 nimages)
{
    using namespace boost::interprocess;

    FILE *fp = fopen(model_file_name, "rb");
    if (0 == fp) {
        fprintf(stderr, "[search.load_model] failed to open model_file %s\n", model_file_name);
        return 0;
    }

    struct stat info;
    stat(model_file_name, &info);
    if( (info.st_mode & __S_IFMT ) == __S_IFDIR) {
        fprintf(stderr, "[search.load_model] %s is a directory\n",  model_file_name);
        fclose(fp);
        return 0;
    }

    //printf("%d %d %d %d\n", sizeof(dim), sizeof(nimages), dim, nimages);

    UInt64 count = dim * nimages;
    search_model *model = new search_model;

    //printf("%d %d\n", sizeof(count), count);

    //fprintf(stdout, "[search.load model] requesting %llu bytes memory ...\n", count * sizeof(DataType));
    model->feature_ptr = new DataType[count];

    if (0 == model->feature_ptr)
    {
        fprintf(stderr, "[search.load_model] Memory error!\n");
        fclose(fp);
        free_model(&model);
        return 0;
    }

    fread((char *)(model->feature_ptr), sizeof(DataType), count, fp);
    fclose(fp);
    model->dim = dim;
    model->nimages = nimages;

    //print_model(model);

    return model;
}
*/

search_model *load_model(const char *model_file_name, const UInt64 dim, const UInt64 nimages)
{
    using namespace boost::interprocess;

    FILE *fp = fopen(model_file_name, "rb");
    if (0 == fp) {
        fprintf(stderr, "[search.load_model] failed to open model_file %s\n", model_file_name);
        return 0;
    }
    fclose(fp);

    struct stat info;
    stat(model_file_name, &info);
    if( (info.st_mode & __S_IFMT ) == __S_IFDIR) {
        fprintf(stderr, "[search.load_model] %s is a directory\n",  model_file_name);
        fclose(fp);
        return 0;
    }

    file_mapping *m_file = new file_mapping(model_file_name, read_only);
    mapped_region *region = new mapped_region(*m_file, read_only);

    UInt64 count = dim * nimages * sizeof(DataType);

    search_model *model = new search_model;
    model->m_file = m_file;
    model->region = region;
    //Get the address of the mapped region
    model->feature_ptr = (DataType*)region->get_address();
    UInt64 region_size  = region->get_size();

    if (0 == model->feature_ptr)
    {
        fprintf(stderr, "[search.load_model] Memory error!\n");
        free_model(&model);
        return 0;
    }

/*    if (count != region_size) {
        fprintf(stderr, "[search.load_model] File size mis-match number of images!\n");
        printf("nimages: %llu\n", nimages);
        printf("dim: %llu\n", dim);
        printf("region_size: %llu\n", region_size);
        printf("count: %llu\n", count);
        free_model(&model);
        return 0;
    }
*/
    model->dim = dim;
    model->nimages = nimages;

    return model;
}

void free_model_contents(search_model* model_ptr)
{
     /*
     if (0 != model_ptr->feature_ptr)
     {
         //fprintf(stdout, "[search.free_model_contents]\n");
         delete [] model_ptr->feature_ptr;
         model_ptr->feature_ptr = 0;
     }
     */
     if (0 != model_ptr->feature_ptr)
     {
         //fprintf(stdout, "[search.free_model_contents]\n");
         delete model_ptr->region;
         delete model_ptr->m_file;
         model_ptr->feature_ptr = 0;
     }
}

void free_model(search_model** model_ptr_ptr)
{
     search_model* model_ptr = *model_ptr_ptr;

     if(0 != model_ptr)
     {
         free_model_contents(model_ptr);
         delete model_ptr;
         model_ptr = 0;
         //fprintf(stdout, "[search.free_model]\n");
     }
}

void print_model(const search_model* model_ptr)
{
    fprintf(stdout, "[search.print_model] %llu images, %llu dims\n", model_ptr->nimages, model_ptr->dim);
}

UInt64 get_dim(const search_model* model_ptr)
{
    return model_ptr->dim;
}

UInt64 get_nr_images(const search_model* model_ptr)
{
    return model_ptr->nimages;
}

// 1 - ((xi * yi) / (norm(x) * norm(y)))

void compute_cosine_distance(const search_model *model, const DataType* query_ptr, double* dist_values)
{
    const DataType *ptr = model->feature_ptr;

    for (UInt64 i=0; i<model->nimages; i++)
    {
        double norm_query = 0;
        double norm_ptr = 0;
        double dist = 0;
        for (UInt64 j=0; j<model->dim; j++)
        {
            norm_query += query_ptr[j] * query_ptr[j];
            norm_ptr += ptr[j] * ptr[j];
            dist += query_ptr[j] * ptr[j];
        }
        ptr += model->dim;
        //fprintf(stdout, "%d %f %f\n", i, dist, sqrt(dist));
        if (norm_query < 1e-8 || norm_ptr < 1e-8) {
            dist_values[i] = (norm_query < 1e-8 && norm_ptr < 1e-8) ? 0 : 1; // define a value for zero input
         } else {
            dist_values[i] = 1. - (dist / (sqrt(norm_query) * sqrt(norm_ptr)));
         }
         //dist_values[i] = 1. - (dist / (sqrt(norm_query) * sqrt(norm_ptr)));
    }
}

void compute_l2_distance(const search_model *model, const DataType* query_ptr, double* dist_values)
{
    const DataType *ptr = model->feature_ptr;

    for (UInt64 i=0; i<model->nimages; i++)
    {
        double dist = 0;
        for (UInt64 j=0; j<model->dim; j++)
        {
            double d = query_ptr[j] - ptr[j];
            dist += (d * d);
            //if (0 == i) fprintf(stdout, "%d %f %f %f\n", j, query_ptr[j], ptr[j], d);
        }
        ptr += model->dim;
        //fprintf(stdout, "%d %f %f\n", i, dist, sqrt(dist));
        dist_values[i] = sqrt(dist);
    }
}

void compute_l1_distance(const search_model *model, const DataType* query_ptr, double* dist_values)
{
    const DataType *ptr = model->feature_ptr;

    for (UInt64 i=0; i<model->nimages; i++)
    {
        double dist = 0;
        for (UInt64 j=0; j<model->dim; j++)
        {
            double d = query_ptr[j] - ptr[j];
            dist += fabs(d);
        }
        ptr += model->dim;
        //fprintf(stdout, "%d %f %f\n", i, dist, sqrt(dist));
        dist_values[i] = dist;
    }
}

/*
 * chi2(x,y) = sum( (xi-yi)^2 / (xi+yi) ) / 2
 */
void compute_chi2_distance(const search_model *model, const DataType* query_ptr, double* dist_values)
{
    const DataType *ptr = model->feature_ptr;
    double dist = 0;
    double d = 0;
    double s = 0;

    for (UInt64 i=0; i<model->nimages; i++)
    {
        dist = 0.0;
        for (UInt64 j=0; j<model->dim; j++)
        {
            d = query_ptr[j] - ptr[j];
            s = query_ptr[j] + ptr[j];
            if (s > 1e-8) {
                dist += (d*d)/s;
            }
        }
        dist /= 2;
        ptr += model->dim;
        //fprintf(stdout, "%d %f %f\n", i, dist, sqrt(dist));
        dist_values[i] = dist;
    }
}

void search_knn(const struct search_model *model, const DataType* query_ptr, const UInt64 k, const int dfunc, struct search_result *results)
{
    double * dist_values = new double[model->nimages];

    switch (dfunc) {
        case 0:
            compute_l1_distance(model, query_ptr, dist_values);
            break;
        case 2:
            compute_chi2_distance(model, query_ptr, dist_values);
            break;
        case 4:
            compute_cosine_distance(model, query_ptr, dist_values);
            break;
        default:
            compute_l2_distance(model, query_ptr, dist_values);
            break;
    }
    /*if (1 == l2) {
        compute_distance(model, query_ptr, dist_values);
    } else {
        compute_l1_distance(model, query_ptr, dist_values);
    } */
    struct search_result *tosort = new search_result[model->nimages];
    for (UInt64 i=0; i<model->nimages; i++)
    {
        tosort[i].index = i;
        tosort[i].value = dist_values[i];
    }
    delete [] dist_values;

    if (k <= (model->nimages >> 1)) {
        partial_sort(tosort, tosort + k, tosort + model->nimages);
    }
    else {
        sort(tosort, tosort + model->nimages);
    }

    for (UInt64 i=0; i<k; i++)
    {
        results[i].index = tosort[i].index;
        results[i].value = tosort[i].value;
    }
    delete [] tosort;
}