//
// Created by joshua on 7/13/17.
//
#include "sim_env/Controller.h"
#include "sim_env/utils/EigenUtils.h"
#include <boost/math/constants/constants.hpp>
#include <cmath>

using namespace sim_env;

sim_env::PIDController::PIDController(float kp, float ki, float kd)
{
    _kp = kp;
    _ki = ki;
    _kd = kd;
    reset();
}

sim_env::PIDController::~PIDController()
{
    // nothing to do here
}

void sim_env::PIDController::setKp(float kp)
{
    _kp = kp;
}

void sim_env::PIDController::setKi(float ki)
{
    _ki = ki;
}

void sim_env::PIDController::setKd(float kd)
{
    _kd = kd;
}

void sim_env::PIDController::setGains(float kp, float ki, float kd)
{
    setKp(kp);
    setKi(ki);
    setKd(kd);
}

void sim_env::PIDController::setTarget(float target_state)
{
    if (target_state != _target) {
        reset();
        _target = target_state;
    }
}

float sim_env::PIDController::getTarget() const
{
    return _target;
}

bool sim_env::PIDController::isTargetSatisfied(float current_state, float threshold) const
{
    return std::fabs(current_state - _target) < threshold;
}

float sim_env::PIDController::control(float current_state)
{
    float error = _target - current_state;
    float delta_error = 0.0f;
    if (not std::isnan(_prev_error)) {
        delta_error = error - _prev_error;
    }
    _prev_error = error;
    _integral_part += error;
    float output = _kp * error + _ki * _integral_part + _kd * delta_error;
    //    if (isTargetSatisfied(current_state, 0.001f)) {
    //        auto logger = DefaultLogger::getInstance();
    //        std::stringstream ss;
    //        ss << "target is satisfied. output is " << output;
    //        ss << "error is " << error << " current state is " << current_state;
    //        ss << "kp: " << _kp << " ki" << _ki << "kd" << _kd;
    //        logger->logDebug(ss.str());
    //    }
    return output;
}

void sim_env::PIDController::reset()
{
    _integral_part = 0.0;
    _prev_error = nanf("");
}

////////////////////// IndependentMDPIDController //////////////////////////////
IndependentMDPIDController::IndependentMDPIDController(float kp, float ki, float kd)
{
    _default_ki = ki;
    _default_kp = kp;
    _default_kd = kd;
    setGains(kp, ki, kd);
}

IndependentMDPIDController::~IndependentMDPIDController()
{
    // nothing to do here.
}

void IndependentMDPIDController::setTarget(const Eigen::VectorXf& target_state)
{
    if (target_state.size() != _controllers.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setTarget]"
                                 "Invalid input state dimension.");
    }
    for (size_t i = 0; i < target_state.size(); ++i) {
        _controllers.at(i).setTarget(target_state[i]);
    }
}

void IndependentMDPIDController::getTarget(Eigen::VectorXf& target_state) const
{
    target_state.resize(_controllers.size());
    for (size_t i = 0; i < _controllers.size(); ++i) {
        target_state[i] = _controllers.at(i).getTarget();
    }
}

void IndependentMDPIDController::control(Eigen::VectorXf& output,
    const Eigen::VectorXf& current_state)
{
    if (_controllers.size() != current_state.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::control]"
                                 "Invalid input state dimension.");
    }
    output.resize(_controllers.size());
    for (size_t i = 0; i < _controllers.size(); ++i) {
        output[i] = _controllers.at(i).control(current_state[i]);
    }
}

void IndependentMDPIDController::reset()
{
    for (auto& controller : _controllers) {
        controller.reset();
    }
}

bool IndependentMDPIDController::isTargetSatisfied(Eigen::VectorXf& current_state, float threshold) const
{
    bool target_satisfied = true;
    for (size_t i = 0; i < _controllers.size(); ++i) {
        target_satisfied = target_satisfied and _controllers.at(i).isTargetSatisfied(current_state[i], threshold);
    }
    return target_satisfied;
}

void IndependentMDPIDController::setGains(float kp, float ki, float kd)
{
    for (auto& controller : _controllers) {
        controller.setGains(kp, ki, kd);
    }
}

void IndependentMDPIDController::setGains(const Eigen::VectorXf& kps, const Eigen::VectorXf& kis, const Eigen::VectorXf& kds)
{
    if (_controllers.size() != kps.size() or _controllers.size() != kis.size() or _controllers.size() != kds.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setGains]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setGains(kps[i], kis[i], kds[i]);
    }
}

void IndependentMDPIDController::setKps(const Eigen::VectorXf& kps)
{
    if (_controllers.size() != kps.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKps]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKp(kps[i]);
    }
}

void IndependentMDPIDController::setKp(float kp)
{
    for (auto& controller : _controllers) {
        controller.setKp(kp);
    }
}

void IndependentMDPIDController::setKis(const Eigen::VectorXf& kis)
{
    if (_controllers.size() != kis.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKis]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKi(kis[i]);
    }
}

void IndependentMDPIDController::setKi(float ki)
{
    for (auto& controller : _controllers) {
        controller.setKi(ki);
    }
}

void IndependentMDPIDController::setKds(const Eigen::VectorXf& kds)
{
    if (_controllers.size() != kds.size()) {
        throw std::runtime_error("[sim_env::IndependentMDPIDController::setKds]"
                                 "Invalid input vector dimension.");
    }
    for (size_t i = 0; i < _controllers.size(); ++i) {
        _controllers.at(i).setKd(kds[i]);
    }
}

void IndependentMDPIDController::setKd(float kd)
{
    for (auto& controller : _controllers) {
        controller.setKd(kd);
    }
}

unsigned int IndependentMDPIDController::getStateDimension()
{
    return (unsigned int)_controllers.size();
}

void IndependentMDPIDController::setStateDimension(unsigned int dim)
{
    if (dim != _controllers.size()) {
        unsigned int prev_dim = (unsigned int)_controllers.size();
        _controllers.resize(dim);
        reset();
        for (unsigned int i = prev_dim; i < _controllers.size(); ++i) {
            _controllers.at(i).setGains(_default_kp, _default_ki, _default_kd);
        }
    }
}

//*************************** RobotController ************************************//
RobotController::~RobotController() = default;

//*************************** RobotPositionController ****************************//
RobotPositionController::RobotPositionController(RobotPtr robot,
    RobotVelocityControllerPtr velocity_controller)
    : _pid_controller(1.0, 0.0, 0.0)
    , _velocity_controller(velocity_controller)
    , _robot(robot)
{
}

RobotPositionController::~RobotPositionController()
{
}

void RobotPositionController::setPositionProjectionFn(PositionProjectionFn pos_constraint)
{
    _pos_proj_fn = pos_constraint;
}

void RobotPositionController::setVelocityProjectionFn(VelocityProjectionFn vel_constraint)
{
    _vel_proj_fn = vel_constraint;
}

void RobotPositionController::setTarget(const Eigen::VectorXf& position)
{
    setTargetPosition(position);
}

void RobotPositionController::setTargetPosition(const Eigen::VectorXf& position)
{
    if (_robot.expired()) {
        LoggerPtr logger = DefaultLogger::getInstance();
        logger->logErr("Can not access underlying robot; the pointer is not valid anymore!",
            "[sim_env::RobotPositionController::setTargetPosition]");
    }
    RobotPtr robot = _robot.lock();
    LoggerPtr logger = robot->getWorld()->getLogger();
    // std::stringstream ss;
    // ss << "Setting target position " << position.transpose();
    // logger->logDebug(ss.str(), "[sim_env::RobotPositionController::setTargetPosition]");
    _pid_controller.setStateDimension((unsigned int)position.size());
    _pid_controller.setTarget(position);
}

unsigned int RobotPositionController::getTargetDimension() const
{
    auto robot = _robot.lock();
    return robot->getNumActiveDOFs();
}

RobotPtr RobotPositionController::getRobot() const
{
    return _robot.lock();
}

inline float cyclicPositionError(const Eigen::Array2f& pos_range, float pos, float target)
{
    float error = target - pos;
    float overflow_error = pos_range[1] - pos + target - pos_range[0];
    float underflow_error = pos_range[0] - pos + target - pos_range[1];
    error = std::abs(error) < std::abs(overflow_error) ? error : overflow_error;
    error = std::abs(error) < std::abs(underflow_error) ? error : underflow_error;
    return error;
}

bool RobotPositionController::control(const Eigen::VectorXf& positions, const Eigen::VectorXf& velocities,
    float timestep, RobotConstPtr robot,
    Eigen::VectorXf& output)
{
    Eigen::VectorXf target_position;
    _pid_controller.getTarget(target_position);
    if (target_position.size() != robot->getActiveDOFs().size()) {
        LoggerPtr logger = robot->getWorld()->getLogger();
        logger->logErr("The provided target position has different dimension from the active DOFs."
                       "[sim_env::RobotPositionController::setTargetPosition]");
        std::stringstream ss;
        ss << "target size is " << target_position.size() << " active dof size is " << robot->getNumActiveDOFs();
        logger->logErr(ss.str());
        return false;
    }
    // project target position onto constraint set (this should only do anything if the user set invalid target positions)
    if (_pos_proj_fn) {
        _pos_proj_fn(target_position, robot);
    }
    Eigen::VectorXf target_velocities(positions.size());
    //    _pid_controller.control(target_velocities, positions);
    // TODO see whether we still can use a PID somehow
    // TODO this is not moving in a straight line. we could make the decision on
    // TODO whether to move in a straight line or not dependent on a parameter
    // Eigen::ArrayX2f velocity_limits = robot->getDOFVelocityLimits();
    // Eigen::ArrayX2f acceleration_limits = robot->getDOFAccelerationLimits();
    // Eigen::VectorXf delta_position = target_position - positions;
    Eigen::VectorXi dof_indices = robot->getActiveDOFs();
    assert(dof_indices.size() == positions.size());
    DOFInformation dof_info;
    for (int idx = 0; idx < dof_indices.size(); ++idx) {
        // get dof information
        robot->getDOFInformation(dof_indices[idx], dof_info);
        float delta_position = target_position[idx] - positions[idx];
        if (dof_info.cyclic) {
            delta_position = cyclicPositionError(dof_info.position_limits, positions[idx], target_position[idx]);
        }
        // first, command max velocity for each dof separately
        float velocity_sign(1.0f);
        if (std::signbit(delta_position)) {
            // need negative velocity
            target_velocities[idx] = dof_info.velocity_limits[0];
            assert(std::signbit(target_velocities[idx]));
            velocity_sign = -1.0f;
        } else {
            // need positive velocity
            target_velocities[idx] = dof_info.velocity_limits[1];
            velocity_sign = 1.0f;
        }
        // next compute the maximum velocity we can have to not overshoot
        float abs_position_error = std::abs(delta_position);
        float abs_max_break_accel = 0.0f;
        if (std::signbit(velocities[idx])) {
            abs_max_break_accel = std::abs(dof_info.acceleration_limits[1]);
        } else {
            abs_max_break_accel = std::abs(dof_info.acceleration_limits[0]);
        }
        float abs_max_break_velocity = std::sqrt(2.0f * abs_position_error * abs_max_break_accel);
        // finally, set the target velocity such maximal, but such that we do not overshoot.
        target_velocities[idx] = velocity_sign * std::min(abs_max_break_velocity, std::abs(target_velocities[idx]));
    }
    // { // TODO delete this block

    //     auto robot = _robot.lock();
    //     auto world = robot->getWorld();
    //     auto logger = world->getLogger();
    //     logger->logDebug(boost::format("Position error: x:%1%, y: %2%, theta: %3%") % delta_position[0] % delta_position[1] % delta_position[2], "[sim_env::PositionController]");
    // }
    if (_vel_proj_fn) {
        _vel_proj_fn(target_velocities, robot);
    }
    _velocity_controller->setTargetVelocity(target_velocities);
    _velocity_controller->control(positions, velocities, timestep, robot, output);
    return true;
}

