/*
This code is based on the original code by Quentin "Leph" Rouxel and Team Rhoban.
The original files can be found at:
https://github.com/Rhoban/model/
*/
#ifndef QUINTICWALKNODE_HPP
#define QUINTICWALKNODE_HPP

#include <iostream>
#include <string>
#include <Eigen/Dense>
#include <chrono>

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Char.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>
#include <moveit_msgs/RobotState.h>
#include <humanoid_league_msgs/RobotControlState.h>
#include <bitbots_quintic_walk/WalkingDebug.h>
#include <bitbots_msgs/JointCommand.h>
#include <bitbots_msgs/FootPressure.h>

#include <dynamic_reconfigure/server.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/move_group_interface/move_group_interface.h>

#include <bitbots_quintic_walk/bitbots_quintic_walk_paramsConfig.h>
#include "bitbots_ik/AnalyticIKSolver.hpp"
#include "bitbots_ik/BioIKSolver.hpp"
#include "bitbots_quintic_walk/WalkEngine.hpp"
#include "DspSDK/DspHandler.h"
#include <std_msgs/Bool.h>
#include <unistd.h>

class QuinticWalkingNode {
public:
    QuinticWalkingNode();

    /**
     * This is the main loop which takes care of stopping and starting of the walking.
     * A small state machine is tracking in which state the walking is and builds the trajectories accordingly.
     */
    void run();

    /**
     * Dynamic reconfigure callback. Takes in new parameters and applies them to the needed software parts
     */
    void reconf_callback(bitbots_quintic_walk::bitbots_quintic_walk_paramsConfig &config, uint32_t level);

    /**
     * Initialize internal WalkEngine to correctly zeroed, usable state
     */
    void initializeEngine();

    bool SetHeadMoveValid(std_srvs::SetBool::Request& req,
                          std_srvs::SetBool::Response& res);

    bool SetSensorEnableValid(std_srvs::SetBool::Request& req,
                              std_srvs::SetBool::Response& res);

    bool ResetOdometry(std_srvs::SetBool::Request& req,
                       std_srvs::SetBool::Response& res);

    bool SetSpecialGaitValid(std_srvs::SetBool::Request& req,
                             std_srvs::SetBool::Response& res);

    bool DoLeftKick(std_srvs::Trigger::Request& req,
                    std_srvs::Trigger::Response& res);

    bool DoRightKick(std_srvs::Trigger::Request& req,
                     std_srvs::Trigger::Response& res);

    bool DoStandFront(std_srvs::Trigger::Request& req,
                      std_srvs::Trigger::Response& res);

    bool DoStandBack(std_srvs::Trigger::Request& req,
                     std_srvs::Trigger::Response& res);

    bool DoWalkKickLeft(std_srvs::Trigger::Request& req,
                        std_srvs::Trigger::Response& res);

    bool DoWalkKickRight(std_srvs::Trigger::Request& req,
                         std_srvs::Trigger::Response& res);

    bool SetTorqueEnable(std_srvs::SetBool::Request& req,
                         std_srvs::SetBool::Response& res);

    bool SetGaitValid(std_srvs::SetBool::Request& req,
                      std_srvs::SetBool::Response& res);

    bool ResetGait(std_srvs::SetBool::Request& req,
                   std_srvs::SetBool::Response& res);

    bool GetSpecialGaitPending(std_srvs::Trigger::Request& req,
                               std_srvs::Trigger::Response& res);

    bool GetOdometryResetPending(std_srvs::Trigger::Request& req,
                                 std_srvs::Trigger::Response& res);

    bool GaitResetPending(std_srvs::Trigger::Request& req,
                          std_srvs::Trigger::Response& res);

    bool GetWalkKickPending(std_srvs::Trigger::Request& req,
                            std_srvs::Trigger::Response& res);

   private:
    void publishControllerCommands(std::vector<std::string> joint_names, std::vector<double> positions);

    void publishDebug(tf::Transform &trunk_to_support_foot, tf::Transform &trunk_to_flying_foot);

    void publishMarker(std::string name_space, std::string frame, geometry_msgs::Pose pose, float r, float g, float b,
                       float a);

    void publishMarkers();

    void publishOdometry();

    void cmdVelCb(geometry_msgs::Twist msg);

    void headPosCb(sensor_msgs::JointState msg);

    void imuCb(sensor_msgs::Imu msg);

    void pressureCb(bitbots_msgs::FootPressure msg);

    void robStateCb(humanoid_league_msgs::RobotControlState msg);

    void jointStateCb(sensor_msgs::JointState msg);

    void kickCb(std_msgs::BoolConstPtr msg);

    void cop_l_cb(const geometry_msgs::PointStamped msg);

    void cop_r_cb(const geometry_msgs::PointStamped msg);

    void calculateJointGoals();

    double getTimeDelta();

    void SetDspCommand();
    void ClearDspValidState();
    void GetDataFromDsp();

    bool _fake_mode;
    bool _debugActive;
    bool _simulation_active;

    bool _first_run;

    double _engineFrequency;

    bool _phaseResetActive;
    double _phaseResetPhase;
    double _groundMinPressure;
    bool _copStopActive;
    double _copXThreshold;
    double _copYThreshold;
    bool _pressureStopActive;
    double _ioPressureThreshold;
    double _fbPressureThreshold;

    bool _imuActive;
    double _imu_pitch_threshold;
    double _imu_roll_threshold;
    double _imu_pitch_vel_threshold;
    double _imu_roll_vel_threshold;

    bool _publishOdomTF;
    int _odomPubFactor;
    std::chrono::time_point<std::chrono::steady_clock> _last_update_time;
    double _last_ros_update_time;

    int _robotState;
    int _marker_id;

    bitbots_quintic_walk::WalkingParameter _params;

    Eigen::Vector3d _trunkPos;
    Eigen::Vector3d _trunkAxis;
    Eigen::Vector3d _footPos;
    Eigen::Vector3d _footAxis;
    bool _isLeftSupport;

    bool _special_gait_pending;
    bool _odometry_reset_pending;
    bool _gait_reset_pending;
    bool _walk_kick_pending;

    Eigen::Vector3d _real_velocities;
    Eigen::Vector3d _real_odometry;
    Eigen::Vector3d _imu_angle;
    Eigen::Vector2d _real_head_pos;
    Eigen::Matrix<double, 7, 1> _real_body_pos;

    /**
     * Saves current orders as [x-direction, y-direction, z-rotation]
     */
    Eigen::Vector3d _currentOrders;
    Eigen::Vector2d _headPos;
    Eigen::Vector2i _gaitId;

    /**
     * Saves max values we can move in a single step as [x-direction, y-direction, z-rotation].
     * Is used to limit _currentOrders to sane values
     */
    Eigen::Vector3d _max_step;

    /**
     * Measures how much distance we can traverse in X and Y direction combined
     */
    double _max_step_xy;
    bitbots_quintic_walk::QuinticWalk _walkEngine;

    bitbots_msgs::JointCommand _command_msg;
    nav_msgs::Odometry _odom_msg;
    geometry_msgs::TransformStamped _odom_trans;

    ros::NodeHandle _nh;

    ros::Publisher _pubControllerCommand;
    ros::Publisher _pubOdometry;
    ros::Publisher _pubSupport;
    tf::TransformBroadcaster _odom_broadcaster;
    ros::Publisher _pubDebug;
    ros::Publisher _pubDebugMarker;

    ros::Subscriber _subCmdVel;
    ros::Subscriber _subHeadPos;
    ros::Subscriber _subRobState;
    ros::Subscriber _subJointStates;
    ros::Subscriber _subKick;
    ros::Subscriber _subImu;
    ros::Subscriber _subPressure;
    ros::Subscriber _subCopL;
    ros::Subscriber _subCopR;

    // dsp command service
    ros::ServiceServer _set_head_move_valid_service;
    ros::ServiceServer _set_sensor_enable_valid_service;
    ros::ServiceServer _set_special_gait_valid_service;
    ros::ServiceServer _do_left_kick_service;
    ros::ServiceServer _do_right_kick_service;
    ros::ServiceServer _do_stand_front_service;
    ros::ServiceServer _do_stand_back_service;
    ros::ServiceServer _do_walk_kick_left_service;
    ros::ServiceServer _do_walk_kick_right_service;
    ros::ServiceServer _set_torque_enable_service;
    ros::ServiceServer _set_gait_valid_service;
    ros::ServiceServer _reset_odometry_service;
    ros::ServiceServer _reset_gait_service;

    ros::ServiceServer _get_special_gait_pending_service;
    ros::ServiceServer _get_odometry_reset_pending_service;
    ros::ServiceServer _gait_reset_pending_service;
    ros::ServiceServer _get_walk_kick_pending_service;

    geometry_msgs::PointStamped _cop_l;
    geometry_msgs::PointStamped _cop_r;

    // MoveIt!
    robot_model_loader::RobotModelLoader _robot_model_loader;
    robot_model::RobotModelPtr _kinematic_model;
    robot_state::RobotStatePtr _goal_state;
    robot_state::RobotStatePtr _current_state;
    const robot_state::JointModelGroup *_all_joints_group;
    const robot_state::JointModelGroup *_legs_joints_group;
    const robot_state::JointModelGroup *_lleg_joints_group;
    const robot_state::JointModelGroup *_rleg_joints_group;