///////////////////////////// RobotVelocityController ///////////////////////////////
//RobotVelocityController::RobotVelocityController(RobotPtr robot):
//        _pid_controller(10.0, 0.0, 0.0),
//        _robot(robot) {
//}

RobotVelocityController::~RobotVelocityController() = default;

void RobotVelocityController::setTarget(const Eigen::VectorXf& target)
{
    setTargetVelocity(target);
}

//void RobotVelocityController::setTargetVelocity(const Eigen::VectorXf &velocity) {
//    if (_robot.expired()) {
//        LoggerPtr logger = DefaultLogger::getInstance();
//        logger->logErr("Can not access underlying robot; the pointer is not valid anymore!",
//                       "[sim_env::RobotVelocityController::setTargetVelocity]");
//    }
//    RobotPtr robot = _robot.lock();
//    Eigen::VectorXf new_target = velocity;
//    LoggerPtr logger = robot->getWorld()->getLogger();
//    // scale the target velocity to limits
//    using namespace utils::eigen;
//    ScalingResult scaling_result = scaleToLimits(new_target, robot->getDOFVelocityLimits());
//    if (scaling_result == ScalingResult::Failure) {
//        logger->logWarn("Impossible target velocity direction detected. The requested velocity can not be scaled to the given limits",
//                        "[sim_env::RobotVelocityController::setTargetVelocity]");
//    } else if (scaling_result == ScalingResult::Scaled) {
//        logger->logWarn("Requested velocity is out of limits. Scaling it down.",
//                        "[sim_env::RobotVelocityController::setTargetVelocity]");
//    }
//    std::stringstream ss;
//    ss << "Setting target velocity " << new_target.transpose();
//    logger->logDebug(ss.str(), "[sim_env::RobotVelocityController::setTargetVelocity]");
//    // set the new target
//    _pid_controller.setStateDimension((unsigned int) new_target.size());
//    _pid_controller.setTarget(new_target);
//}
//
//bool RobotVelocityController::control(const Eigen::VectorXf &positions, const Eigen::VectorXf &velocities,
//                                      float timestep, RobotConstPtr robot,
//                                      Eigen::VectorXf &output) {
//    Eigen::VectorXf target_velocity;
//    _pid_controller.getTarget(target_velocity);
//    if (target_velocity.size() != robot->getActiveDOFs().size()) {
//        LoggerPtr logger = robot->getWorld()->getLogger();
//        logger->logErr("The provided target position has different dimension from the active DOFs."
//                               "[sim_env::RobotVelocityController::setTargetVelocity]");
//        return false;
//    }
//    _pid_controller.control(output, velocities);
////    LoggerPtr logger = robot->getWorld()->getLogger();
////    std::stringstream ss;
////    ss << "Target velocities are " << target_velocity.transpose();
////    ss << " Current velocities are " << velocities.transpose();
////    logger->logDebug(ss.str());
//    // TODO limit efforts?
//    return true;
//}
//
//IndependentMDPIDController& RobotVelocityController::getPIDController() {
//   return _pid_controller;
//}
//
//LoggerPtr RobotVelocityController::getLogger() {
//    if (_robot.expired()) {
//        return DefaultLogger::getInstance();
//    }
//    RobotPtr robot = _robot.lock();
//    return robot->getWorld()->getLogger();
//}

SE2RobotPositionController::SE2RobotPositionController(RobotPtr robot, RobotVelocityControllerPtr velocity_controller)
    : _robot(robot)
    , _velocity_controller(velocity_controller)
{
    assert(robot->getNumDOFs() >= 3);
    assert(!robot->isStatic());
    // velocity limits
    auto dof_vel_limits = robot->getDOFVelocityLimits();
    float x_limit = std::min(std::abs(dof_vel_limits(0, 0)), std::abs(dof_vel_limits(0, 1)));
    float y_limit = std::min(std::abs(dof_vel_limits(1, 0)), std::abs(dof_vel_limits(1, 1)));
    _cartesian_vel_limit = std::min(x_limit, y_limit);
    _angular_vel_limit = std::min(std::abs(dof_vel_limits(2, 0)), std::abs(dof_vel_limits(2, 1)));
    // accelaration limits
    auto dof_acc_limits = robot->getDOFAccelerationLimits();
    x_limit = std::min(std::abs(dof_acc_limits(0, 0)), std::abs(dof_acc_limits(0, 1)));
    y_limit = std::min(std::abs(dof_acc_limits(1, 0)), std::abs(dof_acc_limits(1, 1)));
    _cartesian_acc_limit = std::min(x_limit, y_limit);
    _angular_acc_limit = std::min(std::abs(dof_acc_limits(2, 0)), std::abs(dof_acc_limits(2, 1)));
    _set_point.resize(3);
}

SE2RobotPositionController::~SE2RobotPositionController()
{
}

void SE2RobotPositionController::setPositionProjectionFn(PositionProjectionFn pos_constraint)
{
    _pos_proj_fn = pos_constraint;
}

void SE2RobotPositionController::setVelocityProjectionFn(VelocityProjectionFn vel_constraint)
{
    _vel_proj_fn = vel_constraint;
}

inline float normlizeOrientation(float v)
{
    const float pi = boost::math::constants::pi<float>();
    float sign = std::signbit(v) ? -1.0f : 1.0f;
    return sign * (std::abs(v) - std::floor((std::abs(v) + pi) / (2.0f * pi)) * 2.0f * pi);
}

inline float shortestSO2Direction(float val_1, float val_2)
{
    float value = val_2 - val_1;
    if (std::abs(value) > boost::math::constants::pi<float>()) {
        if (value > 0.0f) { // val_2 > val_1
            value -= 2.0f * boost::math::constants::pi<float>();
        } else {
            value += 2.0f * boost::math::constants::pi<float>();
        }
    }
    return value;
}

void SE2RobotPositionController::setTarget(const Eigen::VectorXf& target)
{
    static const std::string log_prefix("[SE2RobotPositionController::setTarget]");
    if (target.size() != 3) {
        auto robot = getRobot();
        auto logger = robot->getWorld()->getLogger();
        logger->logErr(boost::format("Target has invalid dimension. Expected dimension is 3, actual dimension is %1%") % target.size(), log_prefix);
        return;
    }
    _last_target = target;
    // normalize target orientation
    _last_target[2] = normlizeOrientation(_last_target[2]);

    // logger->logDebug(boost::format("Target %1%, %2%, %3%") % _last_target[0] % _last_target[1] % _last_target[2], log_prefix);
}

void SE2RobotPositionController::setTargetPosition(const Eigen::VectorXf& position)
{
    setTarget(position);
}

unsigned int SE2RobotPositionController::getTargetDimension() const
{
    return 3;
}

RobotPtr SE2RobotPositionController::getRobot() const
{
    auto robot = _robot.lock();
    if (!robot) {
        std::logic_error("[SE2RobotPositionController::getRobot] Could not acquire robot. Weak pointer expired.");
    }
    return robot;
}

bool SE2RobotPositionController::control(const Eigen::VectorXf& positions,
    const Eigen::VectorXf& velocities,
    float timestep,
    RobotConstPtr robot,
    Eigen::VectorXf& output)
{
    static const std::string log_prefix("[SE2RobotPositionController::control]");
    auto logger = robot->getConstWorld()->getConstLogger();
    if (_last_target.size() != 3) {
        logger->logWarn("No target set. Can not control any position", log_prefix);
        return false;
    }
    assert(positions.size() >= 3);
    // compute error in cartesian position
    Eigen::Vector2f cart_error = _last_target.head(2) - positions.head(2);
    // logger->logDebug(boost::format("Target %1%, %2%, %3%") % _last_target[0] % _last_target[1] % _last_target[2], log_prefix);
    float cart_error_norm = cart_error.norm();
    if (cart_error_norm > 0.0f) {
        cart_error /= cart_error_norm;
    }
    // compute the maximum velocity we can have to not overshoot
    float abs_max_break_velocity = std::sqrt(2.0f * cart_error_norm * _cartesian_acc_limit);
    float cart_vel = std::min(abs_max_break_velocity, _cartesian_vel_limit);
    // do the same for angular velocity
    // float angular_error = _last_target[2] - positions[2];
    float angular_error = shortestSO2Direction(positions[2], _last_target[2]);
    float angular_error_norm = std::abs(angular_error);
    float max_break_velocity_angular = std::sqrt(2.0f * angular_error_norm * _angular_acc_limit);
    float omega = std::min(max_break_velocity_angular, _angular_vel_limit);
    // if we are at our target position, command zero velocity, else compute a velocity taking us to our target
    if (angular_error_norm < 1e-4 and cart_error_norm < 1e-4) {
        _set_point.setZero();
    } else {
        // debug log
        // logger->logDebug(boost::format("cart_error: %1%, cart_vel: %2%, angular_error: %3%, angular_vel: %4%") % cart_error_norm % cart_vel % angular_error % omega, log_prefix);
        // now compute how much time we will need for each to reach its destination
        // we travel for some time at max velocity, followed by a decelaration phase
        // in the decelaration we slow down from v to 0, i.e. its duration is v / a. The distance we travel
        // in the decelaration is 0.5f * v / a. Accordingly, the time we are at the max velocity is t = s / v - 0.5 * v / a.
        // If we are in the deceleration phase already, t becomes negative.
        float t_angular_plateau = 0.0f;
        if (angular_error_norm > 0.0f) {
            t_angular_plateau = angular_error_norm / omega - 0.5f * omega / _angular_acc_limit;
        }
        float t_cart_plateau = 0.0f;
        if (cart_error_norm > 0.0f) {
            t_cart_plateau = cart_error_norm / cart_vel - 0.5f * cart_vel / _cartesian_acc_limit;
        }
        float t_angular = std::max(t_angular_plateau, 0.0f) + omega / _angular_acc_limit;
        float t_cart = std::max(t_cart_plateau, 0.0f) + cart_vel / _cartesian_acc_limit;
        // logger->logDebug(boost::format("Estimated duration till target, cart: %1%, angular %2%") % t_cart % t_angular, log_prefix);
        if (t_cart < t_angular) { // we will take more time to reach our angle destination, slow down cartesian
            float a = _cartesian_acc_limit * t_angular;
            float b = _cartesian_acc_limit * _cartesian_acc_limit * t_angular * t_angular - 2.0f * _cartesian_acc_limit * cart_error_norm;
            assert(b >= 0.0f);
            cart_vel = a - std::sqrt(b);
        } else { // we will take more time to reach our cartesian destination, slow down angular
            float a = _angular_acc_limit * t_cart;
            float b = _angular_acc_limit * _angular_acc_limit * t_cart * t_cart - 2.0f * _angular_acc_limit * angular_error_norm;
            assert(b >= 0.0f);
            omega = a - std::sqrt(b);
        }
        // logger->logDebug(boost::format("Commanded velocities: cart: %1%, angular: %2%") % cart_vel % omega, log_prefix);
        // logger->logDebug(boost::format("Cartesian velocity direction (x, y): %1%, %2%") % cart_error[0] % cart_error[1], log_prefix);
        _set_point.head(2) = cart_vel * cart_error;
        _set_point[2] = omega * angular_error / angular_error_norm;
        // TODO We should also define the velocities during acceleration phase. For large position errors and max velocities,
        // TODO it takes the robot different amount of time to accelerate to the respective target velocity for each DoF.
        // TODO Hence, the robot will not move all the time in the direction that we command (this depends on the underlying velocity controller of course).
        // TODO Alternatively, could define a velocity controller that operates in the same way as this position controller, i.e.
        // TODO one that ensures that a commanded target velocity is reached synchronously for all DOFs.
    }
    _velocity_controller->setTargetVelocity(_set_point);
    _velocity_controller->control(positions, velocities, timestep, robot, output);
    return true;
}


#pragma once

#include <csv2/writer.hpp>

#include <fmt/core.h>

#include <boost/math/special_functions/sign.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/numeric.hpp>

#include <cassert>
#include <cmath>
#include <iostream>
#include <limits>
#include <numeric>
#include <ranges>
#include <tuple>
#include <type_traits>

namespace profitview::util
{

class CsvWriter : public csv2::Writer<csv2::delimiter<','>>
{    // Provides shorthand
     // `write()`
public:
    CsvWriter(auto& stream)
        : csv2::Writer<csv2::delimiter<','>>(stream)
    {}

    void write(auto&&... args)
    {
        write_row<std::vector<std::string>>({fmt::format("{}", std::forward<decltype(args)>(args))...});
    }
};

// boost::accumulate should do this, but there's a compile fail
inline auto accumulate(const auto& s, auto i) -> auto { return std::accumulate(std::begin(s), std::end(s), i); }

inline auto accumulate(auto& s, auto i, auto op) -> auto { return std::accumulate(std::begin(s), std::end(s), i, op); }

auto stdev(auto& s, auto m, int p) -> auto
{    // Calculate standard deviation given mean (m)
    auto const variance{[&m, &p](auto a, const auto& v) { return a + (v - m) * (v - m) / (p - 1);}};

    return std::sqrt(accumulate(s, 0.0, variance));
}

auto ma(auto const& s, int p = 0) -> auto { return accumulate(s, 0.0) / (p ? p : s.size()); }

// Exponential Moving Average (ema) difference formula from
// https://en.wikipedia.org/wiki/Moving_average:
auto ema(auto const& s, auto p, auto m = 0) -> auto
{
    auto alpha{2.0f / (p - 1)};
    auto const ema_step{[&alpha](auto a, const auto& price) { return price * alpha + a * (1 - alpha); }};

    return accumulate(s, m, ema_step);
}

auto abs_differences(const auto& prices, int e) -> auto
{
    auto b{prices.end() - e};
    assert(prices.size() > e);

    using namespace std::ranges;

    subrange lagged{b - 1, prices.end() - 1}, aligned{b, prices.end()};
    std::vector<typename std::remove_cvref_t<decltype(prices)>::value_type> differences(e);
    transform(
        lagged, aligned, differences.begin(), [](auto n, auto m) -> auto { return std::abs(n - m); });

    return std::make_tuple(differences, prices.back() - *b);
}

auto is_monotonic(auto const& s) -> std::tuple<bool, bool>    // { monotonic, up }
{                                                             // If monotonic     ascending      { true,  true  }
    // If monotonic     descending     { true,  false }
    // If non-monotonic now ascending  { false, true  }
    // If non-monotonic now descending { false, false }
    auto sgn = [](int a, int b) { return a < b ? 1 : (a == b) ? 0 : -1; };
    int prev = 0;
    for (int i = 0; i < s.size() - 1; ++i)
    {
        int c = sgn(s[i], s[i + 1]);
        if (c != 0)
        {
            if (c != prev && prev != 0)
            {
                return {false, c == 1};
            }
            prev = c;
        }
    }
    return {true, prev == 1};
}

}    // namespace profitview::util


#pragma once

#include "rng.hpp"
#include "utils.hpp"
#include <boost/range/numeric.hpp>
#include <random>

/**
 * Returns a randomly selected element of a vector.
 */
template <class T>
T select_random_element(std::vector<T> v,
                        std::mt19937& gen,
                        std::uniform_real_distribution<double>& uni_dist) {
  using namespace std;
  T element;
  if (v.size() == 0) {
    throw runtime_error(
        "Vector is empty, so we cannot select a random element from it. "
        "(function: select_random_element)\n");
  }
  else if (v.size() == 1) {
    element = v[0];
  }
  else {
    // Sample a double between [0, 1]
    double rand_val = uni_dist(gen);

    // Convert random double between [0, 1] into
    // an integer between [0, v.size() - 1
    int idx = trunc(rand_val * v.size());
    idx = (idx == v.size() ? idx-- : idx);

    element = v[idx];
  }
  return element;
}

double sample_from_normal(double mu, double sd);

/**
 * The KDE class implements 1-D Gaussian kernel density estimators.
 */
class KDE {
  public:
  KDE(){};
  std::vector<double> dataset = {};
  std::vector<double> log_prior_hist = {};
  double delta_theta = 1;
  int n_bins = 1;
  double bw; // bandwidth
  KDE(std::vector<double>);
  KDE(std::vector<double>, int n_bins);
  void set_num_bins(int n_bins);
  int theta_to_bin(double theta);

  // TODO: Made this public just to initialize β.
  // Not sure this is the correct way to do it.
  double mu = 0;
  double most_probable_theta = 0;

  std::vector<double> resample(int n_samples,
                               std::mt19937& rng,
                               std::uniform_real_distribution<double>& uni_dist,
                               std::normal_distribution<double>& norm_dist);
  std::vector<double> resample(int n_samples);
  double pdf(double x);
  std::vector<double> pdf(std::vector<double> v);
  double logpdf(double x);
};


/*
 * Copyright (c) 2015-2021 Agalmic Ventures LLC (www.agalmicventures.com)
 *
 * 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.
 */

#include <boost/test/unit_test.hpp>

#include "Werk/Utility/Bits.hpp"

BOOST_AUTO_TEST_SUITE(BitsTest)

BOOST_AUTO_TEST_CASE(TestBitmask)
{
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(0), 0x00);
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(1), 0x01);
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(2), 0x03);
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(3), 0x07);
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(4), 0x0F);
	BOOST_CHECK_EQUAL(bitmask<uint64_t>(5), 0x1F);
}

BOOST_AUTO_TEST_CASE(TestRoundUpPow2)
{
	//Simple shifting
	uint64_t x = 0x0E;
	uint64_t y = 0x10;
	for (size_t i = 0; i < 32; ++i) {
		BOOST_CHECK_EQUAL(roundUpPow2(x), y);
		x *= 0x10;
		y *= 0x10;
	}

	//Types matter
	BOOST_CHECK_EQUAL(roundUpPow2(0xDEADBEEF), 0);
	BOOST_CHECK_EQUAL(roundUpPow2<uint32_t>(0xDEADBEEF), 0);
	BOOST_CHECK_EQUAL(roundUpPow2<uint64_t>(0xDEADBEEF), 0x100000000);
}

BOOST_AUTO_TEST_CASE(TestPopCount64)
{
	for (size_t i = 0; i < 64; ++i) {
		uint64_t value = bitmask<uint64_t>(i);
		BOOST_CHECK_EQUAL(popCount64(value), i);
	}
}

BOOST_AUTO_TEST_SUITE_END()


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "nrosy.h"

#include <igl/copyleft/comiso/nrosy.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/edge_topology.h>
#include <igl/per_face_normals.h>

#include <iostream>
#include <fstream>

#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <queue>

#include <gmm/gmm.h>
#include <CoMISo/Solver/ConstrainedSolver.hh>
#include <CoMISo/Solver/MISolver.hh>
#include <CoMISo/Solver/GMM_Tools.hh>

namespace igl
{
namespace copyleft
{

namespace comiso
{
class NRosyField
{
public:
  // Init
  IGL_INLINE NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F);

  // Generate the N-rosy field
  // N degree of the rosy field
  // roundseparately: round the integer variables one at a time, slower but higher quality
  IGL_INLINE void solve(const int N = 4);

  // Set a hard constraint on fid
  // fid: face id
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintHard(const int fid, const Eigen::Vector3d& v);

  // Set a soft constraint on fid
  // fid: face id
  // w: weight of the soft constraint, clipped between 0 and 1
  // v: direction to fix (in 3d)
  IGL_INLINE void setConstraintSoft(const int fid, const double w, const Eigen::Vector3d& v);

  // Set the ratio between smoothness and soft constraints (0 -> smoothness only, 1 -> soft constr only)
  IGL_INLINE void setSoftAlpha(double alpha);

  // Reset constraints (at least one constraint must be present or solve will fail)
  IGL_INLINE void resetConstraints();

  // Return the current field
  IGL_INLINE Eigen::MatrixXd getFieldPerFace();

  // Return the current field (in Ahish's ffield format)
  IGL_INLINE Eigen::MatrixXd getFFieldPerFace();

  // Compute singularity indexes
  IGL_INLINE void findCones(int N);

  // Return the singularities
  IGL_INLINE Eigen::VectorXd getSingularityIndexPerVertex();

private:

  // Compute angle differences between reference frames
  IGL_INLINE void computek();

  // Remove useless matchings
  IGL_INLINE void reduceSpace();

  // Prepare the system matrix
  IGL_INLINE void prepareSystemMatrix(const int N);

  // Solve without roundings
  IGL_INLINE void solveNoRoundings();

  // Solve with roundings using CoMIso
  IGL_INLINE void solveRoundings();

  // Round all p to 0 and fix
  IGL_INLINE void roundAndFixToZero();

  // Round all p and fix
  IGL_INLINE void roundAndFix();

  // Convert a vector in 3d to an angle wrt the local reference system
  IGL_INLINE double convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v);

  // Convert an angle wrt the local reference system to a 3d vector
  IGL_INLINE Eigen::Vector3d convertLocalto3D(unsigned fid, double a);

  // Compute the per vertex angle defect
  IGL_INLINE Eigen::VectorXd angleDefect();

  // Temporary variable for the field
  Eigen::VectorXd angles;

  // Hard constraints
  Eigen::VectorXd hard;
  std::vector<bool> isHard;

  // Soft constraints
  Eigen::VectorXd soft;
  Eigen::VectorXd wSoft;
  double          softAlpha;

  // Face Topology
  Eigen::MatrixXi TT, TTi;

  // Edge Topology
  Eigen::MatrixXi EV, FE, EF;
  std::vector<bool> isBorderEdge;

  // Per Edge information
  // Angle between two reference frames
  Eigen::VectorXd k;

  // Jumps
  Eigen::VectorXi p;
  std::vector<bool> pFixed;

  // Mesh
  Eigen::MatrixXd V;
  Eigen::MatrixXi F;

  // Normals per face
  Eigen::MatrixXd N;

  // Singularity index
  Eigen::VectorXd singularityIndex;

  // Reference frame per triangle
  std::vector<Eigen::MatrixXd> TPs;

  // System stuff
  Eigen::SparseMatrix<double> A;
  Eigen::VectorXd b;
  Eigen::VectorXi tag_t;
  Eigen::VectorXi tag_p;

};

} // NAMESPACE COMISO
} // NAMESPACE COPYLEFT
} // NAMESPACE IGL