    // IK solver
    bitbots_ik::BioIKSolver _bioIK_solver;
    std::shared_ptr<DspSDK::DspHandler> _dsp_handler;
};

#endif


// Copyright (c) 2015 The Dogecoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "arith_uint256.h"
#include "chainparams.h"
#include "lemoncoin.h"
#include "main.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(lemoncoin_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(get_next_work_difficulty_limit)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus(0);

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1455393600; // Block # 1

    /*
    pindexLast.nHeight = 239;
    pindexLast.nTime = 1386475638; // Block #239
    pindexLast.nBits = 0x1e0ffff0;
    BOOST_CHECK_EQUAL(CalculateLemonCoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1e00ffff);*/
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield)
{
    /*SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus(145000);

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395094427;

    // First hard-fork at 145,000, which applies to block 145,001 onwards
    pindexLast.nHeight = 145000;
    pindexLast.nTime = 1395094679;
    pindexLast.nBits = 0x1b499dfd;
    BOOST_CHECK_EQUAL(CalculateLemonCoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b671062);*/
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_modulated_upper)
{
    /*SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus(145000);

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395100835;

    // Test the upper bound on modulated time using mainnet block #145,107
    pindexLast.nHeight = 145107;
    pindexLast.nTime = 1395101360;
    pindexLast.nBits = 0x1b3439cd;
    BOOST_CHECK_EQUAL(CalculateLemonCoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b4e56b3);*/
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_modulated_lower)
{
    /*SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus(145000);

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395380517;

    // Test the lower bound on modulated time using mainnet block #149,423
    pindexLast.nHeight = 149423;
    pindexLast.nTime = 1395380447;
    pindexLast.nBits = 0x1b446f21;
    BOOST_CHECK_EQUAL(CalculateLemonCoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b335358);*/
}

BOOST_AUTO_TEST_CASE(get_next_work_digishield_rounding)
{
    /*SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& params = Params().GetConsensus(145000);

    CBlockIndex pindexLast;
    int64_t nLastRetargetTime = 1395094679;

    // Test case for correct rounding of modulated time - this depends on
    // handling of integer division, and is not obvious from the code
    pindexLast.nHeight = 145001;
    pindexLast.nTime = 1395094727;
    pindexLast.nBits = 0x1b671062;
    BOOST_CHECK_EQUAL(CalculateLemonCoinNextWorkRequired(&pindexLast, nLastRetargetTime, params), 0x1b6558a4);*/
}