igl::copyleft::comiso::NRosyField::NRosyField(const Eigen::MatrixXd& _V, const Eigen::MatrixXi& _F)
{
  using namespace std;
  using namespace Eigen;

  V = _V;
  F = _F;

  assert(V.rows() > 0);
  assert(F.rows() > 0);

  // Generate topological relations
  igl::triangle_triangle_adjacency(F,TT,TTi);
  igl::edge_topology(V,F, EV, FE, EF);

  // Flag border edges
  isBorderEdge.resize(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    isBorderEdge[i] = (EF(i,0) == -1) || ((EF(i,1) == -1));

  // Generate normals per face
  igl::per_face_normals(V, F, N);

  // Generate reference frames
  for(unsigned fid=0; fid<F.rows(); ++fid)
  {
    // First edge
    Vector3d e1 = V.row(F(fid,1)) - V.row(F(fid,0));
    e1.normalize();
    Vector3d e2 = N.row(fid);
    e2 = e2.cross(e1);
    e2.normalize();

    MatrixXd TP(2,3);
    TP << e1.transpose(), e2.transpose();
    TPs.push_back(TP);
  }

  // Alloc internal variables
  angles = VectorXd::Zero(F.rows());
  p = VectorXi::Zero(EV.rows());
  pFixed.resize(EV.rows());
  k = VectorXd::Zero(EV.rows());
  singularityIndex = VectorXd::Zero(V.rows());

  // Reset the constraints
  resetConstraints();

  // Compute k, differences between reference frames
  computek();

  softAlpha = 0.5;
}

void igl::copyleft::comiso::NRosyField::setSoftAlpha(double alpha)
{
  assert(alpha >= 0 && alpha < 1);
  softAlpha = alpha;
}

void igl::copyleft::comiso::NRosyField::prepareSystemMatrix(const int N)
{
  using namespace std;
  using namespace Eigen;

  double Nd = N;

  // Minimize the MIQ energy
  // Energy on edge ij is
  //     (t_i - t_j + kij + pij*(2*pi/N))^2
  // Partial derivatives:
  //   t_i: 2     ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //   t_j: 2     (-t_i + t_j - kij - pij*(2*pi/N)) = 0
  //   pij: 4pi/N ( t_i - t_j + kij + pij*(2*pi/N)) = 0
  //
  //          t_i      t_j         pij       kij
  // t_i [     2       -2           4pi/N      2    ]
  // t_j [    -2        2          -4pi/N     -2    ]
  // pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]

  // Count and tag the variables
  tag_t = VectorXi::Constant(F.rows(),-1);
  vector<int> id_t;
  int count = 0;
  for(unsigned i=0; i<F.rows(); ++i)
    if (!isHard[i])
    {
      tag_t(i) = count++;
      id_t.push_back(i);
    }

  unsigned count_t = id_t.size();

  tag_p = VectorXi::Constant(EF.rows(),-1);
  vector<int> id_p;
  for(unsigned i=0; i<EF.rows(); ++i)
  {
    if (!pFixed[i])
    {
      // if it is not fixed then it is a variable
      tag_p(i) = count++;
    }

    // if it is not a border edge,
    if (!isBorderEdge[i])
    {
      // and it is not between two fixed faces
      if (!(isHard[EF(i,0)] && isHard[EF(i,1)]))
      {
          // then it participates in the energy!
          id_p.push_back(i);
      }
    }
  }

  unsigned count_p = count - count_t;
  // System sizes: A (count_t + count_p) x (count_t + count_p)
  //               b (count_t + count_p)

  b = VectorXd::Zero(count_t + count_p);

  std::vector<Eigen::Triplet<double> > T;
  T.reserve(3 * 4 * count_p);

  for(unsigned r=0; r<id_p.size(); ++r)
  {
    int eid = id_p[r];
    int i = EF(eid,0);
    int j = EF(eid,1);
    bool isFixed_i = isHard[i];
    bool isFixed_j = isHard[j];
    bool isFixed_p = pFixed[eid];
    int row;
    // (i)-th row: t_i [     2       -2           4pi/N      2    ]
    if (!isFixed_i)
    {
      row = tag_t[i];
      if (isFixed_i) b(row) += -2               * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i]  , 2             ));
      if (isFixed_j) b(row) +=  2               * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j]  ,-2             ));
      if (isFixed_p) b(row) += -((4 * igl::PI)/Nd) * p[eid] ; else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],((4 * igl::PI)/Nd)));
      b(row) += -2 * k[eid];
      assert(hard[i] == hard[i]);
      assert(hard[j] == hard[j]);
      assert(p[eid] == p[eid]);
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (j)+1 -th row: t_j [    -2        2          -4pi/N     -2    ]
    if (!isFixed_j)
    {
      row = tag_t[j];
      if (isFixed_i) b(row) += 2               * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i]  , -2             ));
      if (isFixed_j) b(row) += -2              * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j] ,  2              ));
      if (isFixed_p) b(row) += ((4 * igl::PI)/Nd) * p[eid] ; else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],-((4 * igl::PI)/Nd)));
      b(row) += 2 * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }
    // (r*3)+2 -th row: pij [   4pi/N   -4pi/N    2*(2pi/N)^2   4pi/N  ]
    if (!isFixed_p)
    {
      row = tag_p[eid];
      if (isFixed_i) b(row) += -(4 * igl::PI)/Nd              * hard[i]; else T.push_back(Eigen::Triplet<double>(row,tag_t[i] ,   (4 * igl::PI)/Nd             ));
      if (isFixed_j) b(row) +=  (4 * igl::PI)/Nd              * hard[j]; else T.push_back(Eigen::Triplet<double>(row,tag_t[j] ,  -(4 * igl::PI)/Nd             ));
      if (isFixed_p) b(row) += -(2 * pow(((2*igl::PI)/Nd),2)) * p[eid] ;  else T.push_back(Eigen::Triplet<double>(row,tag_p[eid],  (2 * pow(((2*igl::PI)/Nd),2))));
      b(row) += - (4 * igl::PI)/Nd * k[eid];
      assert(k[eid] == k[eid]);
      assert(b(row) == b(row));
    }

  }

  A = SparseMatrix<double>(count_t + count_p, count_t + count_p);
  A.setFromTriplets(T.begin(), T.end());

  // Soft constraints
  bool addSoft = false;

  for(unsigned i=0; i<wSoft.size();++i)
    if (wSoft[i] != 0)
      addSoft = true;

  if (addSoft)
  {
    cerr << " Adding soft here: " << endl;
    cerr << " softAplha: " << softAlpha << endl;
    VectorXd bSoft = VectorXd::Zero(count_t + count_p);

    std::vector<Eigen::Triplet<double> > TSoft;
    TSoft.reserve(2 * count_p);

    for(unsigned i=0; i<F.rows(); ++i)
    {
      int varid = tag_t[i];
      if (varid != -1) // if it is a variable in the system
      {
        TSoft.push_back(Eigen::Triplet<double>(varid,varid,wSoft[i]));
        bSoft[varid] += wSoft[i] * soft[i];
      }
    }
    SparseMatrix<double> ASoft(count_t + count_p, count_t + count_p);
    ASoft.setFromTriplets(TSoft.begin(), TSoft.end());

//    ofstream s("/Users/daniele/As.txt");
//    for(unsigned i=0; i<TSoft.size(); ++i)
//      s << TSoft[i].row() << " " << TSoft[i].col() << " " << TSoft[i].value() << endl;
//    s.close();

//    ofstream s2("/Users/daniele/bs.txt");
//    for(unsigned i=0; i<bSoft.rows(); ++i)
//      s2 << bSoft(i) << endl;
//    s2.close();

    // Stupid Eigen bug
    SparseMatrix<double> Atmp (count_t + count_p, count_t + count_p);
    SparseMatrix<double> Atmp2(count_t + count_p, count_t + count_p);
    SparseMatrix<double> Atmp3(count_t + count_p, count_t + count_p);

    // Merge the two part of the energy
    Atmp = (1.0 - softAlpha)*A;
    Atmp2 = softAlpha * ASoft;
    Atmp3 = Atmp+Atmp2;

    A = Atmp3;
    b = b*(1.0 - softAlpha) + bSoft * softAlpha;
  }

//  ofstream s("/Users/daniele/A.txt");
//  for (int k=0; k<A.outerSize(); ++k)
//    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
//    {
//      s << it.row() << " " << it.col() << " " << it.value() << endl;
//    }
//  s.close();
//
//  ofstream s2("/Users/daniele/b.txt");
//  for(unsigned i=0; i<b.rows(); ++i)
//    s2 << b(i) << endl;
//  s2.close();
}

void igl::copyleft::comiso::NRosyField::solveNoRoundings()
{
  using namespace std;
  using namespace Eigen;

  // Solve the linear system
  SimplicialLDLT<SparseMatrix<double> > solver;
  solver.compute(A);
  VectorXd x = solver.solve(b);

  // Copy the result back
  for(unsigned i=0; i<F.rows(); ++i)
    if (tag_t[i] != -1)
      angles[i] = x(tag_t[i]);
    else
      angles[i] = hard[i];

  for(unsigned i=0; i<EF.rows(); ++i)
    if(tag_p[i]  != -1)
      p[i] = roundl(x[tag_p[i]]);
}

void igl::copyleft::comiso::NRosyField::solveRoundings()
{
  using namespace std;
  using namespace Eigen;

  unsigned n = A.rows();

  gmm::col_matrix< gmm::wsvector< double > > gmm_A;
  std::vector<double> gmm_b;
  std::vector<int> ids_to_round;
  std::vector<double> x;

  gmm_A.resize(n,n);
  gmm_b.resize(n);
  x.resize(n);

  // Copy A
  for (int k=0; k<A.outerSize(); ++k)
    for (SparseMatrix<double>::InnerIterator it(A,k); it; ++it)
    {
      gmm_A(it.row(),it.col()) += it.value();
    }

  // Copy b
  for(unsigned i=0; i<n;++i)
    gmm_b[i] = b[i];

  // Set variables to round
  ids_to_round.clear();
  for(unsigned i=0; i<tag_p.size();++i)
    if(tag_p[i] != -1)
      ids_to_round.push_back(tag_p[i]);

  // Empty constraints
  gmm::row_matrix< gmm::wsvector< double > > gmm_C(0, n);

  COMISO::ConstrainedSolver cs;
  //print_miso_settings(cs.misolver());
  cs.solve(gmm_C, gmm_A, x, gmm_b, ids_to_round, 0.0, false, true);

  // Copy the result back
  for(unsigned i=0; i<F.rows(); ++i)
    if (tag_t[i] != -1)
      angles[i] = x[tag_t[i]];
    else
      angles[i] = hard[i];

  for(unsigned i=0; i<EF.rows(); ++i)
    if(tag_p[i]  != -1)
      p[i] = roundl(x[tag_p[i]]);

}

void igl::copyleft::comiso::NRosyField::roundAndFix()
{
  for(unsigned i=0; i<p.rows(); ++i)
    pFixed[i] = true;
}

void igl::copyleft::comiso::NRosyField::roundAndFixToZero()
{
  for(unsigned i=0; i<p.rows(); ++i)
  {
    pFixed[i] = true;
    p[i] = 0;
  }
}

void igl::copyleft::comiso::NRosyField::solve(const int N)
{
  // Reduce the search space by fixing matchings
  reduceSpace();

  // Build the system
  prepareSystemMatrix(N);

  // Solve with integer roundings
  solveRoundings();

  // This is a very greedy solving strategy
  // // Solve with no roundings
  // solveNoRoundings();
  //
  // // Round all p and fix them
  // roundAndFix();
  //
  // // Build the system
  // prepareSystemMatrix(N);
  //
  // // Solve with no roundings (they are all fixed)
  // solveNoRoundings();

  // Find the cones
  findCones(N);
}

void igl::copyleft::comiso::NRosyField::setConstraintHard(const int fid, const Eigen::Vector3d& v)
{
  isHard[fid] = true;
  hard(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::setConstraintSoft(const int fid, const double w, const Eigen::Vector3d& v)
{
  wSoft(fid) = w;
  soft(fid) = convert3DtoLocal(fid, v);
}

void igl::copyleft::comiso::NRosyField::resetConstraints()
{
  using namespace std;
  using namespace Eigen;

  isHard.resize(F.rows());
  for(unsigned i=0; i<F.rows(); ++i)
    isHard[i] = false;
  hard   = VectorXd::Zero(F.rows());

  wSoft  = VectorXd::Zero(F.rows());
  soft   = VectorXd::Zero(F.rows());
}

Eigen::MatrixXd igl::copyleft::comiso::NRosyField::getFieldPerFace()
{
  using namespace std;
  using namespace Eigen;

  MatrixXd result(F.rows(),3);
  for(unsigned i=0; i<F.rows(); ++i)
    result.row(i) = convertLocalto3D(i, angles(i));
  return result;
}

Eigen::MatrixXd igl::copyleft::comiso::NRosyField::getFFieldPerFace()
{
  using namespace std;
  using namespace Eigen;

  MatrixXd result(F.rows(),6);
  for(unsigned i=0; i<F.rows(); ++i)
  {
      Vector3d v1 = convertLocalto3D(i, angles(i));
      Vector3d n = N.row(i);
      Vector3d v2 = n.cross(v1);
      v1.normalize();
      v2.normalize();

      result.block(i,0,1,3) = v1.transpose();
      result.block(i,3,1,3) = v2.transpose();
  }
  return result;
}

void igl::copyleft::comiso::NRosyField::computek()
{
  using namespace std;
  using namespace Eigen;

  // For every non-border edge
  for (unsigned eid=0; eid<EF.rows(); ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = EF(eid,0);
      int fid1 = EF(eid,1);

      Vector3d N0 = N.row(fid0);
      Vector3d N1 = N.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (EV(eid,0) == F(fid0,i))
          fid0_vc = i;
        if (EV(eid,1) == F(fid1,i))
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Vector3d common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      MatrixXd P(3,3);
      VectorXd o = V.row(F(fid0,fid0_vc));
      VectorXd tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      P.transposeInPlace();

      MatrixXd V0(3,3);
      V0.row(0) = V.row(F(fid0,0)).transpose() -o;
      V0.row(1) = V.row(F(fid0,1)).transpose() -o;
      V0.row(2) = V.row(F(fid0,2)).transpose() -o;

      V0 = (P*V0.transpose()).transpose();

      assert(V0(0,2) < 10e-10);
      assert(V0(1,2) < 10e-10);
      assert(V0(2,2) < 10e-10);

      MatrixXd V1(3,3);
      V1.row(0) = V.row(F(fid1,0)).transpose() -o;
      V1.row(1) = V.row(F(fid1,1)).transpose() -o;
      V1.row(2) = V.row(F(fid1,2)).transpose() -o;
      V1 = (P*V1.transpose()).transpose();

      assert(V1(fid1_vc,2) < 10e-10);
      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      MatrixXd R(3,3);
      R << 1,          0,            0,
           0, cos(alpha), -sin(alpha) ,
           0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      assert(V1(0,2) < 10e-10);
      assert(V1(1,2) < 10e-10);
      assert(V1(2,2) < 10e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      VectorXd ref0 = V0.row(1) - V0.row(0);
      VectorXd ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      MatrixXd R2(2,2);
      R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);

      tmp = R2*ref0.head<2>();

      assert(tmp(0) - ref1(0) < 10^10);
      assert(tmp(1) - ref1(1) < 10^10);

      k[eid] = ktemp;
    }
  }

}

void igl::copyleft::comiso::NRosyField::reduceSpace()
{
  using namespace std;
  using namespace Eigen;

  // All variables are free in the beginning
  for(unsigned i=0; i<EV.rows(); ++i)
    pFixed[i] = false;

  vector<VectorXd> debug;

  // debug
//  MatrixXd B(F.rows(),3);
//  for(unsigned i=0; i<F.rows(); ++i)
//    B.row(i) = 1./3. * (V.row(F(i,0)) + V.row(F(i,1)) + V.row(F(i,2)));

  vector<bool> visited(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    visited[i] = false;

  vector<bool> starting(EV.rows());
  for(unsigned i=0; i<EV.rows(); ++i)
    starting[i] = false;

  queue<int> q;
  for(unsigned i=0; i<F.rows(); ++i)
    if (isHard[i] || wSoft[i] != 0)
    {
      q.push(i);
      starting[i] = true;
    }

  // Reduce the search space (see MI paper)
  while (!q.empty())
  {
    int c = q.front();
    q.pop();

    visited[c] = true;
    for(int i=0; i<3; ++i)
    {
      int eid = FE(c,i);
      int fid = TT(c,i);

      // skip borders
      if (fid != -1)
      {
        assert((EF(eid,0) == c && EF(eid,1) == fid) || (EF(eid,1) == c && EF(eid,0) == fid));
        // for every neighbouring face
        if (!visited[fid] && !starting[fid])
        {
          pFixed[eid] = true;
          p[eid] = 0;
          visited[fid] = true;
          q.push(fid);

        }
      }
      else
      {
        // fix borders
        pFixed[eid] = true;
        p[eid] = 0;
      }
    }

  }

  // Force matchings between fixed faces
  for(unsigned i=0; i<F.rows();++i)
  {
    if (isHard[i])
    {
      for(unsigned int j=0; j<3; ++j)
      {
        int fid = TT(i,j);
        if ((fid!=-1) && (isHard[fid]))
        {
          // i and fid are adjacent and fixed
          int eid = FE(i,j);
          int fid0 = EF(eid,0);
          int fid1 = EF(eid,1);

          pFixed[eid] = true;
          p[eid] = roundl(2.0/igl::PI*(hard(fid1) - hard(fid0) - k(eid)));
        }
      }
    }
  }

//  std::ofstream s("/Users/daniele/debug.txt");
//  for(unsigned i=0; i<debug.size(); i += 2)
//    s << debug[i].transpose() << " " << debug[i+1].transpose() << endl;
//  s.close();

}

double igl::copyleft::comiso::NRosyField::convert3DtoLocal(unsigned fid, const Eigen::Vector3d& v)
{
  using namespace std;
  using namespace Eigen;

  // Project onto the tangent plane
  Vector2d vp = TPs[fid] * v;

  // Convert to angle
  return atan2(vp(1),vp(0));
}

Eigen::Vector3d igl::copyleft::comiso::NRosyField::convertLocalto3D(unsigned fid, double a)
{
  using namespace std;
  using namespace Eigen;

  Vector2d vp(cos(a),sin(a));
  return vp.transpose() * TPs[fid];
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::angleDefect()
{
  Eigen::VectorXd A = Eigen::VectorXd::Constant(V.rows(),-2*igl::PI);

  for (unsigned i=0; i < F.rows(); ++i)
  {
    for (int j = 0; j < 3; ++j)
    {
      Eigen::VectorXd a = V.row(F(i,(j+1)%3)) - V.row(F(i,j));
      Eigen::VectorXd b = V.row(F(i,(j+2)%3)) - V.row(F(i,j));
      double t = a.transpose()*b;
      t /= (a.norm() * b.norm());
      A(F(i,j)) += acos(t);
    }
  }

  return A;
}

void igl::copyleft::comiso::NRosyField::findCones(int N)
{
  // Compute I0, see http://www.graphics.rwth-aachen.de/media/papers/bommes_zimmer_2009_siggraph_011.pdf for details

  Eigen::VectorXd I0 = Eigen::VectorXd::Zero(V.rows());

  // first the k
  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      I0(EV(i,0)) -= k(i);
      I0(EV(i,1)) += k(i);
    }
  }

  // then the A
  Eigen::VectorXd A = angleDefect();

  I0 = I0 + A;

  // normalize
  I0 = I0 / (2*igl::PI);

  // round to integer (remove numerical noise)
  for (unsigned i=0; i < I0.size(); ++i)
    I0(i) = round(I0(i));

  // compute I
  Eigen::VectorXd I = I0;

  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (!isBorderEdge[i])
    {
      I(EV(i,0)) -= double(p(i))/double(N);
      I(EV(i,1)) += double(p(i))/double(N);
    }
  }

  // Clear the vertices on the edges
  for (unsigned i=0; i < EV.rows(); ++i)
  {
    if (isBorderEdge[i])
    {
      I0(EV(i,0)) = 0;
      I0(EV(i,1)) = 0;
      I(EV(i,0)) = 0;
      I(EV(i,1)) = 0;
      A(EV(i,0)) = 0;
      A(EV(i,1)) = 0;
    }
  }

  singularityIndex = I;
}

Eigen::VectorXd igl::copyleft::comiso::NRosyField::getSingularityIndexPerVertex()
{
  return singularityIndex;
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
  const Eigen::MatrixXd& V,
  const Eigen::MatrixXi& F,
  const Eigen::VectorXi& b,
  const Eigen::MatrixXd& bc,
  const Eigen::VectorXi& b_soft,
  const Eigen::VectorXd& w_soft,
  const Eigen::MatrixXd& bc_soft,
  const int N,
  const double soft,
  Eigen::MatrixXd& R,
  Eigen::VectorXd& S
  )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V,F);

  // Add hard constraints
  for (unsigned i=0; i<b.size();++i)
    solver.setConstraintHard(b(i),bc.row(i));

  // Add soft constraints
  for (unsigned i=0; i<b_soft.size();++i)
    solver.setConstraintSoft(b_soft(i),w_soft(i),bc_soft.row(i));

  // Set the soft constraints global weight
  solver.setSoftAlpha(soft);

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}

IGL_INLINE void igl::copyleft::comiso::nrosy(
                           const Eigen::MatrixXd& V,
                           const Eigen::MatrixXi& F,
                           const Eigen::VectorXi& b,
                           const Eigen::MatrixXd& bc,
                           const int N,
                           Eigen::MatrixXd& R,
                           Eigen::VectorXd& S
                           )
{
  // Init solver
  igl::copyleft::comiso::NRosyField solver(V,F);

  // Add hard constraints
  for (unsigned i=0; i<b.size();++i)
    solver.setConstraintHard(b(i),bc.row(i));

  // Interpolate
  solver.solve(N);

  // Copy the result back
  R = solver.getFieldPerFace();

  // Extract singularity indices
  S = solver.getSingularityIndexPerVertex();
}


/**
 * @file
 * @author Denise Ratasich
 * @date 11.10.2013
 *
 * @brief Implementation of the SIR particle filter.
 */

#include <ctime>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
// uniform_real gets depricated with newer versions, change to
// uniform_real_distribution with newer version of boost! (see also
// ParticleFilterSIR::resample)
//#include <boost/random/uniform_real_distribution.hpp>

#include "estimation/ParticleFilterSIR.h"
#include "probability/pdfs.h"
#include "probability/sampling.h"

namespace estimation
{
  ParticleFilterSIR::ParticleFilterSIR (unsigned int N)
    : AbstractParticleFilter(N)
  {
    rng.seed((unsigned int)clock());
  }

  ParticleFilterSIR::~ParticleFilterSIR ()
  {
    // nothing to do
  }

  void ParticleFilterSIR::log(std::ostream& os, Log type, int index) const
  {
    switch (type)
    {
    case PARTICLES:
      for (int i = 0; i < particles.size(); i++)
	os << particles[i][index] << " ";
      os << std::endl;
      break;
    case WEIGHTS:
      for (int i = 0; i < particles.size(); i++)
	os << weights[i] << " ";
      os << std::endl;
      break;
    case NEFF:
      os << Neff << std::endl;
      break;
    }
  }

  // -----------------------------------------
  // getters and setters
  // -----------------------------------------
  void ParticleFilterSIR::setProcessNoiseCovariance (MatrixXd& Q)
  {
    this->Q = Q;
    validated = false;
  }

  void ParticleFilterSIR::setMeasurementNoiseCovariance (MatrixXd& R)
  {
    this->R = R;
    validated = false;
  }

  void ParticleFilterSIR::validate (void)
  {
    try
    {
      // check for minimal initialization -----------------------
      if (!f)			// callback empty
	throw std::runtime_error("State transition model missing.");
      if (!h)			// callback empty
	throw std::runtime_error("Observation model missing.");
      if (Q.rows() == 0)
	throw std::runtime_error("Process noise covariance missing.");
      if (R.rows() == 0)
	throw std::runtime_error("Measurement noise covariance missing.");
      if (particles[0].size() == 0)
	throw std::runtime_error("Particles not initialized.");

      // take sizes from required parameters
      int n = particles[0].size();
      int m = R.rows();

      // create other parameters if missing ---------------------
      // none

      // check appropriate sizes of matrices and vectors --------
      if (Q.rows() != n  ||  Q.cols() != n)
	throw std::runtime_error("Process noise covariance has invalid size.");
      if (R.rows() != R.cols())
	throw std::runtime_error("Measurement noise covariance must be a square matrix.");

      // validation finished successfully -----------------------
      validated = true;

      // further things to initialize ---------------------------
      // create output state
      if (out.size() != n) {	// not initialized till now or invalid size from reinit
	out.clear();
	for (int i = 0; i < n; i++)
	  out.add(OutputValue());
      }
    }
    catch(std::exception& e)
    {
      std::string additionalInfo = "ParticleFilterSIR: Validation failed. ";
      throw estimator_error(additionalInfo + e.what());
    }
  }

  // -----------------------------------------
  // Overrides of ParticleFilterSIR's IEstimator implementation
  // -----------------------------------------