BOOST_AUTO_TEST_CASE(hardfork_parameters)
{
    SelectParams(CBaseChainParams::MAIN);
    const Consensus::Params& initialParams = Params().GetConsensus(0);

    BOOST_CHECK_EQUAL(initialParams.nPowTargetTimespan, 60);
    BOOST_CHECK_EQUAL(initialParams.fAllowLegacyBlocks, true);
    BOOST_CHECK_EQUAL(initialParams.fDigishieldDifficultyCalculation, true);

    /*
    const Consensus::Params& initialParamsEnd = Params().GetConsensus(144999);
    BOOST_CHECK_EQUAL(initialParamsEnd.nPowTargetTimespan, 14400);
    BOOST_CHECK_EQUAL(initialParamsEnd.fAllowLegacyBlocks, true);
    BOOST_CHECK_EQUAL(initialParamsEnd.fDigishieldDifficultyCalculation, false);

    const Consensus::Params& digishieldParams = Params().GetConsensus(145000);
    BOOST_CHECK_EQUAL(digishieldParams.nPowTargetTimespan, 60);
    BOOST_CHECK_EQUAL(digishieldParams.fAllowLegacyBlocks, true);
    BOOST_CHECK_EQUAL(digishieldParams.fDigishieldDifficultyCalculation, true);

    const Consensus::Params& digishieldParamsEnd = Params().GetConsensus(371336);
    BOOST_CHECK_EQUAL(digishieldParamsEnd.nPowTargetTimespan, 60);
    BOOST_CHECK_EQUAL(digishieldParamsEnd.fAllowLegacyBlocks, true);
    BOOST_CHECK_EQUAL(digishieldParamsEnd.fDigishieldDifficultyCalculation, true);

    const Consensus::Params& auxpowParams = Params().GetConsensus(371337);
    BOOST_CHECK_EQUAL(auxpowParams.nHeightEffective, 371337);
    BOOST_CHECK_EQUAL(auxpowParams.nPowTargetTimespan, 60);
    BOOST_CHECK_EQUAL(auxpowParams.fAllowLegacyBlocks, false);
    BOOST_CHECK_EQUAL(auxpowParams.fDigishieldDifficultyCalculation, true);

    const Consensus::Params& auxpowHighParams = Params().GetConsensus(700000); // Arbitrary point after last hard-fork
    BOOST_CHECK_EQUAL(auxpowHighParams.nPowTargetTimespan, 60);
    BOOST_CHECK_EQUAL(auxpowHighParams.fAllowLegacyBlocks, false);
    BOOST_CHECK_EQUAL(auxpowHighParams.fDigishieldDifficultyCalculation, true);
    */
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <nt2/sdk/timing/now.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <sstream>
#include <string>

namespace nt2 { namespace details
{
  // Type of accumulator used by all benchmarks
  typedef boost::accumulators::stats< boost::accumulators::tag::mean
                                      , boost::accumulators::tag::median
                                      , boost::accumulators::tag::min
                                      , boost::accumulators::tag::max
                                      , boost::accumulators::tag::count
                                      > stats_t;

  // accumulator for microseconds
  typedef boost::accumulators::accumulator_set<microseconds_t,stats_t>  times_set;

  // accumulator for CPU cycles
  typedef boost::accumulators::accumulator_set<cycles_t,stats_t>        cycles_set;

  // Compute an unique string referring to a proper Experiment result
  template<typename Experiment, typename Stat>
  inline std::string identify_result( std::string const& name
                                    , Experiment const& e
                                    , Stat const& s
                                    )
  {
    std::ostringstream str;
    str << name << e.size() << s.unit();
    return str.str();
  }
} }

#endif


//============================================================================
// Name        : AltitudeMatrix.cpp
// Author      : mm
// Copyright   : This tool and Javascript code are licensed under the MIT license.
// Description :
//============================================================================
#include "AltitudeMatrix.h"
#include "GeoTiffReader.h"
#include "Dct.h"
#include <boost/format.hpp>
#include <cctype>
#include <fstream>

AltitudeMatrix::AltitudeMatrix() {
	masterMatrix.init(MATRIX_SIZE+4, MATRIX_SIZE+4);
	memset(matrix, 0, sizeof(matrix));
	smoothingCount = 0;
    dctRadius = -1;
	baseFileName = "";
}

AltitudeMatrix::~AltitudeMatrix() {
}

void AltitudeMatrix::clear()
{
	masterMatrix.init(MATRIX_SIZE+4, MATRIX_SIZE+4);
	memset(matrix, 0, sizeof(matrix));
	smoothingCount = 0;
}

bool AltitudeMatrix::setDEMMatrix(
	Matrix<short>* pMatrix11, Matrix<short>* pMatrix12, Matrix<short>* pMatrix13,
	Matrix<short>* pMatrix21, Matrix<short>* pMatrix22, Matrix<short>* pMatrix23,
	Matrix<short>* pMatrix31, Matrix<short>* pMatrix32, Matrix<short>* pMatrix33)
{
	if(!pMatrix22){
		return false;
	}

	masterMatrix.copy(pMatrix31, MATRIX_SIZE-2, MATRIX_SIZE-2, 2,           2,           0,             0);
	masterMatrix.copy(pMatrix32, 0,             MATRIX_SIZE-2, MATRIX_SIZE, 2,           2,             0);
	masterMatrix.copy(pMatrix33, 0,             MATRIX_SIZE-2, 2,           2,           MATRIX_SIZE+2, 0);
	masterMatrix.copy(pMatrix21, MATRIX_SIZE-2, 0,             2,           MATRIX_SIZE, 0,             2);
	masterMatrix.copy(pMatrix22, 0,             0,             MATRIX_SIZE, MATRIX_SIZE, 2,             2);
	masterMatrix.copy(pMatrix23, MATRIX_SIZE-2, 0,             2,           MATRIX_SIZE, MATRIX_SIZE+2, 2);
	masterMatrix.copy(pMatrix11, MATRIX_SIZE-2, 0,             2,           2,           0,             MATRIX_SIZE+2);
	masterMatrix.copy(pMatrix12, 0,             0,             MATRIX_SIZE, 2,           2,             MATRIX_SIZE+2);
	masterMatrix.copy(pMatrix13, 0,             0,             2,           2,           MATRIX_SIZE+2, MATRIX_SIZE+2);

	if(!pMatrix12){
		masterMatrix.copy(&masterMatrix, 2, 2, MATRIX_SIZE, 1, 2, 0);
		masterMatrix.copy(&masterMatrix, 2, 2, MATRIX_SIZE, 1, 2, 1);
	}
	if(!pMatrix21){
		masterMatrix.copy(&masterMatrix, 2, 2, 1, MATRIX_SIZE, 0, 2);
		masterMatrix.copy(&masterMatrix, 2, 2, 1, MATRIX_SIZE, 1, 2);
	}
	if(!pMatrix23){
		masterMatrix.copy(&masterMatrix, MATRIX_SIZE+1, 2, 1, MATRIX_SIZE, MATRIX_SIZE+2, 2);
		masterMatrix.copy(&masterMatrix, MATRIX_SIZE+1, 2, 1, MATRIX_SIZE, MATRIX_SIZE+3, 2);
	}
	if(!pMatrix32){
		masterMatrix.copy(&masterMatrix, 2, MATRIX_SIZE+1, MATRIX_SIZE, 1, 2, MATRIX_SIZE+2);
		masterMatrix.copy(&masterMatrix, 2, MATRIX_SIZE+1, MATRIX_SIZE, 1, 2, MATRIX_SIZE+3);
	}

	if(!pMatrix11){
		masterMatrix.setMatrixValue(0, 1, masterMatrix.getMatrixValue(0, 2));
		masterMatrix.setMatrixValue(1, 1, masterMatrix.getMatrixValue(2, 2));
		masterMatrix.setMatrixValue(1, 0, masterMatrix.getMatrixValue(2, 0));
		masterMatrix.setMatrixValue(0, 0, (masterMatrix.getMatrixValue(0, 1) + masterMatrix.getMatrixValue(1, 1) + masterMatrix.getMatrixValue(1, 0))/3);
	}
	if(!pMatrix13){
		masterMatrix.setMatrixValue(0, MATRIX_SIZE+2, masterMatrix.getMatrixValue(0, MATRIX_SIZE+1));
		masterMatrix.setMatrixValue(1, MATRIX_SIZE+2, masterMatrix.getMatrixValue(2, MATRIX_SIZE+1));
		masterMatrix.setMatrixValue(1, MATRIX_SIZE+3, masterMatrix.getMatrixValue(2, MATRIX_SIZE+3));
		masterMatrix.setMatrixValue(0, MATRIX_SIZE+3, (masterMatrix.getMatrixValue(0, MATRIX_SIZE+2) + masterMatrix.getMatrixValue(1, MATRIX_SIZE+2) + masterMatrix.getMatrixValue(1, MATRIX_SIZE+3))/3);
	}
	if(!pMatrix31){
		masterMatrix.setMatrixValue(MATRIX_SIZE+2, 0, masterMatrix.getMatrixValue(MATRIX_SIZE+1, 0));
		masterMatrix.setMatrixValue(MATRIX_SIZE+2, 1, masterMatrix.getMatrixValue(MATRIX_SIZE+1, 2));
		masterMatrix.setMatrixValue(MATRIX_SIZE+3, 1, masterMatrix.getMatrixValue(MATRIX_SIZE+3, 2));
		masterMatrix.setMatrixValue(MATRIX_SIZE+3, 0, (masterMatrix.getMatrixValue(MATRIX_SIZE+2, 0) + masterMatrix.getMatrixValue(MATRIX_SIZE+2, 1) + masterMatrix.getMatrixValue(MATRIX_SIZE+3, 1))/3);
	}
	if(!pMatrix33){
		masterMatrix.setMatrixValue(MATRIX_SIZE+2, MATRIX_SIZE+3, masterMatrix.getMatrixValue(MATRIX_SIZE+1, MATRIX_SIZE+3));
		masterMatrix.setMatrixValue(MATRIX_SIZE+2, MATRIX_SIZE+2, masterMatrix.getMatrixValue(MATRIX_SIZE+1, MATRIX_SIZE+1));
		masterMatrix.setMatrixValue(MATRIX_SIZE+3, MATRIX_SIZE+2, masterMatrix.getMatrixValue(MATRIX_SIZE+3, MATRIX_SIZE+1));
		masterMatrix.setMatrixValue(MATRIX_SIZE+3, MATRIX_SIZE+3, (masterMatrix.getMatrixValue(MATRIX_SIZE+2, MATRIX_SIZE+3) + masterMatrix.getMatrixValue(MATRIX_SIZE+2, MATRIX_SIZE+2) + masterMatrix.getMatrixValue(MATRIX_SIZE+3, MATRIX_SIZE+2))/3);
	}

	return true;
}

bool AltitudeMatrix::setDEMMatrix(Matrix<short>* pDemMatrix)
{
	if(!pDemMatrix){
		return false;
	}

	masterMatrix.copy(pDemMatrix, 0, 0, MATRIX_SIZE, MATRIX_SIZE, 2, 2);

	return true;
}

void AltitudeMatrix::doSmoothing(unsigned long smoothingCount,
                                 bool afterDct)
{
	long i;
	long j;

    for(long repeat=0; repeat<smoothingCount; repeat++){
        std::cout << "Smoothing ..." << (repeat+1) << std::endl;

        for(i=0; i<MATRIX_SIZE+4; i++){
            for(j=0; j<MATRIX_SIZE+4; j++){
                if(afterDct == false){
                    if(repeat == 0){
                        matrix[eFactor1][i][j] = (long)masterMatrix.getMatrixValue(j, i);
                        matrix[eFactor4][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 4;
                        matrix[eFactor6][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 6;
                        matrix[eFactor16][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 16;
                        matrix[eFactor24][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 24;
                        matrix[eFactor36][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 36;
                    }
                    else{
                        if(i>=2 && i<=MATRIX_SIZE+2 && j>=2 && j<=MATRIX_SIZE+2){
                            matrix[eFactor1][i][j] = matrix[eResult][i][j];
                            matrix[eFactor4][i][j] = matrix[eResult][i][j] * 4;
                            matrix[eFactor6][i][j] = matrix[eResult][i][j] * 6;
                            matrix[eFactor16][i][j] = matrix[eResult][i][j] * 16;
                            matrix[eFactor24][i][j] = matrix[eResult][i][j] * 24;
                            matrix[eFactor36][i][j] = matrix[eResult][i][j] * 36;
                        }
                        else{
                            matrix[eFactor1][i][j] = (long)masterMatrix.getMatrixValue(j, i);
                            matrix[eFactor4][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 4;
                            matrix[eFactor6][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 6;
                            matrix[eFactor16][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 16;
                            matrix[eFactor24][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 24;
                            matrix[eFactor36][i][j] = (long)masterMatrix.getMatrixValue(j, i) * 36;
                        }
                    }
                }
                else{
                    matrix[eFactor1][i][j] = matrix[eResult][i][j];
                    matrix[eFactor4][i][j] = matrix[eResult][i][j] * 4;
                    matrix[eFactor6][i][j] = matrix[eResult][i][j] * 6;
                    matrix[eFactor16][i][j] = matrix[eResult][i][j] * 16;
                    matrix[eFactor24][i][j] = matrix[eResult][i][j] * 24;
                    matrix[eFactor36][i][j] = matrix[eResult][i][j] * 36;
                }
            }
        }

        for(i=2; i<MATRIX_SIZE+2; i++){
            for(j=2; j<MATRIX_SIZE+2; j++){
                matrix[eResult][i][j] =
                    ((matrix[eFactor1][i-2][j-2] + matrix[eFactor4][i-1][j-2]  + matrix[eFactor6][i][j-2]  + matrix[eFactor4][i+1][j-2]  + matrix[eFactor1][i+2][j-2] +
                      matrix[eFactor4][i-2][j-1] + matrix[eFactor16][i-1][j-1] + matrix[eFactor24][i][j-1] + matrix[eFactor16][i+1][j-1] + matrix[eFactor4][i+2][j-1] +
                      matrix[eFactor6][i-2][j]   + matrix[eFactor24][i-1][j]   + matrix[eFactor36][i][j]   + matrix[eFactor24][i+1][j]   + matrix[eFactor6][i+2][j] +
                      matrix[eFactor4][i-2][j+1] + matrix[eFactor16][i-1][j+1] + matrix[eFactor24][i][j+1] + matrix[eFactor16][i+1][j+1] + matrix[eFactor4][i+2][j+1] +
                      matrix[eFactor1][i-2][j+2] + matrix[eFactor4][i-1][j+2]  + matrix[eFactor6][i][j+2]  + matrix[eFactor4][i+1][j+2]  + matrix[eFactor1][i+2][j+1])
                     / 256);
            }
        }
    }

	this->smoothingCount = smoothingCount;
}

void AltitudeMatrix::doDCT(long dctRadius)
{
    Dct dct;

    dct.doDCT(&masterMatrix, matrix[eResult], dctRadius);

    this->dctRadius = dctRadius;
}

bool AltitudeMatrix::setBaseFile(std::string & demFileName)
{
	baseFileName = boost::filesystem::path(demFileName).stem().generic_string();

	long latPos = baseFileName.find("_");
	baseFileName = baseFileName.substr(latPos+1, 7);
	return true;
}

void AltitudeMatrix::verify(Matrix<short > *pOrgMatrix)
{
	for(long i=2; i<MATRIX_SIZE+2; i++){
		for(long j=2; j<MATRIX_SIZE+2; j++){
			short orgAltitude = pOrgMatrix->getMatrixValue((j-2)*3, (i-2)*3);
			short resultAltitude = smoothingCount>0 ? matrix[eResult][i][j] : masterMatrix.getMatrixValue(j, i);

			if(abs(orgAltitude - resultAltitude) > HEIGHT_THRESHOLD && resultAltitude==0 && orgAltitude>0){
				std::cout << baseFileName << " x=" << (j-2) << " y=" << (i-2) << " org=" << orgAltitude << " smooth=" << resultAltitude << std::endl;
				if(smoothingCount > 0){
					matrix[eResult][i][j] = orgAltitude;
				}
				else{
					masterMatrix.setMatrixValue(j, i, orgAltitude);
				}
			}
		}
	}
}

void AltitudeMatrix::compare()
{
    long totalChange = 0;

    for(long i=2; i<MATRIX_SIZE+2; i++){
		for(long j=2; j<MATRIX_SIZE+2; j++){
			long orgAltitude = (long)masterMatrix.getMatrixValue(j, i);
			long resultAltitude = matrix[eResult][i][j];
            totalChange += abs((int)orgAltitude - (int)resultAltitude);
        }
    }
    double ratio = totalChange / (MATRIX_SIZE * MATRIX_SIZE);

    std::cout << "change ratio=" << ratio << std::endl;
}

bool AltitudeMatrix::writeJSON(std::string& outputFolder)
{
	std::cout << "Writing " << baseFileName << " ..." <<std::endl;

    boost::filesystem::create_directories(boost::filesystem::path(outputFolder));

	for(long k=0; k<3; k++){
		for(long l=0; l<3; l++){
			std::string outputFileName = outputFolder + baseFileName + (boost::format("M%1%%2%.json") % k % l).str();

			std::ofstream outStream(outputFileName.c_str(), std::ios::out |  std::ios::binary );
			if(!outStream){
				return false;
			}
			outStream << "{\"h\" : [" << std::endl;

			for(long i=0; i<MATRIX_SIZE/3; i++){
				short previous = 0;
				short diff = 0;
				short count = 0;

				std::string lineString = "\"";

				for(long j=0; j<MATRIX_SIZE/3; j++){
					short current;
					if(smoothingCount > 0 || dctRadius >= 0){
						current = matrix[eResult][(3-k)*(MATRIX_SIZE/3)-i+1][l*MATRIX_SIZE/3+j+2];
					}
					else{
						current = masterMatrix.getMatrixValue(l*MATRIX_SIZE/3+j+2, (3-k)*(MATRIX_SIZE/3)-i+1);
					}

					diff = current - previous;
					if(diff == 0){
						if(count == 0){
							lineString += ":";
						}
						count++;
					}
					else{
						if(count>0){
							lineString += heightEncode(count);
							count = 0;
						}

						diff = diff << 1;
						if(diff & 0x8000){
							diff = ~diff;
						}
						lineString += heightEncode(diff);
					}
					previous = current;
				}
				if(count>0){
					lineString += heightEncode(count);
				}
				if(i == MATRIX_SIZE/3-1){
					lineString += "\"";
				}
				else{
					lineString += "\",";
				}
				outStream << lineString << std::endl;
			}
			outStream << "]}" << std::endl;
			outStream.close();
		}
	}
	return true;
}

char* AltitudeMatrix::heightEncode(short height)
{
	static char caEncodeString[10];
	memset(caEncodeString, 0, sizeof(caEncodeString));

	long i=0;
	while(height >= 0x20){
		caEncodeString[i] = (char)((0x20 | (height & 0x1f)) + 0x3f);
		height >>= 5;
		i++;
	}
	if(height + 0x3f == 0x5c){
		caEncodeString[i] = 0x5c;
		caEncodeString[i+1] = 0x5c;
	}
	else{
		caEncodeString[i] = (char)(height + 0x3f);
	}

	return caEncodeString;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     graph.cpp
* \author   Collin Johnson
*
* Definition of convert_map_to_graph.
*/

#include <hssh/global_topological/graph.h>
#include <hssh/global_topological/topological_map.h>
#include <boost/range/adaptor/map.hpp>
#include <cassert>

namespace vulcan
{
namespace hssh
{

TopologicalVertex convert_path_segment_to_vertex(const GlobalPathSegment&  segment,
                                                 const Point<float>& plusPosition,
                                                 const Point<float>& minusPosition);
TopologicalVertex convert_frontier_path_segment_to_vertex(const GlobalPathSegment&  segment,
                                                          const Point<float>& position);

void add_explored_segment(const GlobalPathSegment& segment,
                          const TopologicalMap& map,
                          TopologicalGraph& graph);
void add_frontier_segment(const GlobalPathSegment& segment,
                          const TopologicalMap& map,
                          TopologicalGraph& graph);
GlobalPlace transition_place(const GlobalTransition& transition,
                             const GlobalPathSegment& segment,
                             const TopologicalMap& map);

TopologicalGraph convert_map_to_graph(const TopologicalMap& map)
{
    auto& segments = map.segments();

    TopologicalGraph graph;

    for(auto& s : boost::adaptors::values(segments))
    {
        if(s->isFrontier())
        {
            add_frontier_segment(*s, map, graph);
        }
        else
        {
            add_explored_segment(*s, map, graph);
        }
    }

    return graph;
}

TopologicalVertex convert_location_to_vertex(const GlobalLocation& location, const TopologicalMap& map)
{
    if(location.areaType == AreaType::path_segment)
    {
        assert(map.getPathSegment(location.areaId));

        GlobalPathSegment segment = *map.getPathSegment(location.areaId);

        if(!segment.isFrontier())
        {
            const GlobalPlace plus  = transition_place(segment.plusTransition(), segment, map);
            const GlobalPlace minus = transition_place(segment.minusTransition(), segment, map);

            return TopologicalVertex(convert_path_segment_to_vertex(segment,
                                                                    map.referenceFrame(plus.id()).toPoint(),
                                                                    map.referenceFrame(minus.id()).toPoint()));
        }
        else
        {
            auto transition = segment.plusTransition().isFrontier() ? segment.plusTransition() : segment.minusTransition();
            GlobalPlace place = transition_place(transition, segment, map);

            return TopologicalVertex(convert_frontier_path_segment_to_vertex(segment,
                                                                             map.referenceFrame(place.id()).toPoint()));
        }
    }
    else // at a place
    {
        return convert_place_to_vertex(*map.getPlace(location.areaId), map);
    }
}

TopologicalVertex convert_place_to_vertex(const GlobalPlace& place, const TopologicalMap& map)
{
    Point<float> location = map.referenceFrame(place.id()).toPoint();
    return TopologicalVertex(place.id(), location, NodeData(place.id()));
}

TopologicalVertex convert_path_segment_to_vertex(const GlobalPathSegment&  segment,
                                                 const Point<float>& plusPosition,
                                                 const Point<float>& minusPosition)
{
    Point<float> segmentLocation((plusPosition.x + minusPosition.x) / 2.0f,
                                       (plusPosition.y + minusPosition.y) / 2.0f);

    return TopologicalVertex(segment.id(),
                             segmentLocation,
                             NodeData(segment.id(), true),
                             segment.lambda().magnitude());
}

TopologicalVertex convert_frontier_path_segment_to_vertex(const GlobalPathSegment&  segment,
                                                          const Point<float>& position)
{
    // For frontier segments, put the location halfway down the explored section of the segment.
    Point<float> segmentLocation((position.x + segment.lambda().x) / 2.0f,
                                       (position.y + segment.lambda().y) / 2.0f);

    return TopologicalVertex(segment.id(),
                             segmentLocation,
                             NodeData(segment.id(), true),
                             segment.lambda().magnitude());
}

void add_explored_segment(const GlobalPathSegment& segment,
                          const TopologicalMap& map,
                          TopologicalGraph& graph)
{
    // Ids for the edges in the graph are monotonically increasing. Can just use the current number of edges
    // as the benchmark for determining the next id

    auto plus  = segment.plusTransition();
    auto minus = segment.minusTransition();

    TopologicalVertex plusVertex    = convert_place_to_vertex(transition_place(plus, segment, map), map);
    TopologicalVertex minusVertex   = convert_place_to_vertex(transition_place(minus, segment, map), map);
    TopologicalVertex segmentVertex = convert_path_segment_to_vertex(segment,
                                                                     plusVertex.getPosition(),
                                                                     minusVertex.getPosition());

    TopologicalEdge   plusEdge (graph.numEdges() + 1, plusVertex,  segmentVertex, 0.0);
    TopologicalEdge   minusEdge(graph.numEdges() + 2, minusVertex, segmentVertex, 0.0);

    graph.addVertex(plusVertex);
    graph.addVertex(minusVertex);
    graph.addVertex(segmentVertex);
    graph.addEdge(plusEdge);
    graph.addEdge(minusEdge);
}

void add_frontier_segment(const GlobalPathSegment& segment,
                          const TopologicalMap& map,
                          TopologicalGraph& graph)
{
    auto transition = segment.plusTransition().isFrontier() ? segment.plusTransition() : segment.minusTransition();

    TopologicalVertex placeVertex   = convert_place_to_vertex(transition_place(transition, segment, map), map);
    TopologicalVertex segmentVertex = convert_frontier_path_segment_to_vertex(segment, placeVertex.getPosition());

    TopologicalEdge edge(graph.numEdges() + 1, placeVertex,  segmentVertex, 0.0);

    graph.addVertex(placeVertex);
    graph.addVertex(segmentVertex);
    graph.addEdge(edge);
}

GlobalPlace transition_place(const GlobalTransition& transition,
                             const GlobalPathSegment& segment,
                             const TopologicalMap& map)
{
    return *map.getPlace(transition.otherArea(segment.toArea()).id());
}

} // namespace hssh
} // namespace vulcan


/*
 * ChiSquaredThreeJetsTopPairReconstruction.cpp
 *
 *  Created on: 2 Sep 2011
 *      Author: kreczko
 */

#include "../../interface/ReconstructionModules/ChiSquaredThreeJetsTopPairReconstruction.h"
#include <boost/lexical_cast.hpp>
#include "../../interface/ReconstructionModules/ReconstructionException.h"

namespace BAT {

bool ChiSquaredThreeJetsTopPairReconstruction::meetsJetFromWRequirement(unsigned short jet1Index,
		unsigned short jet2Index) {
	return jet1Index == jet2Index; // two jets from W merged, therefore only 3 jets are needed
}

bool ChiSquaredThreeJetsTopPairReconstruction::meetsGlobalRequirement(const TtbarHypothesisPointer solution) {
	return solution->isPhysical() && solution->hadronicW->mass() > 40;
}

bool ChiSquaredThreeJetsTopPairReconstruction::meetsInitialCriteria() const {
	return met != 0 && leptonFromW != 0 && jets.size() == 3; // needs exactly 3 jets
}

std::string ChiSquaredThreeJetsTopPairReconstruction::getDetailsOnFailure() const {
	std::string msg = "Initial Criteria not met: \n";
	if (leptonFromW == 0)
		msg += "Electron from W: not filled \n";
	else
		msg += "Electron from W: filled \n";

	if (met == 0)
		msg += "Missing transverse energy: not filled \n";
	else
		msg += "Missing transverse energy: filled \n";
	std::string nJets(boost::lexical_cast<std::string>(jets.size()));
	if (jets.size() != 3) {
		if (jets.size() > 3)
			msg += "Number of jets is too large:" + nJets + ", should be == 3 \n";
		else
			msg += "Number of jets is too small:" + nJets + ", should be == 3 \n";
	}

	else
		msg += "Number of jets is OK:" + nJets + "\n";

	return msg;
}

ChiSquaredThreeJetsTopPairReconstruction::ChiSquaredThreeJetsTopPairReconstruction(const LeptonPointer lepton,
		const METPointer met, const JetCollection jets) :
		BasicTopPairReconstruction(lepton, met, jets), ChiSquaredBasedTopPairReconstruction(lepton, met, jets) {

}

ChiSquaredThreeJetsTopPairReconstruction::~ChiSquaredThreeJetsTopPairReconstruction() {
}

} /* namespace BAT */


// Copyright Oleg Maximenko 2014.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/parser/detail/common.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>

namespace svgpp
{

namespace qi = boost::spirit::qi;

namespace detail
{
  typedef boost::tuple<unsigned char, unsigned char, unsigned char> rgb_t;

  namespace
  {
    struct color_keywords
    {
      typedef qi::symbols<char, rgb_t> symbols;
      static bool initialized_;
      static symbols symbols_;
    };

    bool color_keywords::initialized_ = false;
    color_keywords::symbols color_keywords::symbols_;
  }
}

template <class PropertySource, class Iterator, class ColorFactory>
class color_grammar: public qi::grammar<Iterator, typename ColorFactory::color_type()>
{
  typedef color_grammar<PropertySource, Iterator, ColorFactory> this_type;
public:
  typedef typename ColorFactory::color_type color_type;

  color_grammar()
    : this_type::grammar(color)
  {
    namespace phx = boost::phoenix;
    using qi::_1;
    using qi::_2;
    using qi::_3;
    using qi::_a;
    using qi::_b;
    using qi::_c;
    using qi::_val;
    using qi::lit;
    using detail::character_encoding_namespace::space;

    comma
        =   *space
            >> ','
            >> *space;
    color
        =   hex_rule [_val = _1]
        |   (
                 detail::no_case_if_css(PropertySource())[ lit("rgb(") ]
              >> *space
              >> (  components_rule [_val = _1]
                 |  percentage_rule [_val = _1]
                 )
              >> *space
              >> ')'
            )
        |   detail::color_keywords::symbols_ [_val = phx::bind(&color_grammar::color_keyword, _1)];

    hex_rule
        =   lit('#')
            >> hex3 [_a = _1]
            >> ( hex3 [_val = phx::bind(&color_grammar::six_hex_digits, _a, _1)]
               | qi::eps [_val = phx::bind(&color_grammar::three_hex_digits, _a)]
               );

    components_rule
        =   (  integer
            >> comma
            >> integer
            >> comma
            >> integer
            ) [_val = phx::bind(&color_grammar::absolute_components, _1, _2, _3)];

    percentage_rule
        =   (  number
            >> '%'
            >> comma
            >> number
            >> '%'
            >> comma
            >> number
            >> '%'
            ) [_val = phx::bind(&color_grammar::percent_components, _1, _2, _3)];
  }

private:
  typedef typename ColorFactory::percentage_type number_type;

  typename this_type::start_type color;
  qi::rule<Iterator> comma;
  qi::rule<Iterator, color_type (), qi::locals<unsigned int> > hex_rule;
  qi::rule<Iterator, color_type ()> components_rule;
  qi::rule<Iterator, color_type ()> percentage_rule;
  qi::uint_parser<unsigned char, 10, 1, 3> integer;
  // There was mistake in SVG 1.1 that limits percentage to integer values, while CSS permits floating point numbers.
  // Till fixed version is not released, we will use CSS version of percentage definition.
  qi::real_parser<number_type, detail::number_policies<number_type, tag::source::css> > number;
  qi::uint_parser<unsigned int, 16, 3, 3> hex3;

  static color_type three_hex_digits(unsigned int h)
  {
    return ColorFactory::create(
      static_cast<unsigned char>( ( (h & 0xf00) >> 8 ) | ( (h & 0xf00) >> 4 ) ),
      static_cast<unsigned char>( ( (h & 0x0f0) >> 4 ) |   (h & 0x0f0) ),
      static_cast<unsigned char>( ( (h & 0x00f) << 4 ) |   (h & 0x00f) ) );
  }

  static color_type six_hex_digits(unsigned int h1, unsigned int h2)
  {
    return ColorFactory::create(
      static_cast<unsigned char>( h1 >> 4 ),
      static_cast<unsigned char>( ( ( h1 & 0x0f ) << 4 ) | ( h2 >> 8 ) ),
      static_cast<unsigned char>( h2 & 0xff ) );
  }

  static color_type absolute_components(unsigned char r, unsigned char g, unsigned char b)
  {
    return ColorFactory::create(r, g, b);
  }

  static color_type percent_components(number_type r, number_type g, number_type b)
  {
    return ColorFactory::create_from_percent(r, g, b);
  }

  static color_type color_keyword(detail::rgb_t const & rgb)
  {
    return ColorFactory::create(rgb.get<0>(), rgb.get<1>(), rgb.get<2>());
  }
};

namespace detail
{
  namespace
  {
    struct color_keywords_initializer
    {
      color_keywords_initializer()
      {
        if (!color_keywords::initialized_)
        {
          color_keywords::symbols_.add
#define SVGPP_ON(name, r, g, b) (#name, rgb_t(r, g, b))
#include <svgpp/detail/dict/enumerate_colors.inc>
#undef SVGPP_ON
            ;
          color_keywords::initialized_ = true;
        }
      }
    };

    color_keywords_initializer color_keywords_initializer_instance;
  }
}

}


#define BOOST_TEST_MODULE test_magnusson

#include <iostream>
#include <boost/test/unit_test.hpp>
#include "graph.h"
#include "magnusson.h"

using namespace dpct;

BOOST_AUTO_TEST_CASE(minimal_test_magnusson)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 3,-10}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(1, {0, 3,-10}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 2: Node 1"));

    g.addMoveArc(n1, n2, {1.0});

    Magnusson tracker(&g, false);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 7.0);
    BOOST_CHECK_EQUAL(paths.size(), 1);
}

BOOST_AUTO_TEST_CASE(two_cell_test_magnusson)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 3, 5, -10}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(1, {0, 3, 4, -10}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 2: Node 1"));

    g.addMoveArc(n1, n2, {1.0, 1.0});

    Magnusson tracker(&g, false);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 11.0); // arc score is applied only once!
    BOOST_CHECK_EQUAL(paths.size(), 2);
}

BOOST_AUTO_TEST_CASE(magnusson_no_swap_failure_case)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    Magnusson tracker(&g, false);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 18.0);
    BOOST_CHECK_EQUAL(paths.size(), 1);
}

BOOST_AUTO_TEST_CASE(magnusson_simple_swap_test)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    Magnusson tracker(&g, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 29.0);
    BOOST_CHECK_EQUAL(paths.size(), 2);

    // check the paths
    for(TrackingAlgorithm::Path& p : paths)
    {
        for(Arc* a : p)
        {
            BOOST_CHECK_NE(a->getType(), Arc::Swap);
            BOOST_CHECK(a->getSourceNode() != n4.get() || a->getTargetNode() != n5.get());
        }
    }
}

BOOST_AUTO_TEST_CASE(magnusson_simple_swap_test_with_app_dis)
{
    Graph::Configuration config(true, true, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 10}, {10.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 2: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n1, n4, {2.0});
    g.addMoveArc(n2, n4, {2.0});

    Magnusson tracker(&g, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    // BOOST_CHECK_EQUAL(score, 29.0);
    // BOOST_CHECK_EQUAL(paths.size(), 2);
}

BOOST_AUTO_TEST_CASE(magnusson_two_possible_swaps)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));
    Graph::NodePtr n10 = g.addNode(0, {0, 2}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 3"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));
    Graph::NodePtr n11 = g.addNode(0, {0, 2}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 3"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));
    Graph::NodePtr n12 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 3"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});
    g.addMoveArc(n10, n11, {0.0});
    g.addMoveArc(n11, n12, {0.0});
    g.addMoveArc(n11, n5, {0.0});
    g.addMoveArc(n4, n12, {0.0});

    Magnusson tracker(&g, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 34.0);
    BOOST_CHECK_EQUAL(paths.size(), 3);
}

void buildGraph(Graph& g)
{
    std::vector<double> appearanceScore(2, -200);
    std::vector<double> disappearanceScore(2, -200);
    std::vector<double> divisionScore(2, -200);

    // -----------------------------------------------------
    // Timestep 1
    Graph::NodePtr n_1_1 = g.addNode(0, {0, 3, 2}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 1"));

    Graph::NodePtr n_1_2 = g.addNode(0, {0, 2, 7}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n_1_3 = g.addNode(0, {0, 3, -2}, appearanceScore, disappearanceScore,
                                        true, false, std::make_shared<NameData>("Timestep 1: Node 3"));

    // -----------------------------------------------------
    // Timestep 2
    Graph::NodePtr n_2_1 = g.addNode(1, {0, 4, 2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 1"));

    Graph::NodePtr n_2_2 = g.addNode(1, {0, 2, 0}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n_2_3 = g.addNode(1, {0, 2, -2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 3"));

    Graph::NodePtr n_2_4 = g.addNode(1, {0, 2, 0}, appearanceScore, {-1.0, -1.0},
                                        false, false, std::make_shared<NameData>("Timestep 2: Node 4"));

    g.addMoveArc(n_1_1, n_2_1, {0.0});
    g.addMoveArc(n_1_2, n_2_2, {0.0});
    g.addMoveArc(n_1_2, n_2_3, {0.0});
    g.addMoveArc(n_1_3, n_2_4, {0.0});

    // -----------------------------------------------------
    // Timestep 3
    Graph::NodePtr n_3_1 = g.addNode(2, {0, 3, 5}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 3: Node 1"));

    Graph::NodePtr n_3_2 = g.addNode(2, {0, 2, 2}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 3: Node 2"));

    Graph::NodePtr n_3_3 = g.addNode(2, {0, 3, 0}, appearanceScore, disappearanceScore,
                                        false, false, std::make_shared<NameData>("Timestep 3: Node 3"));

    g.addMoveArc(n_2_1, n_3_1, {0.0});
    g.addMoveArc(n_2_2, n_3_2, {0.0});
    g.addMoveArc(n_2_3, n_3_3, {0.0});

    // -----------------------------------------------------
    // Timestep 4
    Graph::NodePtr n_4_1 = g.addNode(3, {0, 4, 3}, appearanceScore, disappearanceScore,
                                        false, true, std::make_shared<NameData>("Timestep 4: Node 1"));

    Graph::NodePtr n_4_2 = g.addNode(3, {0, 2, 1}, appearanceScore, disappearanceScore,
                                        false, true, std::make_shared<NameData>("Timestep 4: Node 2"));

    Graph::NodePtr n_4_3 = g.addNode(3, {0, 2, -4}, appearanceScore, disappearanceScore,
                                        false, true, std::make_shared<NameData>("Timestep 4: Node 3"));

    Graph::NodePtr n_4_4 = g.addNode(3, {0, 4, 2}, appearanceScore, disappearanceScore,
                                        false, true, std::make_shared<NameData>("Timestep 4: Node 4"));

    g.addMoveArc(n_3_1, n_4_1, {0.0});
    g.addMoveArc(n_3_1, n_4_2, {0.0});
    g.addMoveArc(n_3_2, n_4_2, {0.0});
    g.addMoveArc(n_3_2, n_4_3, {0.0});
    g.addMoveArc(n_3_3, n_4_3, {0.0});
    g.addMoveArc(n_3_3, n_4_4, {0.0});

    g.allowMitosis(n_3_2, n_4_3, -1);
    g.allowMitosis(n_3_3, n_4_3, 2);

    std::cout << "Done setting up graph" << std::endl;
}

BOOST_AUTO_TEST_CASE(test_full_magnusson_no_swap)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, false, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    BOOST_CHECK_EQUAL(paths.size(), 5);
    BOOST_CHECK_EQUAL(score, 44.0);

    std::cout << "Tracker returned score " << score << std::endl;
}

BOOST_AUTO_TEST_CASE(test_full_magnusson_with_swap)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    BOOST_CHECK_EQUAL(paths.size(), 5);
    BOOST_CHECK_EQUAL(score, 44.0);

    std::cout << "Tracker returned score " << score << std::endl;
}

BOOST_AUTO_TEST_CASE(test_full_magnusson_graph_constness)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    size_t num_arcs = g.getNumArcs();
    size_t num_timesteps = g.getNumTimesteps();
    size_t num_nodes = g.getNumNodes();

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    tracker.track(paths);

    BOOST_CHECK_EQUAL(g.getNumArcs(), num_arcs);
    BOOST_CHECK_EQUAL(g.getNumNodes(), num_nodes);
    BOOST_CHECK_EQUAL(g.getNumTimesteps(), num_timesteps);
}

void updateNode(Node* n)
{
    n->updateBestInArcAndScore();

    for(Node::ArcIt outArc = n->getOutArcsBegin(); outArc != n->getOutArcsEnd(); ++outArc)
    {
        (*outArc)->update();
    }
}

BOOST_AUTO_TEST_CASE(test_backward_path)
{
    using std::placeholders::_1;
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, true, true);
    // init best in arcs
    for(size_t t = 0; t < g.getNumTimesteps(); ++t)
    {
        g.visitNodesInTimestep(t, std::bind(&updateNode, _1));
    }
    g.visitSpecialNodes(std::bind(&updateNode, _1));

    TrackingAlgorithm::Solution paths;
    tracker.findNonintersectingBackwardPaths(&g.getSourceNode(), &g.getSinkNode(), paths);

    BOOST_CHECK(paths.size() > 0);
    BOOST_CHECK(paths.size() <= g.getSinkNode().getNumInArcs());

    // test that none of the paths overlap
    std::map<const Arc*, size_t> arcUseCount;
    for(TrackingAlgorithm::Path& p : paths)
    {
        for(const Arc* a : p)
        {
            BOOST_CHECK(arcUseCount.find(a) == arcUseCount.end());
            arcUseCount[a] = 1;
        }
    }
}

BOOST_AUTO_TEST_CASE(test_full_magnusson_fast_1st_iter)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, false, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    BOOST_CHECK(paths.size() <= 5);
    BOOST_CHECK(score <= 44.0);
    BOOST_CHECK(score >= 0);

    std::cout << "Tracker returned score " << score << std::endl;

    // Magnusson tracker2(&g, false, false, false);
    // std::vector<TrackingAlgorithm::Path> paths2;
    // score = tracker2.track(paths2);
    // BOOST_CHECK(tracker.getElapsedSeconds() < tracker2.getElapsedSeconds());
}

BOOST_AUTO_TEST_CASE(test_magnusson_maxNumPaths)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    buildGraph(g);

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, false, true, true);
    std::vector<TrackingAlgorithm::Path> paths;
    tracker.setMaxNumberOfPaths(2);
    double score = tracker.track(paths);

    BOOST_CHECK(paths.size() == 2);
    BOOST_CHECK(score <= 44.0);
    BOOST_CHECK(score >= 0);

    std::cout << "Tracker returned score " << score << std::endl;

    // Magnusson tracker2(&g, false, false, false);
    // std::vector<TrackingAlgorithm::Path> paths2;
    // score = tracker2.track(paths2);
    // BOOST_CHECK(tracker.getElapsedSeconds() < tracker2.getElapsedSeconds());
}

BOOST_AUTO_TEST_CASE(magnusson_selector_func)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    Magnusson tracker(&g, false);
    tracker.setPathStartSelectorFunction([](Node* n) -> Arc* {
        Node::ArcIt it = n->getInArcsBegin() + (n->getNumInArcs() - 1);
        return *it;
    });
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 17.0);
    BOOST_CHECK_EQUAL(paths.size(), 1);
}

BOOST_AUTO_TEST_CASE(magnusson_selector_func_2nd_best)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    Magnusson tracker(&g, false);
    tracker.setPathStartSelectorFunction(selectSecondBestInArc);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK_EQUAL(score, 29.0);
    BOOST_CHECK_EQUAL(paths.size(), 2);
}

BOOST_AUTO_TEST_CASE(magnusson_selector_func_random)
{
    Graph::Configuration config(false, false, false);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 1"));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<NameData>("Timestep 1: Node 2"));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 1"));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<NameData>("Timestep 2: Node 2"));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 1"));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<NameData>("Timestep 3: Node 2"));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    Magnusson tracker(&g, false);
    tracker.setPathStartSelectorFunction(selectAtRandom);
    std::vector<TrackingAlgorithm::Path> paths;
    double score = tracker.track(paths);

    std::cout << "Tracker returned score " << score << std::endl;
    BOOST_CHECK(score >= 17.0);
    BOOST_CHECK(paths.size() >= 1);
}