  void ParticleFilterSIR::serialize(std::ostream& os) const
  {
    os << "SIR Particle Filter";
  }

  // -----------------------------------------
  // Particle Filtering
  // -----------------------------------------
  void ParticleFilterSIR::sample (void)
  {
    // estimate next state of particles
    for (int i = 0; i < particles.size(); i++)
    {
      // sample from process noise: zero mean gaussian with covariance Q
      VectorXd w = probability::sampleNormalDistribution(VectorXd::Zero(Q.rows()), Q);

      f(particles[i], u);		// time update
      particles[i] = particles[i] + w;  // add noise
    }
  }

  void ParticleFilterSIR::weight (Input measurements)
  {
    double sumWeights = 0, sumWeightsSquare = 0;

    // measurement vector z
    VectorXd z(R.rows());

    if (measurements.size() != z.size())
      throw std::runtime_error("Number of measurements invalid.");

    // calculate weight
    for (int i = 0; i < particles.size(); i++)
    {
      // weight = likelihood of the measurement
      // estimate expected measurement when in state i
      VectorXd z_expected = VectorXd::Zero(z.size());
      h(z_expected, particles[i]);

      // get probability of the measurement (mean = z_expected,
      // covariance = measurement noise covariance)
      prepareMeasurements(z, measurements, z_expected);	// fills z
      double weight = probability::pdfNormalDistribution(z, z_expected, R);

      weights[i] = weights[i] * weight;	// set weight (consider old weight!)
      sumWeights += weights[i];		// add to sum for normalization
    }

    // normalize weights
    for (int i = 0; i < particles.size(); i++)
    {
      weights[i] /= sumWeights;

      // accumulate for Neff-calculation
      sumWeightsSquare += weights[i] * weights[i];
    }

    // evaluate effective number of particles
    Neff = 1 / sumWeightsSquare;
  }

  void ParticleFilterSIR::resample (void)
  {
    int N = particles.size();

    if (Neff > 0.8*N)
      return;

    // copy current particles to the buffer 'partices_old' where the
    // new ones will be chosen from; the array 'particles' will be
    // filled with these drawn ones
    std::vector<VectorXd> particles_old(particles);

    // 1. generate CDF
    std::vector<double> cdf(N);
    cdf[0] = weights[0];
    for (int i = 1; i < N; i++)
      cdf[i] = cdf[i-1] + weights[i];

    // 2. draw a starting point
    boost::uniform_real<> dist(0, 1.0 / N);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
      random(rng, dist);
    double u0 = random();	// u0 is then betw 0..1/N
    int i = 0;

    for (int j = 0; j < N; j++)
    {
      // 3. move along the CDF
      double uj = u0 + ((double)j)/N;

      // check where the random number in the CDF fits -> this is the
      // sample to choose; a sample with higher weight has a higher
      // span in the CDF, hence will be chosen more often
      while (uj > cdf[i])
      	i++;

      particles[j] = particles_old[i];
      weights[j] = 1.0 / N;
    }
  }
}


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.ekm_and_lisf");

    class empirical_kernel_map_tester : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        empirical_kernel_map_tester (
        ) :
            tester (
                "test_ekm_and_lisf",       // the command line argument name for this test
                "Run tests on the empirical_kernel_map and linearly_independent_subset_finder objects.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
            thetime = time(0);
        }

        time_t thetime;
        dlib::rand rnd;

        template <typename T>
        void validate (
            const T& ekm_small,
            const T& ekm_big
        )
        {
            matrix<double> tmat;
            projection_function<typename T::kernel_type> proj;

            ekm_small.get_transformation_to(ekm_big, tmat, proj);
            DLIB_TEST(tmat.nr() == ekm_big.out_vector_size());
            DLIB_TEST(tmat.nc() == ekm_small.out_vector_size());
            DLIB_TEST((unsigned long)proj.basis_vectors.size() == ekm_big.basis_size() - ekm_small.basis_size());
            for (unsigned long i = 0; i < 6; ++i)
            {
                const typename T::sample_type temp = randm(4,1,rnd);
                DLIB_TEST(length(ekm_big.project(temp) - (tmat*ekm_small.project(temp) + proj(temp))) < 1e-10);
            }
        }

        void test_transformation_stuff()
        {
            typedef matrix<double,0,1> sample_type;
            typedef radial_basis_kernel<sample_type> kernel_type;
            const kernel_type kern(1);

            for (unsigned long n = 1; n < 6; ++n)
            {
                print_spinner();
                for (unsigned long extra = 1; extra < 10; ++extra)
                {
                    std::vector<sample_type> samps_small, samps_big;
                    linearly_independent_subset_finder<kernel_type> lisf_small(kern, 1000);
                    linearly_independent_subset_finder<kernel_type> lisf_big(kern, 1000);
                    for (unsigned long i = 0; i < n; ++i)
                    {
                        samps_small.push_back(randm(4,1,rnd));
                        samps_big.push_back(samps_small.back());
                        lisf_big.add(samps_small.back());
                        lisf_small.add(samps_small.back());
                    }
                    for (unsigned long i = 0; i < extra; ++i)
                    {
                        samps_big.push_back(randm(4,1,rnd));
                        lisf_big.add(samps_big.back());
                    }

                    // test no lisf
                    {
                        empirical_kernel_map<kernel_type> ekm_small, ekm_big;
                        ekm_small.load(kern, samps_small);
                        ekm_big.load(kern, samps_big);

                        validate(ekm_small, ekm_big);
                    }

                    // test with lisf
                    {
                        empirical_kernel_map<kernel_type> ekm_small, ekm_big;
                        ekm_small.load(lisf_small);
                        ekm_big.load(lisf_big);

                        validate(ekm_small, ekm_big);
                    }

                    // test with partly lisf
                    {
                        empirical_kernel_map<kernel_type> ekm_small, ekm_big;
                        ekm_small.load(kern, samps_small);
                        ekm_big.load(lisf_big);

                        validate(ekm_small, ekm_big);
                    }

                    // test with partly lisf
                    {
                        empirical_kernel_map<kernel_type> ekm_small, ekm_big;
                        ekm_small.load(lisf_small);
                        ekm_big.load(kern, samps_big);

                        validate(ekm_small, ekm_big);
                    }

                }
            }

            // test what happens if the bigger ekm only has repeated basis vectors
            {
                empirical_kernel_map<kernel_type> ekm_big, ekm_small;
                std::vector<sample_type> samps_big, samps_small;

                sample_type temp = randm(4,1,rnd);

                samps_small.push_back(temp);
                samps_big.push_back(temp);
                samps_big.push_back(temp);

                ekm_big.load(kern, samps_big);
                ekm_small.load(kern, samps_small);

                validate(ekm_small, ekm_big);

            }
            {
                empirical_kernel_map<kernel_type> ekm_big, ekm_small;
                linearly_independent_subset_finder<kernel_type> lisf_small(kern, 1000);
                std::vector<sample_type> samps_big;

                sample_type temp = randm(4,1,rnd);

                lisf_small.add(temp);
                samps_big.push_back(temp);
                samps_big.push_back(temp);

                ekm_big.load(kern, samps_big);
                ekm_small.load(lisf_small);

                validate(ekm_small, ekm_big);

            }
            {
                empirical_kernel_map<kernel_type> ekm_big, ekm_small;
                std::vector<sample_type> samps_big, samps_small;

                sample_type temp = randm(4,1,rnd);
                sample_type temp2 = randm(4,1,rnd);

                samps_small.push_back(temp);
                samps_small.push_back(temp2);
                samps_big.push_back(temp);
                samps_big.push_back(temp2);
                samps_big.push_back(randm(4,1,rnd));

                ekm_big.load(kern, samps_big);
                ekm_small.load(kern, samps_small);

                validate(ekm_small, ekm_big);

            }
            {
                empirical_kernel_map<kernel_type> ekm_big, ekm_small;
                linearly_independent_subset_finder<kernel_type> lisf_small(kern, 1000);
                std::vector<sample_type> samps_big;

                sample_type temp = randm(4,1,rnd);
                sample_type temp2 = randm(4,1,rnd);

                lisf_small.add(temp);
                lisf_small.add(temp2);
                samps_big.push_back(temp);
                samps_big.push_back(temp2);
                samps_big.push_back(temp);

                ekm_big.load(kern, samps_big);
                ekm_small.load(lisf_small);

                validate(ekm_small, ekm_big);

            }

        }

        void perform_test (
        )
        {
            ++thetime;
            typedef matrix<double,0,1> sample_type;
            //dlog << LINFO << "time seed: " << thetime;
            //rnd.set_seed(cast_to_string(thetime));

            typedef radial_basis_kernel<sample_type> kernel_type;

            for (int n = 1; n < 10; ++n)
            {
                print_spinner();
                dlog << LINFO << "matrix size " << n;

                std::vector<sample_type> samples;
                // make some samples
                for (int i = 0; i < n; ++i)
                {
                    samples.push_back(randm(4,1,rnd));
                    // double up the samples just to mess with the lisf
                    if (n > 5)
                        samples.push_back(samples.back());
                }

                dlog << LINFO << "samples.size(): "<< samples.size();

                const kernel_type kern(1);

                linearly_independent_subset_finder<kernel_type> lisf(kern, 100, 1e-4);
                unsigned long count = 0;
                for (unsigned long i = 0; i < samples.size(); ++i)
                {
                    if (lisf.add(samples[i]))
                    {
                        DLIB_TEST(equal(lisf[lisf.size()-1], samples[i]));
                        ++count;
                    }
                }
                DLIB_TEST(count == lisf.size());

                DLIB_TEST(lisf.size() == (unsigned int)n);

                dlog << LINFO << "lisf.size(): "<< lisf.size();

                // make sure the kernel matrices coming out of the lisf are correct
                DLIB_TEST(dlib::equal(lisf.get_kernel_matrix(), kernel_matrix(kern, lisf), 1e-8));
                DLIB_TEST(dlib::equal(lisf.get_inv_kernel_marix(), inv(kernel_matrix(kern, lisf.get_dictionary())), 1e-8));

                empirical_kernel_map<kernel_type> ekm;
                ekm.load(lisf);
                DLIB_TEST(ekm.basis_size() == lisf.size());

                std::vector<sample_type> proj_samples;
                for (unsigned long i = 0; i < samples.size(); ++i)
                {
                    double err;
                    proj_samples.push_back(ekm.project(samples[i], err));
                    DLIB_TEST(err <= 1e-4);
                    const double error_agreement = std::abs(err - lisf.projection_error(samples[i]));
                    dlog << LTRACE << "err: " << err << "    error_agreement: "<< error_agreement;
                    DLIB_TEST(error_agreement < 1e-11);
                }

                for (int i = 0; i < 5; ++i)
                {
                    sample_type temp = randm(4,1,rnd);
                    double err;
                    ekm.project(temp, err);
                    const double error_agreement = std::abs(err - lisf.projection_error(temp));
                    dlog << LTRACE << "err: " << err << "    error_agreement: "<< error_agreement;
                    DLIB_TEST(error_agreement < 1e-11);
                }

                // make sure the EKM did the projection correctly
                DLIB_TEST(dlib::equal(kernel_matrix(kern, samples), kernel_matrix(linear_kernel<sample_type>(), proj_samples), 1e-5));
            }

            test_transformation_stuff();

        }
    };

    // Create an instance of this object.  Doing this causes this test
    // to be automatically inserted into the testing framework whenever this cpp file
    // is linked into the project.  Note that since we are inside an unnamed-namespace
    // we won't get any linker errors about the symbol a being defined multiple times.
    empirical_kernel_map_tester a;

}


#include "format_helper.h"

#include <iostream>
#include <iomanip>
#include <dirent.h>
#include <sys/stat.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include "math_helper.h"

namespace core {

using namespace std;
using namespace cv;

namespace {
void GetCalibParams(const cv::Mat& P_left, const cv::Mat& P_right, double* calib) {
  //std::cout << P_left << "\n" << P_right;
  calib[0] = P_left.at<double>(0,0);
  calib[1] = P_left.at<double>(1,1);
  calib[2] = P_left.at<double>(0,2);
  calib[3] = P_left.at<double>(1,2);
  //calib[4] = std::abs(P_left.at<double>(0,3) - P_right.at<double>(0,3)) / calib[0];
  double x_diff = P_left.at<double>(0,3) - P_right.at<double>(0,3);
  double y_diff = P_left.at<double>(1,3) - P_right.at<double>(1,3);
  double z_diff = P_left.at<double>(2,3) - P_right.at<double>(2,3);
  calib[4] = std::sqrt(x_diff*x_diff + y_diff*y_diff + z_diff*z_diff) / calib[0];
  //for (int i = 0; i < 5; i++)
  //  std::cout << calib[i] << "\n";
}
}

void FormatHelper::ParseKITTIDatasetConfig(
    const std::string config_file, std::string& cam_params_file, std::string& dataset_name,
    std::string& left_folder, std::string& right_folder, std::string& gt_filepath,
    std::size_t& start_num, std::size_t& end_num, std::size_t& num_width) {
  try {
    po::variables_map vm;
    po::options_description config("Config file options");
    config.add_options()
      ("dataset_name", po::value<std::string>(&dataset_name)->required(), "name")
      ("camera_params,p", po::value<std::string>(&cam_params_file)->required(), "camera params file")
      ("left_folder", po::value<std::string>(&left_folder)->required(), "folder")
      ("right_folder", po::value<std::string>(&right_folder)->required(), "folder")
      ("start_num", po::value<std::size_t>(&start_num)->required(), "start number")
      ("end_num", po::value<std::size_t>(&end_num)->required(), "end number")
      ("num_width", po::value<std::size_t>(&num_width)->required(), "padding")
      ("groundtruth,g", po::value<std::string>(&gt_filepath)->required(), "file with motion GT");

    std::ifstream ifs(config_file);
    if (!ifs) {
      throw "can not open config file: " + config_file + "\n";
    }
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    std::cout << e.what() << "\n";
    throw 1;
  }

  left_folder += "/";
  right_folder += "/";
}

void FormatHelper::ReadCalibKitti(const std::string path, double* grey_cam, double* color_cam)
{
  std::ifstream calib_file(path);
  std::vector<cv::Mat> P;
  P.resize(4);

  std::string word;
  for (int i = 0; i < 4; i++) {
    P[i].create(3, 4, CV_64F);
    calib_file >> word;
    for (int j = 0; j < 12; j++) {
      calib_file >> word;
      int row = j / 4;
      int col = j % 4;
      P[i].at<double>(row,col) = std::stod(word);
    }
    //std::cout << P[i] << "\n";
  }
  //std::cout << "\nCalib mono:\n";
  GetCalibParams(P[0], P[1], grey_cam);
  //std::cout << "\nCalib color:\n";
  GetCalibParams(P[2], P[3], color_cam);
}

// reads next matrix in a row
void FormatHelper::ReadNextRtMatrix(std::ifstream& file, cv::Mat& Rt)
{
   std::string line;
   getline(file, line);
   stringstream stream(line);
   string val;
   //cout << "LINE: " << line << endl;
   Rt = Mat::eye(4, 4, CV_64F);
   for(int i = 0; i < 3; i++) {
      for(int j = 0; j < 4; j++) {
         stream >> val;
         Rt.at<double>(i,j) = std::stod(val);
         //cout << "READ: " << motion_params[i] << endl << endl;
      }
   }
}

void FormatHelper::Read2FrameMotionFromAccCameraMotion(const std::string filename, const int num_of_motions,
                                                       std::vector<cv::Mat>& world_motion,
                                                       std::vector<cv::Mat>& camera_motion) {
  // wrt - world motion
  // crt - camera motion
  std::ifstream file(filename);
  cv::Mat wrt, wrt_prev, wrt_curr;
  // read first (usually identety) matrix
  core::FormatHelper::ReadNextRtMatrix(file, wrt_curr);
  core::MathHelper::invTrans(wrt_curr, wrt_prev);

  for (int i = 0; i < num_of_motions; i++) {
    core::FormatHelper::ReadNextRtMatrix(file, wrt_curr);
    cv::Mat crt = wrt_prev * wrt_curr;
    core::MathHelper::invTrans(wrt_curr, wrt_prev);
    camera_motion.push_back(crt.clone());
    core::MathHelper::invTrans(crt, wrt);
    world_motion.push_back(wrt.clone());
    //std::cout << crt << "\n\n";
    //std::cout << wrt << "\n\n";
  }
}

void FormatHelper::WriteMatRt(const cv::Mat& Rt, std::ofstream& fp) {
   //std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++) {
        double val = Rt.at<double>(i,j);
        fp << std::scientific << val << " ";
      }
   }
   fp << endl;
}

void FormatHelper::WriteMotionToFile(const Eigen::Matrix4d& Rt, std::ofstream& file) {
  //std::setprecision(6);
  for(int i = 0; i < Rt.rows() - 1; i++)
    for(int j = 0; j < Rt.cols(); j++)
      file << std::scientific << Rt(i,j) << " ";
  file << std::endl;
}

void FormatHelper::WriteMatRt(const cv::Mat& Rt, std::ofstream& fp, bool convert_to_cm)
{
   //std::setprecision(6);
   for(int i = 0; i < (Rt.rows-1); i++) {
      for(int j = 0; j < Rt.cols; j++) {
        double val = Rt.at<double>(i,j);
        if(convert_to_cm && j == 3)
          val *= 100.0;
        fp << std::scientific << val << " ";
      }
   }
   fp << endl;
}

// [fx fy cx cy tx=baseline]
void FormatHelper::readCameraParams(const std::string& filepath, double (&cam_params)[5])
{
   ifstream file(filepath);
   std::string line;
   getline(file, line);
   stringstream stream(line);
   string val;
   //cout << "[FormatHelper]: Using camera params: ";
   for(int i = 0; i < 5; i++) {
      stream >> val;
      cam_params[i] = std::stod(val);
      //cout << cam_params[i] << "  ";
   }
   cout << endl;
   file.close();
}

void FormatHelper::readGpsPoints(string filename, vector<Mat> points)
{
   ifstream file(filename);
   string val;
   points.clear();
   Mat pt(2, 1, CV_64F);
   while(!file.eof()) {
      file >> val;
      pt.at<double>(0,0) = std::stod(val);
      file >> val;
      pt.at<double>(1,0) = std::stod(val);
      cout << pt << endl;
      points.push_back(pt.clone());
      for(int i = 0; i < 9; i++) file >> val;
   }
}

void FormatHelper::getCalibParams(std::string& intrinsic_filename, std::string& extrinsic_filename, cv::Mat& P_left,
                    cv::Mat& P_right, cv::Mat& Q, cv::Mat& C_left, cv::Mat& D_left, cv::Mat& C_right, cv::Mat& D_right)
{
   // reading intrinsic parameters
   //FileStorage fs(intrinsic_filename, CV_STORAGE_READ);
   FileStorage fs(intrinsic_filename, FileStorage::READ);
   if(!fs.isOpened())
   {
      cout << "Failed to open file " << intrinsic_filename << endl;
      return;
   }

   //Mat M1, D1, M2, D2;
   fs["M1"] >> C_left;
   fs["D1"] >> D_left;
   fs["M2"] >> C_right;
   fs["D2"] >> D_right;
   Mat R, T, R1, R2;
   fs["R"] >> R;
   fs["T"] >> T;

   // TODO - read this also from file
   //cv::Size imageSize(CALIB_WIDTH, CALIB_HEIGHT);
   //stereoRectify(C_left, D_left, C_right, D_right, imageSize, R, T, R1, R2, P_left, P_right, Q, CALIB_ZERO_DISPARITY,
   //      -1, imageSize);

   return;
}

bool FormatHelper::readStringList(const std::string& filename, std::vector<std::string>& l)
{
   l.resize(0);
   FileStorage fs(filename, FileStorage::READ);
   if( !fs.isOpened() )
      return false;
   FileNode n = fs.getFirstTopLevelNode();
   if( n.type() != FileNode::SEQ )
      return false;
   FileNodeIterator it = n.begin(), it_end = n.end();
   for( ; it != it_end; ++it )
      l.push_back((string)*it);
   return true;
}

/* Returns a list of files in a directory (except the ones that begin with a dot) */
void FormatHelper::GetFilesInFolder(const std::string& dir_path, std::vector<std::string>& files,
                                    bool get_full_path = false) {
  DIR *dir;
  struct dirent *ent;
  struct stat st;
  dir = opendir(dir_path.c_str());
  while ((ent = readdir(dir)) != NULL) {
    const std::string file_name = ent->d_name;
    const std::string full_file_name = dir_path + "/" + file_name;
    if (file_name[0] == '.')
      continue;
    if (stat(full_file_name.c_str(), &st) == -1)
      continue;
    const bool is_directory = (st.st_mode & S_IFDIR) != 0;
    if (is_directory)
      continue;

    if (get_full_path)
      files.push_back(full_file_name);
    else
      files.push_back(file_name);
  }
  closedir(dir);
  std::sort(files.begin(), files.end());
}

}


#include <iostream>
#include <thread>
#include <chrono>
#include <memory>
#include <array>
#include <vector>
#include <random>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>

#include "sfmMyFunctions.h"
#include "sfmExceptionMacro.h"
#include "sfmBasicTypes.h"
#include "sfmPedestrianSpawner.h"

//this always seems to be highlighted but it complies fine
#include "sfmVisualiser.h"