class PositionData2D : public UserData
{
public:
    PositionData2D(double x, double y):
        x_(x),
        y_(y)
    {}

    virtual std::string toString() const
    {
        std::stringstream s;
        s << "Pos(" << x_ << ", " << y_ << ")";
        return s.str();
    }

    double X() const { return x_; }
    double Y() const { return y_; }
private:
    double x_, y_;
};

/*
// TODO: fix swap arc handling when motion model is present!
BOOST_AUTO_TEST_CASE(test_full_magnusson_motion_model)
{
    Graph::Configuration config(true, true, true);
    Graph g(config);

    Graph::NodePtr n1 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<PositionData2D>(0,0));
    Graph::NodePtr n2 = g.addNode(0, {0, 1}, {0.0}, {0.0}, true, false, std::make_shared<PositionData2D>(1,0));

    Graph::NodePtr n3 = g.addNode(1, {0, 5}, {0.0}, {0.0}, false, false, std::make_shared<PositionData2D>(0,1));
    Graph::NodePtr n4 = g.addNode(1, {0, 15}, {0.0}, {0.0}, false, false, std::make_shared<PositionData2D>(1,1));

    Graph::NodePtr n5 = g.addNode(2, {0, 2}, {0.0}, {0.0}, false, true, std::make_shared<PositionData2D>(0,2));
    Graph::NodePtr n6 = g.addNode(2, {0, 1}, {0.0}, {0.0}, false, true, std::make_shared<PositionData2D>(1,2));

    g.addMoveArc(n1, n3, {0.0});
    g.addMoveArc(n2, n4, {0.0});
    g.addMoveArc(n3, n5, {4.0});
    g.addMoveArc(n4, n5, {0.0});
    g.addMoveArc(n4, n6, {0.0});

    // set up motion model
    Magnusson::MotionModelScoreFunction momoscofu = [&](Node* a, Node* b, Node* c)
    {
        typedef std::shared_ptr<PositionData2D> Pos2DPtr;

        // only evaluate if we're looking at 3 proper nodes, otherwise return default value
        if(a == nullptr || b == nullptr || c == nullptr || g.isSpecialNode(a) || g.isSpecialNode(b) || g.isSpecialNode(c))
        {
            return -10.0;
        }

        Pos2DPtr posA = std::static_pointer_cast<PositionData2D>(a->getUserData());
        Pos2DPtr posB = std::static_pointer_cast<PositionData2D>(b->getUserData());
        Pos2DPtr posC = std::static_pointer_cast<PositionData2D>(c->getUserData());
        double dist1 = sqrt( pow(posA->X() - posB->X(), 2.0) + pow(posA->Y() - posB->Y(), 2.0) );
        double dist2 = sqrt( pow(posC->X() - posB->X(), 2.0) + pow(posC->Y() - posB->Y(), 2.0) );

        double velocityMagnitude = fabsf(dist1 - dist2);
        std::cout << "Points: {" << posA->toString() << ", " << posB->toString() << ", " << posC->toString()
                  << "} have distances " << dist1 << ", " << dist2 << " and vel " << velocityMagnitude << std::endl;
        return -velocityMagnitude;
    };

    // -----------------------------------------------------
    // Tracking
    Magnusson tracker(&g, false, false);
    std::vector<TrackingAlgorithm::Path> paths;
    tracker.setMotionModelScoreFunction(momoscofu);
    double score = tracker.track(paths);

    BOOST_CHECK_EQUAL(paths.size(), 3);
    BOOST_CHECK(score < 24.6 && score > 24.5); // should be 25 - (sqrt(2)-1)

    std::cout << "Tracker returned score " << score << std::endl;
}
*/


/*
 * Copyright 2017 Maeve Automation
 *
 * 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 "ar_isp_field/node_handler.h"

#include <cv_bridge/cv_bridge.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/optional.hpp>

#include <algorithm>
#include <limits>
#include <string>
#include <tuple>
#include <vector>

#include "ar_isp_field/geometry.h"
#include "open_maeve/isp_field/isp_field.h"
#include "open_maeve/isp_field/visualize.h"
#include "open_maeve/maeve_geometry/tau.h"

namespace open_maeve {
namespace {
const auto EPS = 1e-4;
const auto NaN = std::numeric_limits<double>::quiet_NaN();
const auto INF = std::numeric_limits<double>::infinity();
}  // namespace

//------------------------------------------------------------------------------

std::vector<std::string> AR_ISPFieldNodeHandler::initializeTimeQueues(
    const std::vector<int>& id_list) {
  std::vector<std::string> frame_names;
  frame_names.reserve(id_list.size());

  std::for_each(std::begin(id_list), std::end(id_list), [&](const int id) {
    const auto ar_frame_name = params_.ar_frame_prefix + std::to_string(id);
    frame_names.push_back(ar_frame_name);
    ar_max_extent_time_queue_[ar_frame_name] = MaeveTimeQueue<double>(
        params_.ar_time_queue_size, params_.ar_time_queue_max_gap);
  });

  return frame_names;
}

//------------------------------------------------------------------------------

AR_ISPFieldNodeHandler::AR_ISPFieldNodeHandler(const std::string& node_name)
    : nh_(node_name), it_(nh_), tf2_listener_(tf2_buffer_) {
  if (!params_.load(nh_)) {
    ROS_FATAL_STREAM("Failed to load parameters. Fatal error.");
    return;
  }

  // Set up frame list and time queues.
  ar_obstacle_tag_frames_ = initializeTimeQueues(params_.ar_tag_obstacle_ids);
  ar_target_tag_frames_ = initializeTimeQueues(params_.ar_tag_target_ids);

  // Compute raw tag corner points (CW ordering).
  const auto half_extent = params_.ar_tag_size / 2.0;
  ar_corner_points_[0] = Eigen::Vector3d(half_extent, -half_extent, 0.0);
  ar_corner_points_[1] = Eigen::Vector3d(half_extent, half_extent, 0.0);
  ar_corner_points_[2] = Eigen::Vector3d(-half_extent, half_extent, 0.0);
  ar_corner_points_[3] = Eigen::Vector3d(-half_extent, -half_extent, 0.0);

  // Instantiate transforms.
  hc_ = PotentialTransform<ConstraintType::HARD>(
      params_.hard_constraint_transform);
  sc_ = PotentialTransform<ConstraintType::SOFT>(
      params_.soft_constraint_transform);

  // Register callback.
  camera_sub_ = it_.subscribeCamera(
      params_.camera_topic, 1, &AR_ISPFieldNodeHandler::cameraCallback, this);

  // Set up command handler.
  command2d_mgr_.initialize(nh_, params_.control_command_input_topic);

  // Control command output publisher.
  control_command_output_pub_ =
      nh_.advertise<controller_interface_msgs::Command2D>(
          params_.control_command_output_topic, 1);

  // Visualize?
  viz_isp_field_pub_ = it_.advertise(params_.viz_isp_field_topic, 1);
  horizon_visualizer_.initialize(
      HorizonVisualizer::Params(params_.horizon_viz_height,
                                sc_.shapeParameters().range_min,
                                sc_.shapeParameters().range_max),
      params_.visualize_horizons, it_);
}

//------------------------------------------------------------------------------

std::vector<cv::Point2d> AR_ISPFieldNodeHandler::projectPoints(
    const Eigen::Affine3d& camera_T_artag) const {
  // Compute the corner points under the given transform.
  const auto corner_points =
      arTagCornerPoints(camera_T_artag, ar_corner_points_);

  // Allocate return structure.
  std::vector<cv::Point2d> image_points;
  image_points.reserve(corner_points.size());

  // Convert Eigen -> OpenCV.
  const auto camera_points = arEigenPoints2OpenCV(corner_points);

  // Project camera_points into image_points.
  for (auto i = 0; i < 4; ++i) {
    image_points.push_back(camera_model_.project3dToPixel(camera_points[i]));
  }

  // Done.
  return image_points;
}

//------------------------------------------------------------------------------

boost::optional<std::tuple<Eigen::Affine3d, double>>
AR_ISPFieldNodeHandler::getTransformAndStamp(const std::string& ar_tag_frame,
                                             const ros::Time& timestamp) const {
  Eigen::Affine3d T;
  ros::Time T_timestamp;
  try {
    const auto T_msg = tf2_buffer_.lookupTransform(params_.camera_frame_name,
                                                   ar_tag_frame, ros::Time(0));
    T = tf2::transformToEigen(T_msg);
    T_timestamp = T_msg.header.stamp;
    const auto age = (timestamp - T_timestamp).toSec();

    // If the transform is too old, the detection is stale.
    if (age > params_.ar_tag_max_age) {
      return boost::none;
    }
  } catch (const tf2::TransformException& ex) {
    return boost::none;
  }

  return std::make_tuple(T, T_timestamp.toSec());
}

//------------------------------------------------------------------------------

bool AR_ISPFieldNodeHandler::computePotentialFields(
    const ros::Time& timestamp, const ConstraintType& constraint_type,
    FieldMap& field_map) {
  auto updated = false;

  // Compute max extents for each AR tag and add to time queues.
  std::for_each(
      std::begin(field_map), std::end(field_map),
      [&](FieldMap::value_type& pair) {
        // Get references and initialize field.
        const auto& frame_name = pair.first;
        auto& field = pair.second;
        field.setTo(0.0);

        // Only use valid transforms.
        auto t = NaN;
        Eigen::Affine3d camera_T_artag = Eigen::Affine3d::Identity();
        if (const auto vals = getTransformAndStamp(frame_name, timestamp)) {
          std::tie(camera_T_artag, t) = *vals;
        } else {
          return;
        }

        // Project AR tag onto image plane.
        const auto image_corner_points = projectPoints(camera_T_artag);

        // Get max extent and add to time queue.
        const auto s = arComputeMaxXY_Extent(image_corner_points);
        ar_max_extent_time_queue_[frame_name].insert(t, s);

        // Compute measurement values.
        // TODO(me): should put a filter on these dt values.
        auto s_dot = NaN;
        auto t_delta = NaN;
        if (const auto dt = ar_max_extent_time_queue_[frame_name].bfd_dt(t)) {
          std::tie(t_delta, s_dot) = *dt;
        } else {
          // If the backward differencing operation fails probably the queue has
          // recently become empty. It's expected and probably not an error.
          return;
        }
        const auto tau = tauFromDiscreteScaleDt(s, s_dot, t_delta, EPS);
        const auto tau_dot = 0.0;

        // Compute potential values.
        const auto p_value = (constraint_type == ConstraintType::HARD)
                                 ? hc_(cv::Point2d(tau, tau_dot))
                                 : sc_(cv::Point2d(params_.target_reward, 0.0));

        // Print output?
        if (params_.verbose && std::isfinite(tau)) {
          ROS_INFO_STREAM(constraint_type << " :" << frame_name << " - tau: "
                                          << tau << ", tau_dot: " << tau_dot);
          ROS_INFO_STREAM(constraint_type << " :" << frame_name
                                          << " - k0: " << p_value.x
                                          << ", k1: " << p_value.y);
        }

        // Fill ISP.
        arFillISP(p_value, image_corner_points, field);

        // Mark that at least one potential field has been computed.
        updated = true;
      });

  // Done.
  return updated;
}

//------------------------------------------------------------------------------

void AR_ISPFieldNodeHandler::initFieldStorage(
    const cv::Size& size, const std::vector<std::string>& frame_list,
    FieldMap& field_map) {
  std::for_each(std::begin(frame_list), std::end(frame_list),
                [&](const std::string& frame_name) {
                  field_map[frame_name] = zeroISP_Field(size);
                });
}

//------------------------------------------------------------------------------

void AR_ISPFieldNodeHandler::cameraCallback(
    const sensor_msgs::Image::ConstPtr& msg,
    const sensor_msgs::CameraInfoConstPtr& info_msg) {
  // Make sure field maps have storage allocated, but do it only once.
  static auto init = true;
  if (init) {
    // Initialize camera model.
    camera_model_.fromCameraInfo(info_msg);

    // Initialize ISP controller.
    params_.isp_controller_params.principal_point_x = camera_model_.cx();
    params_.isp_controller_params.focal_length_x = camera_model_.fx();
    isp_controller_ = ISP_Controller2D(params_.isp_controller_params);

    // Initialize storage.
    initFieldStorage(camera_model_.fullResolution(), ar_obstacle_tag_frames_,
                     obstacle_field_map_);
    initFieldStorage(camera_model_.fullResolution(), ar_target_tag_frames_,
                     target_field_map_);
    init = false;
  }

  // Compute a potential field for each tag.
  const auto obstacles_updated = computePotentialFields(
      msg->header.stamp, ConstraintType::HARD, obstacle_field_map_);
  const auto targets_updated = computePotentialFields(
      msg->header.stamp, ConstraintType::SOFT, target_field_map_);

  // Compose fields into an ISP.
  cv::Mat ISP = computeISP();

  // Do any requested visualization.
  visualize(ISP, msg->header);

  // Get most recent desired control.
  ControlCommand u_d;
  if (const auto cmd_msg_ptr_opt = command2d_mgr_.most_recent_msg_ptr()) {
    const auto& cmd_msg = *(*cmd_msg_ptr_opt);
    u_d = command2D_Msg2ControlCommand(cmd_msg);
    if (!u_d.valid()) {
      u_d = params_.default_guidance_control;
      ROS_ERROR_STREAM("u_d not valid: "
                       << u_d << ", sending default guidance control: " << u_d);
    }
  } else {
    u_d = params_.default_guidance_control;
  }

  // Don't try to use uninitialized controller.
  if (!isp_controller_.isInitialized()) {
    ROS_ERROR_STREAM("ISP controller is not initialized.");
    return;
  }

  // Compute control with desired method.
  const auto& p = ISP_Controller2D::get_params(isp_controller_);
  const cv::Rect ROI =
      ISP_ROI(ISP, p.erosion_kernel.height, p.erosion_kernel.horizon);
  const auto u_star = params_.potential_only_guidance
                          ? isp_controller_.potentialControl(ISP, ROI)
                          : isp_controller_.SD_Control(ISP, ROI, u_d);

  // Visualize controller horizons; do this after computing control.
  horizon_visualizer_.visualize(msg->header, isp_controller_);

  // Publish control.
  control_command_output_pub_.publish(
      controlCommand2Command2D_Msg(u_star, msg->header));
}

//------------------------------------------------------------------------------

cv::Mat AR_ISPFieldNodeHandler::computeISP() const {
  // Initialize to zeroed field.
  cv::Mat ISP = zeroISP_Field(camera_model_.fullResolution());

  // Compose hard constraint field.
  std::for_each(
      std::begin(obstacle_field_map_), std::end(obstacle_field_map_),
      [&](const FieldMap::value_type& pair) { ISP = ISP + pair.second; });

  // Compose soft constraint field.
  std::for_each(
      std::begin(target_field_map_), std::end(target_field_map_),
      [&](const FieldMap::value_type& pair) { ISP = ISP + pair.second; });

  // Done.
  return ISP;
}

//------------------------------------------------------------------------------

void AR_ISPFieldNodeHandler::visualize(const cv::Mat& ISP,
                                       const std_msgs::Header& header) const {
  // If no topic, nothing to do.
  if (params_.viz_isp_field_topic.empty()) {
    return;
  }

  // Compute visualization of ISP.
  const auto visual =
      computeISPFieldVisualization(ISP, params_.viz_potential_bounds);

  // Convert visualization to ROS message.
  const auto viz_msg = cv_bridge::CvImage(header, "bgr8", visual).toImageMsg();

  // Publish.
  viz_isp_field_pub_.publish(viz_msg);
}

//------------------------------------------------------------------------------

}  // namespace open_maeve


//
//   Project Name:        Kratos
//   Last modified by:    $Author: antonia $
//   Date:                $Date: 2009-01-14 12:09:16 $
//   Revision:            $Revision: 1.6 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "python/add_equation_systems_to_python.h"
#include "spaces/ublas_space.h"
//#include "spaces/parallel_ublas_space.h"

#include "linear_solvers/direct_solver.h"
#include "linear_solvers/iterative_solver.h"
#include "linear_solvers/skyline_lu_factorization_solver.h"
#include "external_includes/superlu_solver.h"
#include "external_includes/gmres_solver.h"

namespace Kratos
{

namespace Python
{
    void  AddLinearSolversToPython()
    {
        typedef UblasSpace<double, CompressedMatrix, Vector> SpaceType;
	//      typedef ParallelUblasSpace<double, CompressedMatrix, Vector> ParallelSpaceType;
        typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;
	// typedef UblasSpace<double, Matrix, Vector> ParallelLocalSpaceType;
        typedef LinearSolver<SpaceType,  LocalSpaceType> LinearSolverType;
        //typedef LinearSolver<ParallelSpaceType,  ParallelLocalSpaceType> ParallelLinearSolverType;
        typedef DirectSolver<SpaceType,  LocalSpaceType> DirectSolverType;
        //typedef Reorderer<ParallelSpaceType,  ParallelLocalSpaceType > ParallelReordererType;
        //typedef DirectSolver<ParallelSpaceType,  ParallelLocalSpaceType, ParallelReordererType > ParallelDirectSolverType;
        typedef SuperLUSolver<SpaceType,  LocalSpaceType> SuperLUSolverType;
        typedef IterativeSolver<SpaceType, LocalSpaceType> IterativeSolverType;
        typedef GMRESSolver<SpaceType, LocalSpaceType> GMRESSolverType;
        typedef Preconditioner<SpaceType,  LocalSpaceType> PreconditionerType;

        using namespace boost::python;

        //***************************************************************************
        //linear solvers
        //***************************************************************************
        class_<SuperLUSolverType, bases<DirectSolverType>, boost::noncopyable >
                ( "SuperLUSolver",
                  init<>() );

        class_<GMRESSolverType, bases<IterativeSolverType>, boost::noncopyable >
                ( "GMRESSolver")
                .def(init<double>())
                .def(init<double, unsigned int>())
                .def(init<double, unsigned int,  PreconditionerType::Pointer>())
                .def(self_ns::str(self))
                ;

  }

}  // namespace Python.

} // Namespace Kratos