int main(int argc, char** argv)
{
    //variables needed throughout the function
    double world_width_x = 50.0;
    double world_height_y = 10.0;
    int no_pedestrians = 100;
    int choice;
    double dt = 0.1;
    double finish_time_s = 100;
    double v_max = 1.3;
    std::vector<std::shared_ptr<sfm::Forces> >pedestrians;

    // Create viewer and initialise with required number of pedestrians
    //left here as its easier to reposition the viewer to where you would like it before
    //entering the selection, i left a second commented later incase someone didnt like
    //this, it is a little jarring at first.
    sfm::Visualiser viewer(no_pedestrians, world_width_x, world_height_y);

    // neatened up the interface to easily pick from three demos
    std::cout   << "Please Pick Demo of 100 pedestrians over 50 seconds; \n"
                << "1(Directional),2(Targeted) or 3(All random);) " << std::endl;
    std::cin >> choice;

    //this has all the pedestrians moving straight along the x axis to their opposite side
    if(choice == 1){

        //set up variables for directional functions
        sfm::dir2d left_side_x(1,2);
        sfm::dir2d left_side_y(0.1,9.9);
        sfm::dir2d right_side_x(48,49);
        sfm::dir2d right_side_y(0.1,9.9);
        sfm::dir2d direc1(1,0);
        sfm::dir2d direc2(-1,0);

        //creates 2 sets of pedestrians in the same format as in the open_mp file
        //essencially we create 2 sets of pedestrians and append them to the total.
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;
        pedestrians1 = sfm::Factory::Distributed(pedestrians1,"Directional",no_pedestrians/2,direc1,direc1,left_side_x,left_side_y);
        for(int point = 0; point < pedestrians1.size();++point){
            pedestrians.emplace_back(pedestrians1[point]);
        }
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians2;
        pedestrians2 = sfm::Factory::Distributed(pedestrians2,"Directional",no_pedestrians/2,direc2,direc2,right_side_x,right_side_y);
        for(int point = 0; point < pedestrians2.size();++point){
            pedestrians.emplace_back(pedestrians2[point]);
        }
    }

    //this has all the pedestrians moving straight along to a point on the opposite side
    else if(choice == 2){

        //variables for targeted pedestrians
        sfm::dir2d left_side_x(1,2);
        sfm::dir2d left_side_y(0.1,9.9);
        sfm::dir2d dest_left_x(48,49);
        sfm::dir2d dest_left_y(0.1,9.9);
        sfm::dir2d right_side_x(48,49);
        sfm::dir2d right_side_y(0.1,9.9);
        sfm::dir2d dest_right_x(1,2);
        sfm::dir2d dest_right_y(0.1,9.9);

        // this uses the distributed fixed class method
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians1;
        pedestrians1 = sfm::Factory::Distributed(pedestrians1,"Targeted",no_pedestrians/2,dest_left_x,dest_left_y,left_side_x,left_side_y);
        for(int point = 0; point < pedestrians1.size();++point){
            pedestrians.emplace_back(pedestrians1[point]);
        }
        std::vector<std::shared_ptr<sfm::Forces> >pedestrians2;
        pedestrians2 = sfm::Factory::Distributed(pedestrians2,"Targeted",no_pedestrians/2,dest_right_x,dest_right_y,right_side_x,right_side_y);
        for(int point = 0; point < pedestrians2.size();++point){
            pedestrians.emplace_back(pedestrians2[point]);
        }

    }

    // this will randomly spawn 100 pedestrains at 100 randompoints
    else if(choice == 3){

        //very simple to spawn random pedestrians
        pedestrians = sfm::Factory::Spawner(pedestrians,no_pedestrians);
    }
    else{
        std::cout << "Pick one of the three demos" <<std::endl;
    }

    // Create viewer and initialise with required number of pedestrians
    //sfm::Visualiser viewer(no_pedestrians, world_width_x, world_height_y);

    //for loop over time
    for(int t=0; t<(finish_time_s/dt);++t){

        //next a for loop that goes through each pedestrian and calculates its resultant force and then updates the new qualities
        //detailed explanation in open mp
        for(int j=0; j<pedestrians.size();++j){
            viewer.SetPedestrian(j, pedestrians[j]->Return_Current_Position()[1],
                                    pedestrians[j]->Return_Current_Position()[0],
                                    pedestrians[j]->Return_Velocity()[1],
                                    pedestrians[j]->Return_Velocity()[0]);
            sfm::dir2d temp_force = pedestrians[j]->Resultant_force(pedestrians,temp_force, dt);
            sfm::dir2d  new_velocity = (temp_force*dt) + pedestrians[j]->Return_Velocity();
            if(new_velocity.length() > v_max*pedestrians[j]->Return_Speed()){
                new_velocity = new_velocity*(v_max*pedestrians[j]->Return_Speed()/new_velocity.length());
                }
            sfm::dir2d position(pedestrians[j]->Return_Current_Position()[1],pedestrians[j]->Return_Current_Position()[0]);
            sfm::pos2d new_position = {position[1]+(new_velocity[1]*dt),(position[0]+new_velocity[0]*dt)};
            pedestrians[j]->Update_Velocity(new_velocity);
            pedestrians[j]->Update_Current_Position(new_position);
        }

        // Tell viewer to redraw scene
        viewer.UpdateScene();

        // Sleep for a bit so can see visualiser updating, i left it as 10 miliseconds at the
        // actual sleep time of 100 ms seemed a bit too slow (0.1s dt)
        std::this_thread::sleep_for (std::chrono::milliseconds(100));

    }

    return 0;
}


//! \file examples/Arrangement_on_surface_2/overlay_unbounded.cpp
// A face overlay of two arrangements with unbounded faces.

#include <string>
#include <boost/lexical_cast.hpp>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Arr_linear_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_extended_dcel.h>
#include <CGAL/Arr_overlay_2.h>
#include <CGAL/Arr_default_overlay_traits.h>

// Define a functor for creating a label from a characer and an integer.
struct Overlay_label
{
  std::string operator() (char c, int i) const
  {
    return boost::lexical_cast<std::string>(c) +
      boost::lexical_cast<std::string>(i);
  }
};

typedef CGAL::Exact_predicates_exact_constructions_kernel   Kernel;
typedef CGAL::Arr_linear_traits_2<Kernel>                   Traits_2;
typedef Traits_2::Point_2                                   Point_2;
typedef Traits_2::Segment_2                                 Segment_2;
typedef Traits_2::Ray_2                                     Ray_2;
typedef Traits_2::Line_2                                    Line_2;
typedef Traits_2::X_monotone_curve_2                        X_monotone_curves_2;
typedef CGAL::Arr_face_extended_dcel<Traits_2, char>        DcelA;
typedef CGAL::Arrangement_2<Traits_2, DcelA>                ArrangementA_2;
typedef CGAL::Arr_face_extended_dcel<Traits_2, int>         DcelB;
typedef CGAL::Arrangement_2<Traits_2, DcelB>                ArrangementB_2;
typedef CGAL::Arr_face_extended_dcel<Traits_2, std::string> DcelRes;
typedef CGAL::Arrangement_2<Traits_2, DcelRes>              ArrangementRes_2;
typedef CGAL::Arr_face_overlay_traits<ArrangementA_2,
                                      ArrangementB_2,
                                      ArrangementRes_2,
                                      Overlay_label>        Overlay_traits;

int main ()
{
  // Construct the first arrangement, induced by two line y = x and y = -x.
  ArrangementA_2          arr1;

  insert (arr1, Line_2 (Point_2(0, 0), Point_2(1, 1)));
  insert (arr1, Line_2 (Point_2(0, 0), Point_2(1, -1)));

  // Label the four (unbounded) face of the arrangement as 'A' to 'D'.
  // We do so by traversing the incident faces to the halfedges aroung the
  // single arrangement vertex (0, 0).
  CGAL_assertion (arr1.number_of_vertices() == 1);

  ArrangementA_2::Halfedge_around_vertex_circulator  first, curr;
  char                                               clabel = 'A';

  curr = first = arr1.vertices_begin()->incident_halfedges();
  do {
    curr->face()->set_data (clabel);
    ++clabel;
    ++curr;
  } while (curr != first);
  std::cout << "Done with arr1." << std::endl;

  // Construct the second arrangement, containing a single square-shaped face.
  ArrangementB_2          arr2;

  insert (arr2, Segment_2 (Point_2(-4, -4), Point_2(4, -4)));
  insert (arr2, Segment_2 (Point_2(4, -4), Point_2(4, 4)));
  insert (arr2, Segment_2 (Point_2(4, 4), Point_2(-4, 4)));
  insert (arr2, Segment_2 (Point_2(-4, 4), Point_2(-4, -4)));

  // Give the unbounded face the index 1, and the bounded face the index 2.
  CGAL_assertion (arr2.number_of_faces() == 2);

  ArrangementB_2::Face_iterator    fit;

  for (fit = arr2.faces_begin(); fit != arr2.faces_end(); ++fit)
    fit->set_data ((fit == arr2.unbounded_face()) ? 1 : 2);
  std::cout << "Done with arr2." << std::endl;

  // Compute the overlay of the two arrangements.
  ArrangementRes_2       overlay_arr;
  Overlay_traits         overlay_traits;

  overlay (arr1, arr2, overlay_arr, overlay_traits);

  // Go over the faces of the overlaid arrangement and their labels.
  ArrangementRes_2::Face_iterator  res_fit;

  std::cout << "The overlay faces are: ";
  for (res_fit = overlay_arr.faces_begin();
       res_fit != overlay_arr.faces_end(); ++res_fit)
  {
    std::cout << res_fit->data() << " ("
              << (res_fit->is_unbounded() ? "unbounded" : "bounded")
              << ")." << std::endl;
  }

  return 0;
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// 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)
//
// Custom pointer-to-point example

#include <iostream>

#include <boost/foreach.hpp>

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>
#include <boost/geometry/strategies/strategies.hpp>

BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::vector)
BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::deque)

// Sample point, having x/y
struct my_point
{
    double x,y;
};

namespace boost { namespace geometry { namespace traits {

template<> struct tag<my_point>
{ typedef point_tag type; };

template<> struct coordinate_type<my_point>
{ typedef double type; };

template<> struct coordinate_system<my_point>
{ typedef cs::cartesian type; };

template<> struct dimension<my_point> : boost::mpl::int_<2> {};

template<>
struct access<my_point, 0>
{
    static double get(my_point const& p)
    {
        return p.x;
    }

    static void set(my_point& p, double const& value)
    {
        p.x = value;
    }
};

template<>
struct access<my_point, 1>
{
    static double get(my_point const& p)
    {
        return p.y;
    }

    static void set(my_point& p, double const& value)
    {
        p.y = value;
    }
};

}}} // namespace boost::geometry::traits

int main()
{
    typedef std::vector<my_point*> ln;
    ln myline;
    for (float i = 0.0; i < 10.0; i++)
    {
        my_point* p = new my_point;
        p->x = i;
        p->y = i + 1;
        myline.push_back(p);
    }

    std::cout << boost::geometry::length(myline) << std::endl;

    typedef boost::geometry::model::d2::point_xy<double> point_2d;
    typedef boost::geometry::model::box<point_2d> box_2d;
    box_2d cb(point_2d(1.5, 1.5), point_2d(4.5, 4.5));

    // This will NOT work because would need dynamicly allocating memory for point* in algorithms:
    // std::vector<ln> clipped;
    //boost::geometry::intersection(cb, myline, clipped);

    // This works because outputs to a normal struct point, no point*
    typedef boost::geometry::model::linestring<point_2d> linestring_2d;
    boost::geometry::detail::no_rescale_policy rescale_policy;
    std::vector<linestring_2d> clipped;
    boost::geometry::strategy::intersection::liang_barsky<box_2d, point_2d> strategy;
    boost::geometry::detail::intersection::clip_range_with_box<linestring_2d>(cb,
                    myline, rescale_policy, std::back_inserter(clipped), strategy);

    std::cout << boost::geometry::length(clipped.front()) << std::endl;

    // free
    BOOST_FOREACH(my_point* p, myline)
    {
        delete p;
    }

    return 0;
}


/*

Copyright (c) 2005-2015, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef DIFFUSIONFORCE_HPP_
#define DIFFUSIONFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractForce.hpp"
#include "AbstractOffLatticeCellPopulation.hpp"
#include "RandomNumberGenerator.hpp"

/**
 * A 'diffusion force' class to model the random movement of nodes.
 *
 * This class works with all off-lattice cell populations.
 */
template<unsigned DIM>
class DiffusionForce : public AbstractForce<DIM>
{
private :

    /**
     * Absolute temperature (in Kelvin).
     */
    double mAbsoluteTemperature;

    /**
     * Viscosity of media. We assume that this is measured in units of
     * kg (10 microns)^(-1) h^(-1), and that cell diameters are scaled with
     * a characteristic length of 10 microns.
     */
    double mViscosity;

    /**
     * The Boltzmann constant, which takes the value 4.97033568e-7
     * (10 microns)^2 kg h^(-2) K^(-1).
     */
    static const double msBoltzmannConstant;

    /**
     * Archiving.
     */
    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mAbsoluteTemperature;
        archive & mViscosity;
    }

public :

    /**
     * Constructor.
     */
    DiffusionForce();

    /**
     * Destructor.
     */
    ~DiffusionForce();

    /**
     * Set the absolute temperature, which affects the
     * diffusion constant.
     *
     * @param absoluteTemperature the temperature in Kelvin
     */
    void SetAbsoluteTemperature(double absoluteTemperature);

    /**
      * Set the media viscosity (dynamic), which affects the
      * diffusion constant.
      *
      * @param viscosity the viscosity
      */
    void SetViscosity(double viscosity);

    /**
     * Get the absolute temperature.
     *
     * @return mAbsoluteTemperature
     */
    double GetAbsoluteTemperature();

    /**
      * Get the viscosity.
      *
      * @return mViscosity
      */
    double GetViscosity();

    /**
     * Calculate the scaling of the diffusion constant
     *
     * @return the scaled diffusion constant.
     */
    double GetDiffusionScalingConstant();

    /**
     * Overridden AddForceContribution() method.
     * Note that this method requires cell/node radii to be set.
     *
     * @param rCellPopulation reference to the tissue
     */
    void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(DiffusionForce)

#endif /*DIFFUSIONFORCE_HPP_*/