#include <Eigen/Geometry>
#include <sm/boost/null_deleter.hpp>

namespace aslam {
namespace backend {

#define _TEMPLATE template <int IRows, int ICols, typename TScalar, typename TNode>
#define _CLASS GenericMatrixExpression<IRows, ICols, TScalar, TNode>

_TEMPLATE _CLASS::GenericMatrixExpression(node_ptr_t root) : _root(root) {}

_TEMPLATE
_CLASS::GenericMatrixExpression(node_t* root) : _root(root, sm::null_deleter()) {}

_TEMPLATE
template <typename DERIVED>
_CLASS::GenericMatrixExpression(const Eigen::MatrixBase<DERIVED>& mat) : _root(new typename node_t::constant_t(mat)) {}

_TEMPLATE
typename _CLASS::matrix_t _CLASS::toFullMatrix() const { return _root->evaluate(); }

_TEMPLATE
void _CLASS::evaluateJacobians(JacobianContainer& outJacobians) const {
    _root->evaluateJacobians(outJacobians, IdentityDifferential<tangent_vector_t, TScalar>());
}

_TEMPLATE
void _CLASS::evaluateJacobians(JacobianContainer& outJacobians, const Eigen::MatrixXd& applyChainRule) const {
    _root->evaluateJacobians(outJacobians, MatrixDifferential<TScalar, Eigen::Matrix<TScalar, Eigen::Dynamic, IRows> >(
                                               applyChainRule.cast<TScalar>()));
}

_TEMPLATE
void _CLASS::evaluateJacobians(JacobianContainer& outJacobians, const differential_t& diff) const {
    return _root->evaluateJacobians(outJacobians, diff);
}

_TEMPLATE
void _CLASS::getDesignVariables(DesignVariable::set_t& designVariables) const {
    return _root->getDesignVariables(designVariables);
}

_TEMPLATE
inline _CLASS::operator GenericMatrixExpression<IRows, ICols, TScalar>() const {
    typedef default_self_t result_t;

    class ResultNode : public result_t::template UnaryOperationResult<ResultNode, self_t> {
      public:
        typedef typename result_t::template UnaryOperationResult<ResultNode, self_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const { this->_currentValue = this->getOperandNode().evaluate(); }

        inline typename base_t::apply_diff_return_t applyDiff(
            const typename base_t::operand_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector;
        }
    };
    return ResultNode::create(*this);
}

_TEMPLATE GenericMatrixExpression<ICols, IRows, TScalar> _CLASS::transpose() const {
    typedef GenericMatrixExpression<ICols, IRows, TScalar> result_t;

    class ResultNode : public result_t::template UnaryOperationResult<ResultNode, self_t> {
      public:
        typedef typename result_t::template UnaryOperationResult<ResultNode, self_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getOperandNode().evaluate().transpose();
        }

        inline typename base_t::apply_diff_return_t applyDiff(
            const typename base_t::operand_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector.transpose();
        }
    };

    return ResultNode::create(*this);
}

_TEMPLATE GenericMatrixExpression<ICols, IRows, TScalar> _CLASS::inverse() const {
    typedef GenericMatrixExpression<ICols, IRows, TScalar> result_t;

    static_assert(ICols == IRows, "generalized inverse not yet supported.");

    class ResultNode : public result_t::template UnaryOperationResult<ResultNode, self_t> {
      public:
        typedef typename result_t::template UnaryOperationResult<ResultNode, self_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getOperandNode().evaluate().inverse();
        }

        inline typename base_t::apply_diff_return_t applyDiff(
            const typename base_t::operand_t::tangent_vector_t& tangent_vector) const {
            evaluateImplementation();
            return -this->_currentValue * tangent_vector * this->_currentValue;
        }
    };

    return ResultNode::create(*this);
}

_TEMPLATE template <int IColsOther, typename TOtherNode>
GenericMatrixExpression<IRows, IColsOther, TScalar> _CLASS::operator*(
    const GenericMatrixExpression<ICols, IColsOther, TScalar, TOtherNode>& other) const {
    typedef GenericMatrixExpression<IRows, IColsOther, TScalar> result_t;
    typedef GenericMatrixExpression<ICols, IColsOther, TScalar, TOtherNode> other_t;

    class ResultNode : public result_t::template BinaryOperationResult<ResultNode, self_t, other_t> {
      public:
        typedef typename result_t::template BinaryOperationResult<ResultNode, self_t, other_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getLhsNode().evaluate() * this->getRhsNode().evaluate();
        }

        inline typename base_t::apply_diff_return_t applyLhsDiff(
            const typename base_t::lhs_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector * this->getRhsNode().evaluate();
        }
        inline typename base_t::apply_diff_return_t applyRhsDiff(
            const typename base_t::rhs_t::tangent_vector_t& tangent_vector) const {
            return this->getLhsNode().evaluate() * tangent_vector;
        }
    };

    return ResultNode::create(*this, other);
}

_TEMPLATE template <typename TOtherNode>
typename _CLASS::default_self_t _CLASS::operator+(
    const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode>& other) const {
    typedef default_self_t result_t;
    typedef GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode> other_t;

    class ResultNode : public result_t::template BinaryOperationResult<ResultNode, self_t, other_t> {
      public:
        typedef typename result_t::template BinaryOperationResult<ResultNode, self_t, other_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getLhsNode().evaluate() + this->getRhsNode().evaluate();
        }

        inline typename base_t::apply_diff_return_t applyLhsDiff(
            const typename base_t::lhs_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector;
        }
        inline typename base_t::apply_diff_return_t applyRhsDiff(
            const typename base_t::rhs_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector;
        }
    };

    return ResultNode::create(*this, other);
}

_TEMPLATE
template <typename TOtherNode>
typename _CLASS::default_self_t _CLASS::operator-(
    const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode>& other) const {
    typedef default_self_t result_t;
    typedef GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode> other_t;

    class ResultNode : public result_t::template BinaryOperationResult<ResultNode, self_t, other_t> {
      public:
        typedef typename result_t::template BinaryOperationResult<ResultNode, self_t, other_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getLhsNode().evaluate() - this->getRhsNode().evaluate();
        }

        inline typename base_t::apply_diff_return_t applyLhsDiff(
            const typename base_t::lhs_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector;
        }
        inline typename base_t::apply_diff_return_t applyRhsDiff(
            const typename base_t::rhs_t::tangent_vector_t& tangent_vector) const {
            return -tangent_vector;
        }
    };

    return ResultNode::create(*this, other);
}

_TEMPLATE
typename _CLASS::default_self_t _CLASS::operator-() const {
    typedef default_self_t result_t;

    class ResultNode : public result_t::template UnaryOperationResult<ResultNode, self_t> {
      public:
        typedef typename result_t::template UnaryOperationResult<ResultNode, self_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const { this->_currentValue = -this->getOperandNode().evaluate(); }

        inline typename base_t::apply_diff_return_t applyDiff(
            const typename base_t::operand_t::tangent_vector_t& tangent_vector) const {
            return -tangent_vector;
        }
    };

    return ResultNode::create(*this);
}

_TEMPLATE
typename _CLASS::default_self_t _CLASS::operator*(TScalar scalar) const {
    typedef default_self_t result_t;

    class ResultNode : public result_t::template UnaryOperationResult<ResultNode, self_t> {
      public:
        typedef typename result_t::template UnaryOperationResult<ResultNode, self_t> base_t;

        ResultNode(TScalar scalar) : _scalar(scalar) {}
        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            this->_currentValue = this->getOperandNode().evaluate() * _scalar;
        }

        inline typename base_t::apply_diff_return_t applyDiff(
            const typename base_t::operand_t::tangent_vector_t& tangent_vector) const {
            return tangent_vector * _scalar;
        }

      private:
        TScalar _scalar;
    };

    return ResultNode::create(*this, scalar);
}

namespace internal {
template <int ILhsCols, int IRhsCols>
struct CrossProductCalculator {};

template <>
struct CrossProductCalculator<Eigen::Dynamic, Eigen::Dynamic> {
    template <typename TLhs, typename TRhs, typename TResult>
    inline static void calcCrossInto(const TLhs& lhs, const TRhs& rhs, TResult& result) {
        const int resultCols = result.cols();
        if (lhs.cols() == 1)
            for (int i = 0; i < resultCols; i++)
                result.template block<3, 1>(0, i) = lhs.cross(rhs.template block<3, 1>(0, i));
        else
            for (int i = 0; i < resultCols; i++)
                result.template block<3, 1>(0, i) = lhs.template block<3, 1>(0, i).cross(rhs);
    }
};
template <int ILhsCols>
struct CrossProductCalculator<ILhsCols, 1> {
    template <typename TLhs, typename TRhs, typename TResult>
    inline static void calcCrossInto(const TLhs& lhs, const TRhs& rhs, TResult& result) {
        const int resultCols = result.cols();
        for (int i = 0; i < resultCols; i++)
            result.template block<3, 1>(0, i) = lhs.template block<3, 1>(0, i).cross(rhs);
    }
};
template <int IRhsCols>
struct CrossProductCalculator<1, IRhsCols> {
    template <typename TLhs, typename TRhs, typename TResult>
    inline static void calcCrossInto(const TLhs& lhs, const TRhs& rhs, TResult& result) {
        const int resultCols = result.cols();
        for (int i = 0; i < resultCols; i++)
            result.template block<3, 1>(0, i) = lhs.cross(rhs.template block<3, 1>(0, i));
    }
};
template <>
struct CrossProductCalculator<1, 1> {
    template <typename TLhs, typename TRhs, typename TResult>
    inline static void calcCrossInto(const TLhs& lhs, const TRhs& rhs, TResult& result) {
        result = lhs.cross(rhs);
    }
};
}  // namespace internal

_TEMPLATE
template <int IColsOther, typename TOtherNode,
          typename std::enable_if<IRows == 3 && (ICols == 1 || IColsOther == 1), int>::type>
GenericMatrixExpression<3, (ICols == 1 ? IColsOther : ICols), TScalar> _CLASS::cross(
    const GenericMatrixExpression<3, IColsOther, TScalar, TOtherNode>& other) const {
    enum { RESULT_COLS = (ICols == 1 ? IColsOther : ICols) };
    typedef GenericMatrixExpression<3, RESULT_COLS, TScalar> result_t;
    typedef GenericMatrixExpression<3, IColsOther, TScalar, TOtherNode> other_t;
    typedef internal::CrossProductCalculator<ICols, IColsOther> calculator_t;

    class ResultNode : public result_t::template BinaryOperationResult<ResultNode, self_t, other_t> {
      public:
        typedef typename result_t::template BinaryOperationResult<ResultNode, self_t, other_t> base_t;

        virtual ~ResultNode() {}

        virtual void evaluateImplementation() const {
            calculator_t::calcCrossInto(this->getLhsNode().evaluate(), this->getRhsNode().evaluate(),
                                        this->_currentValue);
        }

        inline typename base_t::apply_diff_return_t applyLhsDiff(
            const typename base_t::lhs_t::tangent_vector_t& tangent_vector) const {
            auto rhs = this->getRhsNode().evaluate();
            typename base_t::apply_diff_return_t result(3, std::max(tangent_vector.cols(), rhs.cols()));
            calculator_t::calcCrossInto(tangent_vector, rhs, result);
            return result;
        }
        inline typename base_t::apply_diff_return_t applyRhsDiff(
            const typename base_t::rhs_t::tangent_vector_t& tangent_vector) const {
            auto lhs = this->getLhsNode().evaluate();
            typename base_t::apply_diff_return_t result(3, std::max(lhs.cols(), tangent_vector.cols()));
            calculator_t::calcCrossInto(lhs, tangent_vector, result);
            return result;
        }
    };

    return ResultNode::create(*this, other);
}
#undef _TEMPLATE
#undef _CLASS

}  // namespace backend
}  // namespace aslam


/**
 * Copyright (c) 2011, 2012
 * Claudio Kopper <claudio.kopper@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id: I3CLSimRandomValueApplyFunction.cxx 108199 2013-07-12 21:33:08Z nwhitehorn $
 *
 * @file I3CLSimRandomValueApplyFunction.cxx
 * @version $Revision: 108199 $
 * @date $Date: 2013-07-12 15:33:08 -0600 (Fri, 12 Jul 2013) $
 * @author Claudio Kopper
 */

#include <icetray/serialization.h>
#include <icetray/I3Logging.h>
#include <clsim/random_value/I3CLSimRandomValueApplyFunction.h>

#include <boost/lexical_cast.hpp>

#include <string>
#include <sstream>
#include <vector>
#include <cmath>
#include <limits>

I3CLSimRandomValueApplyFunction::
I3CLSimRandomValueApplyFunction(const I3CLSimRandomValuePtr &randomDistUsed,
                                const std::string &applyFunctionName)
:
randomDistUsed_(randomDistUsed),
applyFunctionName_(applyFunctionName)
{
    if (!randomDistUsed_) {
        log_fatal("The \"randomDistUsed\" parameter must not be NULL!");
    }

    if (applyFunctionName_=="") {
        log_fatal("The \"applyFunctionName\" parameter must be set!");
    }
}

I3CLSimRandomValueApplyFunction::~I3CLSimRandomValueApplyFunction()
{
}

I3CLSimRandomValueApplyFunction::I3CLSimRandomValueApplyFunction() {;}

std::size_t I3CLSimRandomValueApplyFunction::NumberOfParameters() const
{
    return randomDistUsed_->NumberOfParameters();
}

double I3CLSimRandomValueApplyFunction::SampleFromDistribution(const I3RandomServicePtr &random,
                                                               const std::vector<double> &parameters) const
{
    if (!random) log_fatal("random service is NULL!");

    const double sampledValue = randomDistUsed_->SampleFromDistribution(random, parameters);

    if      (applyFunctionName_=="cos")  return std::cos (sampledValue);
    else if (applyFunctionName_=="sin")  return std::sin (sampledValue);
    else if (applyFunctionName_=="tan")  return std::tan (sampledValue);
    else if (applyFunctionName_=="acos") return std::acos(sampledValue);
    else if (applyFunctionName_=="asin") return std::asin(sampledValue);
    else if (applyFunctionName_=="atan") return std::atan(sampledValue);
    else if (applyFunctionName_=="exp")  return std::exp (sampledValue);
    else {
        log_fatal("The function \"%s\" is currently not implemented in host code.", applyFunctionName_.c_str());
        return NAN;
    }
}

std::string I3CLSimRandomValueApplyFunction::GetOpenCLFunction
(const std::string &functionName,
 const std::string &functionArgs,
 const std::string &functionArgsToCall,
 const std::string &uniformRandomCall_co,
 const std::string &uniformRandomCall_oc
 ) const
{
    // some simple substitutions
    std::string applyFunctionNameNativeMath;
    if (applyFunctionName_=="cos") applyFunctionNameNativeMath="native_cos";
    else if (applyFunctionName_=="sin") applyFunctionNameNativeMath="native_sin";
    else if (applyFunctionName_=="tan") applyFunctionNameNativeMath="native_tan";
    else if (applyFunctionName_=="acos") applyFunctionNameNativeMath="native_acos";
    else if (applyFunctionName_=="asin") applyFunctionNameNativeMath="native_asin";
    else if (applyFunctionName_=="atan") applyFunctionNameNativeMath="native_atan";
    else if (applyFunctionName_=="exp") applyFunctionNameNativeMath="native_exp";
    else applyFunctionNameNativeMath=applyFunctionName_;

    std::string parametersInDecl, parametersToCall;
    for (std::size_t i=0;i<randomDistUsed_->NumberOfParameters();++i)
    {
        parametersInDecl += ", float p" + boost::lexical_cast<std::string>(i);
        parametersToCall += ", p" + boost::lexical_cast<std::string>(i);
    }

    const std::string functionDecl = std::string("inline float ") + functionName + "(" + functionArgs + parametersInDecl +")";
    const std::string functionNameUsed = std::string("") + functionName + "_used";

    const std::string usedFunctionDecl =
    randomDistUsed_->GetOpenCLFunction
    (
     functionNameUsed,
     functionArgs,
     functionArgsToCall,
     uniformRandomCall_co,
     uniformRandomCall_oc
    );

    return usedFunctionDecl + "\n\n" + functionDecl + ";\n\n" + functionDecl + "\n"
    "{\n"
    "    const float number = " + functionNameUsed + "(" + functionArgsToCall + parametersToCall + ");\n"
    "    \n"
    "#ifdef USE_NATIVE_MATH\n"
    "    return " + applyFunctionNameNativeMath + "(number);\n"
    "#else\n"
    "    return " + applyFunctionName_ + "(number);\n"
    "#endif\n"
    "}\n"
    ;
}

bool I3CLSimRandomValueApplyFunction::CompareTo(const I3CLSimRandomValue &other) const
{
    try
    {
        const I3CLSimRandomValueApplyFunction &other_ = dynamic_cast<const I3CLSimRandomValueApplyFunction &>(other);

        if (other_.applyFunctionName_ != applyFunctionName_) return false;
        if (!(*(other_.randomDistUsed_) == *randomDistUsed_)) return false;

        return true;
    }
    catch (const std::bad_cast& e)
    {
        // not of the same type, treat it as non-equal
        return false;
    }

}

template <class Archive>
void I3CLSimRandomValueApplyFunction::serialize(Archive &ar, unsigned version)
{
    if (version>i3clsimrandomvalueapplyfunction_version_)
        log_fatal("Attempting to read version %u from file but running version %u of I3CLSimRandomValueApplyFunction class.",
                  version,
                  i3clsimrandomvalueapplyfunction_version_);

    ar & make_nvp("I3CLSimRandomValue", base_object<I3CLSimRandomValue>(*this));
    ar & make_nvp("applyFunctionName", applyFunctionName_);
    ar & make_nvp("randomDistUsed", randomDistUsed_);
}

I3_SERIALIZABLE(I3CLSimRandomValueApplyFunction);


#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <unistd.h>
#include <map>
#include <list>

#include <Eigen/Eigenvalues>

// OpenCV headers
#include <cv.h>
#include <highgui.h>

#include <opencv2/contrib/contrib.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <opencv_workbench/syllo/syllo.h>

#include <opencv_workbench/utils/AnnotationParser.h>
#include <opencv_workbench/utils/ColorMaps.h>
#include <opencv_workbench/plot/Plot.h>
#include <opencv_workbench/track/Dynamics.h>
#include <opencv_workbench/track/KalmanFilter.h>
#include <opencv_workbench/track/EKF.h>
#include <opencv_workbench/wb/Entity.h>
#include <opencv_workbench/wb/Blob.h>
#include <opencv_workbench/wb/BlobProcess.h>
#include <opencv_workbench/trajectory/TrajectoryAnalysis.h>
#include <opencv_workbench/utils/OpenCV_Helpers.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

#include <yaml-cpp/yaml.h>

using std::cout;
using std::endl;

#define PI 3.14159265359

cv::Point2d rotate_2D(cv::Point2d v, double rad)
{
     cv::Point2d ans;
     ans.x = v.x*cos(rad) - v.y*sin(rad);
     ans.y = v.x*sin(rad) + v.y*cos(rad);
     return ans;
}

// our data types
struct Vec3_Yaml {
     double x, y, z;
};

struct Vec2_Yaml {
     double x, y;
};

void operator >> (const YAML::Node& node, Vec2_Yaml& v)
{
     node[0] >> v.x;
     node[1] >> v.y;
}

void operator >> (const YAML::Node& node, Vec3_Yaml& v)
{
     node[0] >> v.x;
     node[1] >> v.y;
     node[2] >> v.z;
}

double sonar_noise = 0.0;
double sonar_range = 20.0;

void operator >> (const YAML::Node& node, Dynamics &dyn) {
     std::string str_temp;
     node["type"] >> str_temp;
     dyn.set_type(str_temp);

     int id;
     node["id"] >> id;
     dyn.set_id(id);

     Vec3_Yaml vec3;
     node["position"] >> vec3;
     double heading;
     node["heading"] >> heading;

     Dynamics::state_5d_type x0;
     x0[0] = vec3.x;
     x0[1] = vec3.y;
     x0[2] = heading * PI / 180.0;
     x0[3] = 0;
     x0[4] = 0;
     dyn.set_x0(x0);

     str_temp = "";
     node["motion"] >> str_temp;
     if (str_temp == "roomba") {
          dyn.set_model(Dynamics::roomba);
     } else {
          dyn.set_model(Dynamics::cart);
     }

     if(const YAML::Node *pInputs = node.FindValue("input_sequence")) {
          for(unsigned int i = 0; i < pInputs->size(); i++) {
               Vec2_Yaml vec2;
               (*pInputs)[i]["control_input"] >> vec2;
               Dynamics::state_5d_type input;
               input[0] = vec2.x;
               input[1] = vec2.y;

               double time;
               (*pInputs)[i]["time"] >> time;

               dyn.set_input(input,time);
          }
     } else {
          Vec2_Yaml vec2;
          node["control_input"] >> vec2;
          Dynamics::state_5d_type input;
          input[0] = vec2.x;
          input[1] = vec2.y;
          dyn.set_input(input);
     }

     if(const YAML::Node *pName = node.FindValue("sonar_noise")) {
          *pName >> sonar_noise;
     }

     if(const YAML::Node *pName = node.FindValue("sonar_range")) {
          *pName >> sonar_range;
     }
}

void draw_sonar_lines(cv::Mat &src, cv::Mat &dst, double beam_width)
{
     double img_height = src.cols;
     double img_width = src.rows;

     dst = src.clone();
     /////////////////////////////////////////////////////////////////////
     // Draw the sonar's outside polygon
     double start_angle = (PI - beam_width) / 2 ;
     double end_angle = start_angle + beam_width;// * PI / 180.0;//90.0 * PI / 180.0;//1.963495408;
     cv::Point start(img_width/2,img_height);
     cv::Point end_1(start.x+img_height*cos(start_angle), start.y-img_height*sin(start_angle));
     cv::Point end_2(start.x+img_height*cos(end_angle), start.y-img_height*sin(end_angle));

     // Line from sonar head to start of outer-arc
     cv::line(dst, start, end_1, cv::Scalar(0,0,0), 1, 8, 0);
     cv::Point prev = end_1;

     // Draw outer-arc polygon
     double num_pts = 25;
     double step = (end_angle - start_angle) / num_pts;
     for (double ang = start_angle; ang <= end_angle; ang += step) {
          //contour.push_back(cv::Point(start.x+img_height*cos(ang), start.y+img_height*sin(ang)));
          cv::Point p(cv::Point(start.x+img_height*cos(ang), start.y-img_height*sin(ang)));
          cv::line(dst, prev, p, cv::Scalar(0,0,0), 1, 8, 0);
          prev = p;
     }

     // Line from outer-arc to real final arc position
     cv::line(dst, prev, end_2, cv::Scalar(0,0,0), 1, 8, 0);

     // Line from outer-arc back to sonar head
     cv::line(dst, end_2, start, cv::Scalar(0,0,0), 1, 8, 0);
}

#define USE_HUNGARIAN 0
#define USE_MHT 1
#define USE_SINGLE 0

int main(int argc, char *argv[])
{
     cout << "================================" << endl;
     cout << "  Simple Sonar Simulator" << endl;
     cout << "================================" << endl;

     if (argc < 2) {
          cout << "usage: " << argv[0] << " yaml-file" << endl;
          return -1;
     }

     double t0 = 0;
     double dt;
     double tend;
     double sample_period;

     std::ifstream fin(argv[1]);
     YAML::Parser parser(fin);
     YAML::Node doc;
     parser.GetNextDocument(doc);

     // Get time information:
     doc["time_step"] >> dt;
     doc["time_end"] >> tend;

     if(const YAML::Node *psampleperiod = doc.FindValue("sensor_sample_period")) {
          *psampleperiod >> sample_period;
     } else {
          sample_period = dt;
     }

     int sample_period_count = cvRound(sample_period / dt);

     double traj_threshold;
     doc["traj_threshold"] >> traj_threshold;

     bool display_plot;
     doc["display_plot"] >> display_plot;

     // Get entity information:
     std::vector<Dynamics> dyns;
     const YAML::Node& entities = doc["entities"];
     for(unsigned int i = 0; i < entities.size(); i++) {
          Dynamics dyn;
          entities[i] >> dyn;
          dyns.push_back(dyn);
     }

     // Get reference to the Sonar sensor
     Dynamics *sonar = NULL;
     for (std::vector<Dynamics>::iterator it = dyns.begin(); it != dyns.end();
          it++) {
          it->set_time(0, dt, tend);
          it->set_process_noise(0.0);
          it->set_measurement_noise(sonar_noise);

          if (it->type() == "sensor") {
               sonar = &(*it);
          }
     }
     if (sonar == NULL) {
          cout << "sensor not found in yaml file." << endl;
          return -1;
     }

     // Plot vectors
     std::map< std::string, std::vector<cv::Point3d> > vectors;
     const std::string title = "Tracks";
     std::vector<std::string> labels;
     std::vector<std::string> styles;

     // Setup plot and basic options
     syllo::Plot plot;
     std::string options;
     options = "set size ratio -1\n";
     options += "set view equal xy\n";
     options += "set size 1,1\n";

     // Sonar Image Setup
     double max_angle_ = 0.392699082;
     double min_angle_ = -0.392699082;
     double x_min = 0.00;  // R_min
     double x_max = sonar_range; // R_max
     double y_max = sin(max_angle_)*x_max*2;
     double beam_width_ = max_angle_ * 2;

     int img_width = 600;
     int img_height = 600;
     cv::Mat frame_img = cv::Mat::zeros(img_height, img_width, CV_8UC3);
     frame_img = cv::Scalar(255,255,255);

     cv::Mat history_img = cv::Mat::zeros(img_height, img_width, CV_8UC3);
     history_img = cv::Scalar(255,255,255);

     // Setup rotation matrix
     cv::Point center = cv::Point( frame_img.cols/2, frame_img.rows/2 );
     double angle = 180.0;
     double rot_scale = 1.0;
     cv::Mat rot_mat(2,3,CV_8UC1);
     rot_mat = cv::getRotationMatrix2D( center, angle, rot_scale );

     TrajectoryAnalysis traj;
     traj.set_simulated(true);
     std::map<int, std::list<wb::Blob> > tracks_history;

     BlobProcess blob_process;
     std::vector<wb::Blob> tracks;
     // Loop through all generated trajectories
     bool step_flag = true;
     int loop_number = 0;
     int frame_number = 0;
     bool exit_sim = false;
     std::vector<double> tt = Dynamics::time_vector(t0,dt,tend);
     for (std::vector<double>::iterator it = tt.begin();
          it != tt.end() && !exit_sim; it++, loop_number++) {

          bool scan_sample = loop_number % sample_period_count == 0 ? true : false;

          std::map<int, wb::Blob> tracks_frame;

          //frame_img = cv::Scalar(255,255,255);

          // Clear out the plot vectors
          //vectors.clear() ; labels.clear(); styles.clear();

          Dynamics::state_5d_type sonar_state = sonar->state();
          cv::Point2d sonar_pos(sonar_state[0], sonar_state[1]);
          double sonar_heading = sonar_state[2];

          std::vector<wb::Blob> measurements;

          // Run Kalman Filter on all tracks
          for(std::vector<wb::Blob>::iterator it_tracks = tracks.begin();
              it_tracks != tracks.end(); it_tracks++) {
               if (it_tracks->is_confirmed()) {
                    it_tracks->predict_tracker();
               }
          }

          // Create measurements from sensor's perspective
          for (std::vector<Dynamics>::iterator it_ent = dyns.begin();
               it_ent != dyns.end(); it_ent++) {

               // Grab the current state and then step the model
               Dynamics::state_5d_type measurement = it_ent->measurement();

               cv::Point2d pos(measurement[0], measurement[1]);
               cv::Point3d pos_3d(measurement[0], measurement[1], 0);

               vectors[syllo::int2str(it_ent->id())].push_back(pos_3d);

               if (it_ent->type() == "sensor") {
                    // Skip if this is the sensor
                    continue;
               }

               if (scan_sample) {
                    /////////////////////////////////////////////////
                    // Can the sonar see the object at this point?
                    // Within range and bearing limits?
                    // Compute bearing from sonar to target
                    // Position of contact, relative to ownship
                    cv::Point2d offset = pos - sonar_pos;
                    cv::Point2d cnt_relative_pos = rotate_2D(offset, -sonar_heading);
                    double bearing_to_cnt = atan2(cnt_relative_pos.y,
                                                  cnt_relative_pos.x);

                    double dist = cv::norm(offset);

                    if ((min_angle_ <= bearing_to_cnt) && (bearing_to_cnt <= max_angle_) &&
                        (dist >= x_min) && dist <= x_max) {
                         // Put target in sonar's image

                         //cout << "Bearing: " << bearing_to_cnt * 180 / PI;

                         // convert offset to polar coordinates
                         double R = dist;
                         double theta = bearing_to_cnt;

                         // x = R*cos(theta), y = R*sin(theta);
                         double x = R*cos(theta);
                         double y = R*sin(theta);

                         int x_pos = cvRound(img_width/2 - y / (y_max/2) * img_height*sin(max_angle_));
                         int y_pos = cvRound(img_height - x / x_max * img_height);

                         if (x_pos < 0 || x_pos > frame_img.cols || y_pos < 0 || y_pos > frame_img.rows) {
                              // error, skip
                              cout << "bounds" << endl;
                              continue;
                         }

                         cv::Point blob_pos(x_pos, y_pos);

                         wb::Blob blob;
                         blob.set_id(it_ent->id());
                         blob.set_centroid(cv::Point2d(x,y));
                         wb::Point p;
                         p.set_position(blob_pos);
                         p.set_value(255);
                         blob.add_point(p);
                         blob.set_frame(frame_number);
                         blob.init();

                         tracks_history[it_ent->id()].push_back(blob);
                         tracks_frame[it_ent->id()] = blob;

                         measurements.push_back(blob);

                         wb::drawTriangle(frame_img, blob_pos, cv::Scalar(0,0,0), 10);

                         cv::circle(frame_img,blob_pos,1,cv::Scalar(0,0,0),-1,8,0);
                         cv::circle(history_img,blob_pos,1,cv::Scalar(0,0,0),-1,8,0);

                         std::ostringstream convert;
                         convert << it_ent->id();
                         const std::string& text = convert.str();
                         cv::putText(frame_img, text, cv::Point(x_pos+5, y_pos-5), cv::FONT_HERSHEY_DUPLEX, 0.75, cv::Scalar(0,0,0), 1, 8, false);
                    }

                    // Draw sonar lines on images
                    draw_sonar_lines(frame_img, frame_img, beam_width_);
                    draw_sonar_lines(history_img, history_img, beam_width_);

#if USE_SINGLE
                    // Assume single track for now
                    if (tracks.size() == 0) {
                         tracks = measurements;
                         tracks.back().set_age(6); // make it tracked
                    } else {
                         Eigen::MatrixXf Zm;
                         Zm.resize(2,1);
                         Zm << measurements.back().pixel_centroid().x, measurements.back().pixel_centroid().y;

                         if(tracks.back().tracker().is_within_region(Zm,0.99)) {
                              cout << "In region!" << endl;
                         } else {
                              cout << "Out of region!" << endl;
                         }

                         measurements.back().matched_track(tracks.back());
                         tracks = measurements;
                    }
#elif USE_HUNGARIAN
                    // Send "measurements" to hungarian assignment algorithm
                    std::vector<wb::Blob> fused_tracks;
                    blob_process.assign_hungarian(measurements, tracks, fused_tracks);
                    tracks = fused_tracks;
#elif USE_MHT
                    // Send "measurements" to MHT
                    std::vector<wb::Blob> fused_tracks;
                    blob_process.assign_mht(measurements, tracks, fused_tracks);
                    tracks = fused_tracks;
#endif
               } // if scan_sample
               blob_process.set_blobs(tracks);

               // Draw Tracks on image
               cv::Mat tracks_img(frame_img.size(), CV_8UC3);
               tracks_img = cv::Scalar(255,0,0);
               blob_process.overlay(tracks_img, tracks_img, TRACKS | ERR_ELLIPSE);
               cv::imshow("Tracks", tracks_img);

               //// Compute Trajectory Analysis
               //cv::Mat traj_img = frame_img;//img_color;//.clone();
               //traj.trajectory_similarity(tracks_history, frame_number,
               //                           traj_img, traj_threshold);
               //
               //cv::Mat history_traj_img = history_img;//img_color;//.clone();
               //traj.trajectory_similarity(tracks_history, frame_number,
               //                           history_traj_img, traj_threshold);
               //cv::imshow("History", history_traj_img);
               //cv::imshow("Frame", traj_img);

               cv::Mat traj_img = frame_img;
               traj.trajectory_similarity_track_diff(tracks_frame, frame_number,
                                                     traj_img, traj_threshold);

               cv::imshow("Frame", traj_img);

               /////////////////////////////////////////////////////////////////////

               // Handle pausing / stepping
               if (step_flag) {
                    int key = cv::waitKey(0);
                    if (key == 'q' || key == 1048689) { // 'q' key
                         cout << "Ending early." << endl;
                         exit_sim = true;
                    } else if (key == 'p' || key == 1048688) {
                         step_flag = 0;
                    }
               } else {
                    int key = cv::waitKey(30);
                    if (key == 'q' || key == 1048689) {
                         cout << "Ending early." << endl;
                         exit_sim = true;
                    } else if (key == 'p' || key == 1048688) {
                         step_flag = 1;
                    }
               }
          }

          // Generate Trajectories for all objects that have dynamics
          for (std::vector<Dynamics>::iterator it_ent = dyns.begin();
               it_ent != dyns.end(); it_ent++) {
               // Grab the current state and then step the model
               it_ent->step_motion_model(dt, *it);
          }

          if (display_plot) {
               // Plot the tracks
               std::vector<std::string> empty_objects;
               plot.plot(vectors, title, labels, styles, options, empty_objects, false);
          }
          frame_number++;
     }
     cv::waitKey(0);
     return 0;
}


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <vector>
#include "kmers/multinomial-model.hpp"

template <typename T>
T read_val(std::istream& in) {
  T x;
  in.read(reinterpret_cast<char*>(&x), sizeof(T));
  return x;
}
template <typename T>
std::vector<T> read_array(std::istream& in, int32_t N) {
  std::vector<T> y(N);
  for (int32_t n = 0; n < N; ++n)
    y[n] = read_val<T>(in);
  return y;
}

Eigen::Map<Eigen::SparseMatrix<float, Eigen::RowMajor>>
read_xt(const std::string& filename) {
  std::fstream in(filename, std::ios::binary | std::ios::in);
  if (!in) {
    std::string msg = "reader: couldn't open file = " + filename;
    throw std::runtime_error(msg);
  }
  int32_t rows = read_val<int>(in);
  std::cout << "reader:  rows = " << rows
            << std::endl;

  int32_t cols = read_val<int>(in);
  std::cout << "reader:  cols = " << cols
            << std::endl;

  int32_t nnz = read_val<int>(in);
  std::cout << "reader:  nnz = " << nnz
            << std::endl;

  std::vector<int> outerIndexPtr = read_array<int>(in, rows + 1);
  std::cout << "reader:  read outerIndexPtr"
            << std::endl;

  std::vector<int> innerIndices = read_array<int>(in, nnz);
  std::cout << "reader:  read innerIndices"
            << std::endl;

  std::vector<float> values = read_array<float>(in, nnz);
  std::cout << "reader: read innerIndices"
            << std::endl;

  in.close();
  std::cout << "reader:  finished reading, closed file stream"
            << std::endl;

  std::cout << "reader:  building Eigen::Map"
            << std::endl;

  return Eigen::Map<Eigen::SparseMatrix<float, Eigen::RowMajor>>(
      rows, cols, nnz, outerIndexPtr.data(), innerIndices.data(),
      values.data());
}

int main(int argc, char* argv[]) {
  std::string filename = argv[1];
  std::cout << "reader: reading from filename = " << filename
            << std::endl;

  Eigen::Map<Eigen::SparseMatrix<float, Eigen::RowMajor>> xt
      = read_xt(filename);

  std::cout << "reader:  built matrix"
            << std::endl;
  std::cout << "reader:  xt.rows() = " << xt.rows()
            << std::endl;
  std::cout << "reader:  xt.cols() = " << xt.cols()
            << std::endl;
  std::cout << "reader:  xt.nonZeros() = " << xt.nonZeros()
            << std::endl;

  std::cout << "reader:  building multinomial model"
            << std::endl;

  Eigen::VectorXf y = Eigen::VectorXf::Ones(xt.cols());
  std::cout << "reader: y.rows() = " << y.rows()
            << std::endl;
  multinomial_model model(xt, y);
  std::cout << "reader: model.y_.size() = " << model.y_.size()
            << std::endl;

  Eigen::VectorXf beta = Eigen::VectorXf::Ones(xt.rows());
  float lp = model.log_density(beta);
  std::cout << "lp = " << lp << std::endl;
}


/*
ID: neo-white
LANG: C++
TASK: Kmeans_lloyd.cpp
*/

#include "Kmeans_lloyd.h"
#include "../Core/Core.h"
#include <boost/serialization/vector.hpp>
#include <boost/serialization/queue.hpp>
#include <boost/serialization/export.hpp>
#include <vector>
#include <queue>
#include <fstream>
#include <time.h>

BOOST_CLASS_EXPORT(Kmeans)
MLalgorithm* Kmeans::operator +(MLalgorithm &rhs){
    MLalgorithm *pret = new Kmeans();
    pret->Init(this->nrow, this->ncol);
    for(size_t i = 0; i< data.size() ; ++i)
        pret->data[i] = data[i] + rhs.data[i];
    return pret;
}

std::vector<int> Kmeans_lloyd::init_random(size_t length, size_t random_seed)
{
    int tmp;
    std::vector<int> queue;
    for(size_t i=0;i<length;i++)
        queue.push_back(0);

    srand((unsigned)time(NULL));
    for(size_t j=0;j<random_seed;j++)
    {
        while(1)
        {
            tmp = (rand()%length);
            if(queue[tmp] == 0)
            {
                queue[tmp] = 1;
                break;
            }
        }
    }
    return queue;
}
void Kmeans_lloyd::beginDataScan(Flexible_vector *records, size_t feat_dim)
{
    size_t n_records = records->prob.l;
    size_t n_cluster = this->n_cluster;
    std::vector<int> q = init_random(n_records,n_cluster);
    for (size_t i = 0; i < n_records; ++ i){
        int x = Random(10007);
        weight.push_back(x);
        if (q.size() < n_cluster){
            q.push(i);
        }else if (x < weight[q.top()]){
            q.pop();
            q.push(i);
        }
    }
    while(!q.empty()){
        std::vector<double> record;
        record.clear();
        size_t i = q.top();
        for(size_t j = 0; j < feat_dim ; ++j){
            record.push_back(records[i*feat_dim+j]);
        }
        add(record, n_cluster - q.size());
        q.pop();
    }
}

void Kmeans::endDataScan(){
    for(size_t i = 0; i < nrow; ++i){
        for(size_t j = 0; j < ncol; ++j){
            data[i*ncol+j] /= data[i*ncol + ncol-1];
        }
        data[i*ncol + ncol-1] = 1;
    }

    //for(size_t i = 0; i < nrow; ++i){
    //for(size_t j = 0; j < ncol; ++j){
    //std::cout << data[i*ncol+j] << " ";
    //}
    //std::puts("");
    //}
}

MLalgorithm* Kmeans::processRecord(std::vector<double> records, size_t feat_dim){
    MLalgorithm* pret = new Kmeans();
    pret->Init(this->nrow, this->ncol);
    size_t n_records = records.size()/feat_dim;
    for (size_t i = 0 ; i < n_records ; i++){
        std::vector<double> record;
        record.clear();
        for(size_t j = 0; j < feat_dim ; j ++){
            record.push_back(records[i*feat_dim+j]);
        }
        double dis = (1L<<16)-1;
        int who = -1;
        //std::cout<<"bp"<<std::endl;
        for(size_t k = 0; k < this->nrow; k++){
            std::vector<double> centroid = this->get_value(k);
            double tmp = 0;
            for (size_t u = 0; u < record.size(); ++u){
                tmp += (centroid[u] - record[u])*(centroid[u] - record[u]);
            }

            if (tmp < dis){
                who = k;
                dis = tmp;
            }
        }
        record.push_back(1);
        pret->add(record, who);
    }
    return pret;
}

bool Kmeans::isConverged(MLalgorithm* rhs, size_t feat_dim, double eps){
    double diff = 0;
    for(size_t i = 0; i < nrow; i ++){
        double tmp = 0;
        for(size_t j = 0;  j < feat_dim; j++){
            tmp += (data[i*ncol+j] - rhs->data[i*ncol+j]) * (data[i*ncol+j] - rhs->data[i*ncol+j]);
        }
        tmp = sqrt(tmp);
        diff += tmp;
    }
    if (diff <= eps) return true;
    else return false;

}

void Kmeans::finish(std::vector<double> records, size_t feat_dim){
    std::ofstream file;
    file.open("./ans.txt");
    std::vector<float> record;
    size_t n_records = records.size()/feat_dim;
    for(size_t i = 0; i < n_records; ++i){
        std::vector<double> record;
        record.clear();
        for(size_t j = 0; j < feat_dim; ++ j){
            record.push_back(records[i*feat_dim+j]);
        }
        double dis = (1<<16)-1;
        int who = -1;
        for(size_t k = 0; k < this->nrow; k++){
            std::vector<double> centroid = this->get_value(k);
            double tmp = 0;
            for (size_t u = 0; u < record.size(); ++u){
                tmp += (centroid[u] - record[u])*(centroid[u] - record[u]);
            }

            if (tmp < dis){
                who = k;
                dis = tmp;
            }
        }
        file << who << std::endl;

    }

}


// This program causes/caused some versions of egcs to crash.

#include <blitz/benchext.h>
#include <random/uniform.h>
#include <blitz/array.h>

#ifdef BZ_HAVE_COMPLEX

void initializeRandomDouble(double* data, int numElements);

BZ_USING_NAMESPACE(blitz)

int QCDBlitzVersion(BenchmarkExt<int>& bench);
int QCDBlitzTunedVersion(BenchmarkExt<int>& bench);

int dontActuallyRunBenchmark()
{
    return 1;
}

int main()
{
    if (dontActuallyRunBenchmark())
        return 0;

    cout << "Blitz++ QCD Benchmark" << endl
         << "Working... (this may take a while)  ";
    cout.flush();

    BenchmarkExt<int> bench("Lattice QCD Benchmark", 2);

    //bench.setRateDescription("Millions of operations/s"); \todo removed for now
    bench.beginBenchmarking();

    QCDBlitzVersion(bench);
    QCDBlitzTunedVersion(bench);

    bench.endBenchmarking();

    bench.saveMatlabGraph("qcd.m");

    cout << "Done." << endl;

    return 0;
}

int QCDBlitzVersion(BenchmarkExt<int>& bench)
{
    typedef TinyMatrix<complex<double>, 3, 2> spinor;
    typedef TinyMatrix<complex<double>, 3, 3> SU3Gauge;

    bench.beginImplementation("Blitz++");

    while (!bench.doneImplementationBenchmark())
    {
        int length = bench.getParameter();
        int iters = (int)bench.getIterations();

        Array<spinor,1> res(length), src(length);
        Array<SU3Gauge,1> M(length);

        initializeRandomDouble((double*)src.data(),
            length * sizeof(spinor) / sizeof(double));
        initializeRandomDouble((double*)M.data(),
            length * sizeof(SU3Gauge) / sizeof(double));

        bench.start();
        long i;
        for (i=0; i < iters; ++i)
        {
            for (int i=0; i < length; ++i)
	      res(i) = M(i)*src(i);
        }
        bench.stop();

        // Time overhead
        bench.startOverhead();
        for (i=0; i < iters; ++i)
        {
        }
        bench.stopOverhead();
    }

    bench.endImplementation();

    return 0;
}

    typedef TinyMatrix<complex<double>, 3, 2> spinor;
    typedef TinyMatrix<complex<double>, 3, 3> gaugeFieldElement;

    struct latticeUnit {
        spinor one;
        gaugeFieldElement gauge;
        spinor two;
    };

int QCDBlitzTunedVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ (tuned)");

    while (!bench.doneImplementationBenchmark())
    {
        int length = bench.getParameter();
        int iters = (int)bench.getIterations();

        Array<latticeUnit,1> lattice(length);

        initializeRandomDouble((double*)lattice.data(),
            length * sizeof(latticeUnit) / sizeof(double));

        bench.start();
        long i;
        for (i=0; i < iters; ++i)
        {
            for (int i=0; i < length; ++i)
	      lattice(i).two = lattice(i).gauge * lattice(i).two;
        }
        bench.stop();

        // Time overhead
        bench.startOverhead();
        for (i=0; i < iters; ++i)
        {
        }
        bench.stopOverhead();
    }

    bench.endImplementation();

    return 0;
}

void initializeRandomDouble(double* data, int numElements)
{
    // This is a temporary kludge until I implement random complex
    // numbers.

  ranlib::Uniform<double> rnd;

    for (int i=0; i < numElements; ++i)
        data[i] = rnd.random();
}

#else // BZ_HAVE_COMPLEX

#include <blitz/blitz.h>

int main()
{
    cout << "This benchmark requires <complex> from the ISO/ANSI C++ standard."
         << endl;
    return 0;
}

#endif // BZ_HAVE_COMPLEX


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012        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_UNIT_DETAILS_ULP_HPP_INCLUDED
#define NT2_SDK_UNIT_DETAILS_ULP_HPP_INCLUDED

#include <nt2/sdk/unit/config.hpp>
#include <nt2/sdk/unit/details/is_sequence.hpp>
#include <nt2/sdk/unit/details/smallest_type.hpp>
#include <nt2/sdk/unit/details/eval.hpp>
#include <nt2/sdk/unit/stats.hpp>
#include <nt2/sdk/meta/type_id.hpp>

#include <boost/dispatch/attributes.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/fusion/include/is_sequence.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/current_function.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/foreach.hpp>
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <vector>

namespace nt2 { namespace details
{
  /// Hold which value was faulty and by how much
  template<class A, class B> struct failed_value
  {
    A           value;
    B           desired_value;
    double      ulp_error;
    std::size_t index;
  };

  // NaN aware max
  template<typename T>
  T safe_max(T a, T b)
  {
    return a < b ? b : ( (b != b) ? b : a );
  }

  /// ULP test is a success
  NT2_TEST_UNIT_DECL void ulp_pass(const char* desc, double ulpd, double N);

  /// ULP test is a failure
  NT2_TEST_UNIT_DECL
  void ulp_fail ( const char* desc, const char* f, int line
                , std::size_t size, double N, bool ok
                );

  /// Precompiled implementation max_ulps on double
  NT2_TEST_UNIT_DECL double max_ulps(double a, double b);

  /// Precompiled implementation max_ulps on float
  NT2_TEST_UNIT_DECL double max_ulps(float  a, float  b);

  BOOST_FORCEINLINE double max_ulps(bool a, bool b)
  {
    return a==b ? 0 : 1;
  }

  template<class T> BOOST_FORCEINLINE
  double max_ulps(T const& a, T const& b, boost::mpl::true_ const&)
  {
    typedef typename boost::dispatch::meta::as_unsigned<T>::type u_t;
    return (a<b) ? u_t(b-a) : u_t(a-b);
  }

  template<class T> BOOST_FORCEINLINE
  double max_ulps(T const& a, T const& b, boost::mpl::false_ const&)
  {
    return max_ulps(a.value(), b.value() );
  }

  /// Default implementation of max_ulps forward to generic ulpdist
  template<class T>
  BOOST_FORCEINLINE double max_ulps(T const& a, T const& b)
  {
    return max_ulps(a,b, boost::mpl::bool_<boost::is_integral<T>::value>() );
  }

  struct max_ulps_caller
  {
    typedef double result_type;
    template<class T>
    BOOST_FORCEINLINE double operator()(T const& a, T const& b) const
    {
      return max_ulps(a, b);
    }
  };

  /// Perform a test of equality on A and B with a given ulp tolerance
  /// Detects if A and/or B is actually a Fusion sequence or not
  template< class A, class B
          , bool IsAFusionSeq=boost::fusion::traits::is_sequence<A>::value
          , bool IsBFusionSeq=boost::fusion::traits::is_sequence<A>::value
          >
  struct max_ulp_value_;

  /// A and B are not Fusion Sequence, call distance_fn on the value
  template< class A, class B>
  struct max_ulp_value_<A,B,false,false>
  {
    typedef failed_value<A,B> failure_type;

    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails, std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      double d = distance_fn ( nt2::details::smallest_a(a,b)
                             , nt2::details::smallest_b(a,b)
                             );

      if(!(d <= max_ulpd) )
      {
        typename VF::value_type f = { a, b, d, i };
        fails.push_back(f);
      }

      ru = d;

      return true;
    }
  };

  /// A and B are Fusion Sequence, call distance_fn on every elements and
  /// compute the maximum
  template< class A, class B>
  struct max_ulp_value_<A,B,true,true>
  {
    template<int I, int N> struct max_ulp_seq_
    {
      template<class X, class Y, class F>
      BOOST_FORCEINLINE double
      operator()( X const& a, Y const& b, double z, F const& distance_fn ) const
      {
        return max_ulp_seq_<I+1,N>()
              ( a , b
              , safe_max( z
                        , distance_fn( nt2::details::
                                       smallest_a( boost::fusion::at_c<I>(a)
                                                 , boost::fusion::at_c<I>(b)
                                                 )
                                     , nt2::details::
                                       smallest_b( boost::fusion::at_c<I>(a)
                                                 , boost::fusion::at_c<I>(b)
                                                 )
                                     )
                        )
              , distance_fn
              );
      }
    };

    template<int N> struct max_ulp_seq_<N,N>
    {
      template<class X, class Y, class F>
      BOOST_FORCEINLINE double
      operator()( X const&, Y const&, double z, F const& ) const
      {
        return z;
      }
    };

    typedef failed_value<A,B> failure_type;

    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails, std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      double d =  max_ulp_seq_<0,boost::fusion::result_of
                                              ::size<A>::value>()(a,b,0.,distance_fn);

      if(!(d <=  max_ulpd))
      {
        failure_type f = {a, b, d, i};
        fails.push_back(f);
      }

      ru =  d;

      return true;
    }
  };

  /// Perform a test of equality on A and B with a given ulp tolerance
  /// Detects if A and/or B is actually a sequence and apply distance_fn on
  /// every elements of said sequences
  template< class A, class B
          , bool IsASeq=nt2::details::is_sequence<A>::value
          , bool IsBSeq=nt2::details::is_sequence<B>::value
          >
  struct max_ulp_;

  template< class A, class B>
  struct max_ulp_<A,B,false,false>
  {
    typedef typename max_ulp_value_<A,B>::failure_type failure_type;

    /// Main operator() checks of A and B are Fusion Sequence then
    /// jump into the proper eval() member functions depending on this
    /// status.
    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails, std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      return max_ulp_value_<A,B>()(a, b, max_ulpd, fails, i, ru, distance_fn);
    }
  };

  /// Both A and B are sequence, so we iterate over elements using the single
  /// value version of max_ulp_
  template<class A, class B>
  struct max_ulp_< A, B, true,true>
  {
    typedef failed_value< typename A::value_type
                        , typename B::value_type
                        >                         failure_type;

    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails, std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      if( std::distance(b.begin(),b.end()) == std::distance(a.begin(),a.end()))
      {
        double res(0);

        typename A::const_iterator ab = a.begin();
        typename A::const_iterator ae = a.end();
        typename B::const_iterator bb = b.begin();

        bool ok(true);
        while(ok && ab != ae)
        {
          double r(0);

          // Gather potential ULP errors from inside data
          typedef typename max_ulp_ < typename A::value_type
                                    , typename B::value_type
                                    >::failure_type local_failure_type;

          std::vector<local_failure_type> local_fails;

          ok = max_ulp_ < typename A::value_type
                        , typename B::value_type
                        >()(*ab,*bb,max_ulpd,local_fails,i,r,distance_fn);

          // Does inner check went ok and do we have fails ?
          if(ok && !local_fails.empty())
          {
            // Register current error
            typename VF::value_type f = { *ab,*bb, r, i };
            fails.push_back(f);
          }

          // Update global max ulp error
          res = safe_max(res,r);
          ab++;
          bb++;
          i++;
        }

        ru = res;

        return ok;
      }
      else
      {
        return false;
      }
    }
  };

  /// A is a sequence while B is not, this means B has exactly one element
  template<class A, class B>
  struct max_ulp_< A, B, true, false>
  {
    typedef failed_value<typename A::value_type, B> failure_type;

    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails, std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      if( std::distance(a.begin(),a.end()) == 1)
      {
        return max_ulp_<typename A::value_type,B>() ( *a.begin(),b
                                                    , max_ulpd,fails,i, ru
                                                    , distance_fn
                                                    );
      }
      else
      {
        return false;
      }
    }
  };

  /// B is a sequence while A is not, this means B has exactly one element
  template<class A, class B>
  struct max_ulp_< A, B, false, true>
  {
    typedef failed_value<A, typename B::value_type> failure_type;

    template<class VF, class F>
    BOOST_FORCEINLINE bool
    operator()( A const& a, B const& b
              , double max_ulpd, VF& fails , std::size_t i
              , double& ru
              , F const& distance_fn
              ) const
    {
      if( std::distance(b.begin(),b.end()) == 1)
      {
        return max_ulp_<A,typename B::value_type>() ( a,*b.begin()
                                                    , max_ulpd,fails,i, ru
                                                    , distance_fn
                                                    );
      }
      else
      {
        return false;
      }
    }
  };
} }

namespace nt2 { namespace unit
{
  /// INTERNAL ONLY
  /// Main test for equality over any types A and B within a given ulp tolerance
  template<class A, class B, class VF, class F>
  BOOST_FORCEINLINE
  bool max_ulp( A const& a, B const& b, double max_ulpd, VF& fails, double& ru, F const& distance_fn )
  {
    return details::max_ulp_<A,B>()(a,b,max_ulpd,fails,0,ru,distance_fn);
  }

  /// INTERNAL ONLY
  /// Main test for equality over any types A and B within a given ulp tolerance
  template<class A, class B, class VF>
  BOOST_FORCEINLINE
  bool max_ulp( A const& a, B const& b, double max_ulpd, VF& fails, double& ru )
  {
    return max_ulp(a, b, max_ulpd, fails, ru, details::max_ulps_caller());
  }
} }

namespace nt2 { namespace details
{

  template<typename Fails>
  BOOST_FORCEINLINE
  void report_ulp_unit_error( const char* desc, const char* func, int line
                            , Fails const& ulps, double ulpd, double N
                            , bool
                            )
  {
    if( ulps.empty() )
    {
      ::nt2::details::ulp_pass( desc, ulpd, N );
    }
    else
    {
      ::nt2::details::ulp_fail( desc, func, line, ulps.size(),N, true);

      BOOST_FOREACH ( typename Fails::const_reference f, ulps )
      {
        std::cout << std::setprecision(20)
                  << "\tlhs: "  << f.value
                  << ", rhs: "  << f.desired_value
                  << ", ULP: "  << f.ulp_error
                  << ", @( "    << f.index << " )";
        std::cout << std::endl;
      }

      std::cout << std::endl;
    }
  }

  template<typename A, typename B, class F>
  BOOST_FORCEINLINE
  void test_ulp_equal ( const char* desc, const char* func, int line
                      , A const& a, B const& b
                      , double N
                      , F const& distance_fn
                      )
  {
    typedef BOOST_TYPEOF(nt2::unit::eval(a))                        a_t;
    typedef BOOST_TYPEOF(nt2::unit::eval(b))                        b_t;
    typedef typename nt2::details::max_ulp_<a_t,b_t>::failure_type  f_t;

    std::vector< f_t > ulps;
    double ulpd = 0;
    bool ok = find_ulp_error(a,b,ulps,ulpd,N,distance_fn);

    if(ok)
    {
      ::nt2::details::report_ulp_unit_error(desc, func, line, ulps, ulpd, N, true);
    }
    else
    {
      ::nt2::details::ulp_fail( desc, func, line, ulps.size(), N, false);
    }
  }

  template<typename A, typename B, typename Fails, class F>
  BOOST_FORCEINLINE
  bool find_ulp_error ( A const& a, B const& b
                      , Fails& ulps, double& ulpd, double N
                      , F const& distance_fn
                      )
  {
    bool ok = nt2::unit::max_ulp( nt2::unit::eval(a)
                                , nt2::unit::eval(b)
                                , N, ulps, ulpd
                                , distance_fn
                                );
    return ok;
  }
} }

#endif


//==================================================================================================
/*!
  @file

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/ldexp.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/halfeps.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/detail/constant/limitexponent.hpp>
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/function/scalar/dec.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  using iT = bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using pi_t = bs::pack<iT, N>;

  T a1[N],  b[N], d[N];
  iT a2[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = (i%2) ? T(i) : T(-i);
     a2[i] = i%(sizeof(T)*8-1);
     b[i] = bs::ldexp(a1[i], a2[i]);
     d[i] = bs::pedantic_(bs::ldexp)(a1[i], a2[i]);
   }

  p_t aa1(&a1[0], &a1[0]+N);
  pi_t aa2(&a2[0], &a2[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t dd(&d[0], &d[0]+N);
  STF_IEEE_EQUAL(bs::ldexp(aa1, aa2), bb);
  STF_IEEE_EQUAL(bs::pedantic_(bs::ldexp)(aa1, aa2), dd);
}

STF_CASE_TPL("Check ldexp on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void tests(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N],  b[N], c[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-1.*i);
    b[i] = bs::ldexp(a1[i], 2);
    c[i] = bs::pedantic_(bs::ldexp)(a1[i], 2);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t cc(&c[0], &c[0]+N);

  STF_IEEE_EQUAL(bs::ldexp(aa1, 2)            , bb);
  STF_IEEE_EQUAL(bs::pedantic_(bs::ldexp)(aa1, 2) , cc);
}

STF_CASE_TPL("Check ldexp on pack/scalar" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;
  tests<T, N>(runtime);
  tests<T, N/2>(runtime);
  tests<T, N*2>(runtime);
}

STF_CASE_TPL("ldexp", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ldexp;
  using p_t = bs::pack<T>;

  using pi_t = bd::as_integer_t<p_t>;
  using r_t = decltype(ldexp(p_t(), pi_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(ldexp(bs::Inf<p_t>(),  2), bs::Inf<r_t>());
  STF_EQUAL(ldexp(bs::Minf<p_t>(), 2), bs::Minf<r_t>());
  STF_IEEE_EQUAL(ldexp(bs::Nan<p_t>(),  2), bs::Nan<r_t>());
#endif

  STF_EQUAL(ldexp(bs::Mone<p_t>(), 2), -bs::Four<r_t>());
  STF_EQUAL(ldexp(bs::One<p_t>(),  2), bs::Four<r_t>());
  STF_EQUAL(ldexp(bs::Zero<p_t>(), 2), bs::Zero<r_t>());
  STF_EQUAL(ldexp(bs::One <p_t>(), bs::Minexponent<p_t>()), bs::Smallestposval<r_t>());
  STF_EQUAL(ldexp(bs::One<p_t>()-bs::Halfeps<p_t>(),  bs::Maxexponent<p_t>()), bs::Valmax<p_t>()/2);

}

STF_CASE_TPL("ldexpi", STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ldexp;
 using p_t = bs::pack<T>;
  using pi_t = bd::as_integer_t<p_t>;
  using r_t = decltype(ldexp(p_t(), pi_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  STF_EQUAL(ldexp(bs::One <p_t>(), 2), r_t(4));
  STF_EQUAL(ldexp(bs::Three<p_t>(), 2), r_t(12));
  STF_EQUAL(ldexp(bs::Mone<p_t>(), 2), r_t(-4));
}

STF_CASE_TPL("ldexp floating exponent", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ldexp;
  using p_t = bs::pack<T>;

  using r_t = decltype(bs::pedantic_(ldexp)(p_t(), p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  STF_EQUAL(ldexp(p_t(-1), p_t(2)), p_t(-4));
  STF_EQUAL(bs::ldexp(bs::One<p_t>(),  p_t(2)), p_t(4));
  STF_EQUAL(bs::ldexp(bs::Zero<p_t>(), p_t(2)), bs::Zero<p_t>());

  for(int i=bs::Minexponent<T>(); i < bs::Minexponent<T>(); ++i)
  {
    STF_EQUAL(bs::ldexp(p_t(1.5), p_t(i)), p_t(std::ldexp(T(1.5), i)));
    STF_EQUAL(bs::ldexp(p_t(-1.5), p_t(i)),p_t(std::ldexp(T(-1.5), i)));
  }

}


// Dervives from VTK

/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkLandmarkTransform.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

  =========================================================================*/

#include "bisPointRegistrationUtils.h"
#include "bisJSONParameterList.h"
#include "bisEigenUtil.h"
#include "bisPointLocator.h"
#include <vector>
#include <Eigen/Dense>

namespace bisPointRegistrationUtils {

  int isPointSetValid(bisSimpleMatrix<float>* Points,
                      int minrows,
                      int numcols,
                      int debug) {

    if (Points==NULL) {
      if (debug)
        std::cout << "__ NULL input matrix set " << std::endl;
      return 0;
    }

    int N_PTS = Points->getNumRows();
    int N_COLS= Points->getNumCols();
    if (N_PTS < minrows ||    N_COLS != numcols  ) {
      if (debug)
        std::cout << "Update: Matrix not valid " << N_PTS << "*" << N_COLS << std::endl;
      return 0;
    }

    return N_PTS;
  }

  int computeCentroid(bisSimpleMatrix<float>* Points,
                      float centroid[3],
                      int debug) {

    for (int ia=0;ia<=2;ia++)
      centroid[ia]=0.0;

    int N_PTS=isPointSetValid(Points,2,3,debug);
    if (N_PTS<1)
      return 0;

    float* inp=Points->getData();
    for (int pt=0;pt<N_PTS;pt++) {
      for (int ia=0;ia<=2;ia++)
        centroid[ia]=centroid[ia]+inp[pt*3+ia];
    }

    for (int ia=0;ia<=2;ia++)
      centroid[ia]/=float(N_PTS);

    return 1;
  }

  // ----------------------------------------------------------------------------------------------------
  // transform Points by transformation
  // ----------------------------------------------------------------------------------------------------
  bisSimpleMatrix<float>* transformPoints(bisSimpleMatrix<float>* Input,
                                          bisAbstractTransformation* Transformation,
                                          int debug) {

    if (Transformation == NULL) {
      std::cout << "NULL transformation in transformPoints " << std::endl;
        return NULL;
      }

    int N_PTS=isPointSetValid(Input);
    if (N_PTS==0) {
      std::cout << "Bad points =" << N_PTS << std::endl;
      return NULL;
    }

    if (debug) {
      std::cout << "___ Transforming " << N_PTS << " points with " << Transformation->getClassName() << std::endl;
      std::cout << "___ Transforming " << N_PTS << " points with " << Input->getNumRows() << "*" << Input->getNumCols() << std::endl;
    }

    bisSimpleMatrix<float>* Output=new bisSimpleMatrix<float>();
    Output->zero(N_PTS,3);
    float* inp=Input->getData();
    float* out=Output->getData();

    for (int pt=0;pt<N_PTS;pt++)
      {
        int index=pt*3;
        float x[3],y[3];
        for (int ia=0;ia<=2;ia++)
          x[ia]=inp[index+ia];
        //if (pt>930)
        //std::cout << "pt=" << pt << " x=" << x[0] << "," << x[1] << "," << x[2] << " " << std::endl;
        Transformation->transformPoint(x,y);
        //if (pt>930) {
        //          std::cout << "--> y=" << y[0] << "," << y[1] << "," << y[2] << std::endl;
        //          std::cout << std::endl;
        //        }
        for (int ia=0;ia<=2;ia++)
          out[index+ia]=y[ia];
      }

    std::cout << "___ Transforming " << N_PTS << " points with " << Transformation->getClassName() << " done." << std::endl;

    return Output;
  }

  // ----------------------------------------------------------------------------------------------------
  float distance2(float x[3],float y[3]) {

    return powf(x[0]-y[0],2.0)+
      powf(x[1]-y[1],2.0)+
      powf(x[2]-y[2],2.0);
  }

    // ----------------------------------------------------------------------------------------------------
  // transform Points by transformation
  // ----------------------------------------------------------------------------------------------------
  float computeMappingError(bisSimpleMatrix<float>* Input,
                            bisSimpleMatrix<float>* Output,
                            bisAbstractTransformation* Transformation,
                            int debug) {

    if (Transformation == NULL) {
      std::cout << "NULL transformation in transformPoints " << std::endl;
      return -1.0;
    }

    int N_PTS=isPointSetValid(Input);
    if (N_PTS==0) {
       std::cerr << "Bad input point set" << std::endl;
       return -1.0;
    }

    if (!isPointSetValid(Output,N_PTS,3,0)) {
      std::cerr << "Bad target point set" << std::endl;
      return -1.0;
    }

    if (debug)
      std::cout << "___ Transforming " << N_PTS << " points with " << Transformation->getClassName() << std::endl;

    float* inp=Input->getData();
    float* out=Output->getData();
    float sumdist2=0.0;

    for (int pt=0;pt<N_PTS;pt++) {
      float x[3],tx[3],y[3];
      for (int ia=0;ia<=2;ia++) {
        x[ia]=inp[pt*3+ia];
        y[ia]=out[pt*3+ia];
      }
      Transformation->transformPoint(x,tx);
      sumdist2+=distance2(tx,y);
    }

    return sqrt(sumdist2/float(N_PTS));
  }

  // ----------------------------------------------------------------------------------------------------
  // Compute Lnadmark Transform
  // ----------------------------------------------------------------------------------------------------
  int computeLandmarkTransformation(bisSimpleMatrix<float>* RawSourceLandmarks,
                                    bisSimpleMatrix<float>* RawTargetLandmarks,
                                    int mode,
                                    bisMatrixTransformation* OutputTransformation,
                                    bisSimpleVector<float>* RawWeights,
                                    int debug)

  {
    OutputTransformation->identity();

    if (!RawSourceLandmarks  || !RawTargetLandmarks) {
      std::cout << "Bad Inputs " << std::endl;
      return 0;
    }

    int N_PTS = RawSourceLandmarks->getNumRows();
    int N_COLS= RawSourceLandmarks->getNumCols();

    if (N_PTS != RawTargetLandmarks->getNumRows() ||
        N_COLS != RawTargetLandmarks->getNumCols() ||
        N_COLS !=3 ||
        N_PTS < 4) {
      std::cout << "Update: Source and Target Landmarks contain a different number of points or not enough points (" << N_PTS << "," << N_COLS <<")" << std::endl;
      return 0;
    }

    if (RawWeights) {
      if (RawWeights->getLength()!=N_PTS) {
        std::cout << "Bad Weights specified " << std::endl;
        return 0;
      }
    }

    if (debug) {
      std::cout << "___ Compute Landmark Transform: Source and Target Landmarks have the same number of points  (" << N_PTS << "," << N_COLS <<")" << std::endl;
      std::cout << "___ Mode = " << mode << std::endl;
      if (RawWeights)
        std::cout << "___ Using Weights " << std::endl;
    }

    Eigen::MatrixXf Source=bisEigenUtil::mapToEigenMatrix(RawSourceLandmarks);
    Eigen::MatrixXf Target=bisEigenUtil::mapToEigenMatrix(RawTargetLandmarks);
    Eigen::VectorXf Weights;
    if (RawWeights) {
      Weights=bisEigenUtil::mapToEigenVector(RawWeights);
    } else {
      Weights=Eigen::VectorXf::Zero(N_PTS);
      for (int i=0;i<N_PTS;i++)
        Weights(i)=1.0;
      std::cout << "___ Setting all weights to 1.0" << std::endl;
    }

    // --- compute the necessary transform to match the two sets of landmarks ---

    /*
      The solution is based on
      Berthold K. P. Horn (1987),
      "Closed-form solution of absolute orientation using unit quaternions,"
      Journal of the Optical Society of America A, 4:629-642
    */

    // Original python implementation by David G. Gobbi
    // -- find the centroid of each set --

    float source_centroid[3] = { 0, 0, 0 };
    float target_centroid[3] = { 0, 0, 0 };
    float sumw=0.0;
    for (int i = 0; i < N_PTS; i++) {
      float w=Weights(i);
      for (int ia=0;ia<=2;ia++) {
        source_centroid[ia] += Source(i,ia)*w;
        target_centroid[ia] += Target(i,ia)*w;
      }
      sumw+=w;
    }

    for (int ia=0;ia<=2;ia++) {
      source_centroid[ia] = source_centroid[ia]/ sumw;
      target_centroid[ia] = target_centroid[ia]/ sumw;
    }

    if (debug) {
      std::cout << "___ Source Centroid = " << source_centroid[0] << "," << source_centroid[1] << "," << source_centroid[2] << std::endl;
      std::cout << "___ Target Centroid = " << target_centroid[0] << "," << target_centroid[1] << "," << target_centroid[2] << std::endl;
      std::cout << "___ sumw = " << sumw << " N_PTS = " << N_PTS << std::endl;
    }

    // -- build the 3x3 matrix M --

    float M[3][3];
    float AAT[3][3];
    for (int i = 0; i < 3; i++)  {
      AAT[i][0] = M[i][0] = 0.0F; // fill M with zeros
      AAT[i][1] = M[i][1] = 0.0F;
      AAT[i][2] = M[i][2] = 0.0F;
    }

    float a[3], b[3];
    float sa = 0.0F, sb = 0.0F;
    for (int pt = 0; pt < N_PTS; pt++) {

      float wgt=Weights(pt);

      // get the origin-centred point (a) in the source set
      for (int ia=0;ia<=2;ia++) {
        a[ia]=Source(pt,ia);
        b[ia]=Target(pt,ia);
      }
      a[0] -= source_centroid[0];
      a[1] -= source_centroid[1];
      a[2] -= source_centroid[2];
      // get the origin-centred point (b) in the target set
      b[0] -= target_centroid[0];
      b[1] -= target_centroid[1];
      b[2] -= target_centroid[2];
      // accumulate the products a*T(b) into the matrix M
      for (int i = 0; i < 3; i++) {
        M[i][0] += wgt * a[i] * b[0];
        M[i][1] += wgt * a[i] * b[1];
        M[i][2] += wgt * a[i] * b[2];

        // for the affine transform, compute ((a.a^t)^-1 . a.b^t)^t.
        // a.b^t is already in M.  here we put a.a^t in AAT.
        if (mode == 2 ) {
          AAT[i][0] += wgt * a[i] * a[0];
          AAT[i][1] += wgt * a[i] * a[1];
          AAT[i][2] += wgt * a[i] * a[2];
        }
      }
    // accumulate scale factors (if desired)
      sa += wgt*(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
      sb += wgt*(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
    }

    // if source or destination is degenerate then only report
    // translation
    if (sa == 0.0 || sb == 0.0) {
      bisUtil::mat44 Mat44;
      for (int i=0;i<=3;i++) {
        for (int j=0;j<=3;j++) {
          if (i==j)
            Mat44[i][j]=1.0;
          else
            Mat44[i][j]=0.0;
        }
      }

      Mat44[0][3] = target_centroid[0] - source_centroid[0];
      Mat44[1][3] = target_centroid[1] - source_centroid[1];
      Mat44[2][3] = target_centroid[2] - source_centroid[2];
      OutputTransformation->setMatrix(Mat44);

      return 1;
    }

    Eigen::MatrixXf E_M=Eigen::MatrixXf(3,3);
    Eigen::MatrixXf E_AAT=Eigen::MatrixXf(3,3);
    for (int i=0;i<=2;i++) {
      for (int j=0;j<=2;j++) {
        E_M(i,j)=M[i][j];
        E_AAT(i,j)=AAT[i][j];
      }
    }

    Eigen::MatrixXf OutputMatrix(4,4);
    OutputMatrix(3,3)=1;

    if (mode == 2)
      {
        E_M=E_AAT.inverse()*E_M;
        // Matrix = M^t
        for (int i = 0; i < 3; ++i)
          for (int j = 0; j < 3; ++j)
            OutputMatrix(i,j) = E_M(j,i);
      }
    else
      {
        // compute required scaling factor (if desired)
        float scale = (float)sqrt(sb / sa);
        if (debug)
          std::cout << "___ Scale=" << scale << std::endl;

        float N[4][4];
        for (int i = 0; i < 4; i++) {
          N[i][0] = 0.0F; // fill N with zeros
          N[i][1] = 0.0F;
          N[i][2] = 0.0F;
          N[i][3] = 0.0F;
        }
        // on-diagonal elements
        N[0][0] = M[0][0] + M[1][1] + M[2][2];
        N[1][1] = M[0][0] - M[1][1] - M[2][2];
        N[2][2] = -M[0][0] + M[1][1] - M[2][2];
        N[3][3] = -M[0][0] - M[1][1] + M[2][2];
        // off-diagonal elements
        N[0][1] = N[1][0] = M[1][2] - M[2][1];
        N[0][2] = N[2][0] = M[2][0] - M[0][2];
        N[0][3] = N[3][0] = M[0][1] - M[1][0];

        N[1][2] = N[2][1] = M[0][1] + M[1][0];
        N[1][3] = N[3][1] = M[2][0] + M[0][2];
        N[2][3] = N[3][2] = M[1][2] + M[2][1];

        // -- build the 4x4 matrix N --
        Eigen::MatrixXf NMat(4,4);
        for (int i=0;i<=3;i++)
          for (int j=0;j<=3;j++)
            NMat(i,j)=N[i][j];

        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(NMat);
        Eigen::MatrixXf eigenvectors=eigensolver.eigenvectors();
        Eigen::VectorXf eigenvalues=eigensolver.eigenvalues();

        if (debug) {
          std::cout << "___ Eigenvalues=" << eigenvalues << std::endl << std::endl;
          std::cout << "___ Eigenvectors=" << eigenvectors << std::endl;
        }

        float maxeigen=eigenvalues(0);
        int maxindex=0;
        for (int i=1;i<=3;i++) {
          if (eigenvalues(i)>maxeigen) {
            maxeigen=eigenvalues(i);
            maxindex=i;
          }
        }

        if (debug)
          std::cout << "___ Max Eigen value=" << maxeigen << "( at index=" << maxindex << ")" << std::endl;

        // the eigenvector with the largest eigenvalue is the quaternion we want
        // (they are sorted in decreasing order for us by JacobiN)
        // Find largest eigenvalue
        float w = eigenvectors(0,maxindex);
        float x = eigenvectors(1,maxindex);
        float y = eigenvectors(2,maxindex);
        float z = eigenvectors(3,maxindex);

        // convert quaternion to a rotation matrix

        float ww = w * w;
        float wx = w * x;
        float wy = w * y;
        float wz = w * z;

        float xx = x * x;
        float yy = y * y;
        float zz = z * z;

        float xy = x * y;
        float xz = x * z;
        float yz = y * z;

        OutputMatrix(0,0) = ww + xx - yy - zz;
        OutputMatrix(1,0) = 2.0 * (wz + xy);
        OutputMatrix(2,0) = 2.0 * (-wy + xz);

        OutputMatrix(0,1) = 2.0 * (-wz + xy);
        OutputMatrix(1,1) = ww - xx + yy - zz;
        OutputMatrix(2,1) = 2.0 * (wx + yz);

        OutputMatrix(0,2) = 2.0 * (wy + xz);
        OutputMatrix(1,2) = 2.0 * (-wx + yz);
        OutputMatrix(2,2) = ww - xx - yy + zz;

        if (mode != 0) {
          std::cout << "___ Adding Scale=" << scale << std::endl;
          // add in the scale factor (if desired)
          for (int i = 0; i < 3; i++)
            {
              OutputMatrix(i,0) *= scale;
              OutputMatrix(i,1) *= scale;
              OutputMatrix(i,2) *= scale;
            }
        }
      }

    // the translation is given by the difference in the transformed source
    // centroid and the target centroid
    float sx = OutputMatrix(0,0) * source_centroid[0] +    OutputMatrix(0,1) * source_centroid[1] +    OutputMatrix(0,2) * source_centroid[2];
    float sy = OutputMatrix(1,0) * source_centroid[0] +    OutputMatrix(1,1) * source_centroid[1] +    OutputMatrix(1,2) * source_centroid[2];
    float sz = OutputMatrix(2,0) * source_centroid[0] +    OutputMatrix(2,1) * source_centroid[1] +    OutputMatrix(2,2) * source_centroid[2];

    OutputMatrix(0,3) = target_centroid[0] - sx;
    OutputMatrix(1,3) = target_centroid[1] - sy;
    OutputMatrix(2,3) = target_centroid[2] - sz;

    // fill the bottom row of the 4x4 matrix
    OutputMatrix(3,0) = 0.0;
    OutputMatrix(3,1) = 0.0;
    OutputMatrix(3,2) = 0.0;
    OutputMatrix(3,3) = 1.0;

    if (debug)
      std::cout << "___ Output = " << OutputMatrix << std::endl;

    bisUtil::mat44 Mat44;
    for (int i=0;i<=3;i++)
      for (int j=0;j<=3;j++)
        Mat44[i][j]=OutputMatrix(i,j);

    OutputTransformation->setMatrix(Mat44);
    return 1;
  }

  // ----------------------------------------------------------------------------------------------------------------
  // Print Utilities

  void printTwoPoints(bisSimpleMatrix<float>* pts,std::string name) {
    int rows=pts->getNumRows();
    int cols=pts->getNumCols();
    float* data=pts->getData();
    int index=3*(rows/2);
    std::cout << "Set = " << name << " " << rows << "*" << cols << std::endl;
    if (cols==3) {
      int ia=0;
      std::cout << "       Point " << int(ia) << "=" << data[3*ia] << "," << data[3*ia+1] << "," << data[3*ia+2] << std::endl;
      ia=rows/2;
      std::cout << "       Point " << int(ia) << "=" << data[3*ia] << "," << data[3*ia+1] << "," << data[3*ia+2] << std::endl;
    } else {
      std::cout << "       Point 0=" << data[0] << std::endl;
      std::cout << "       Point " << int(rows/2) << "=" << data[index] << std::endl;
    }
    std::cout << std::endl;
  }

  void printJointPoints(bisSimpleMatrix<float>* pts,bisSimpleMatrix<float>* pts2,bisSimpleVector<float>* wv,std::string name,int incr) {
    int rows=pts->getNumRows();
    int cols=pts->getNumCols();
    float* data=pts->getData();
    float* data2=pts2->getData();
    float* w=wv->getData();

    std::cout << "Set = " << name << " " << rows << "*" << cols << std::endl;
    if (cols==3) {
      for (int ia=0;ia<rows;ia+=incr) {
        std::cout << "       Point " << int(ia) << "=" << data[3*ia] << "," << data[3*ia+1] << "," << data[3*ia+2];
        std::cout << "  --> "<< data2[3*ia] << "," << data2[3*ia+1] << "," << data2[3*ia+2] << " (w=" << w[ia] << ")" << std::endl;
      }
    }
    std::cout << std::endl;
  }

  void printTwoElements(bisSimpleVector<float>* pts,std::string name) {
    int rows=pts->getLength();
    float* data=pts->getData();
    std::cout << "Set = " << name << " " << rows << std::endl;
    std::cout << "       Point 0=" << data[0] << std::endl;
    std::cout << "       Point " << int(rows/2) << "=" << data[rows/2] << std::endl;
    std::cout << std::endl;
  }

  void printMatrix(bisMatrixTransformation* xform, std::string name) {
    bisUtil::mat44 m;
    std::cout << " " << name << " = ";
    xform->getMatrix(m);
    for (int ia=0;ia<=3;ia++) {
      std::cout << "[ " ;
      for (int ib=0;ib<=3;ib++) {
        std::cout << m[ia][ib] << " ";
      }
      std::cout << "]" << std::endl << "  ";
    }
    std::cout << std::endl;
  }

}
/** Computes best fit Landmark Transformation (see VTK/VTKLandmarkTransform.cxx) given two sets of points
 * @param RawSourceLandmarks the source points (Nx3)
 * @param RawTargetLandmarks the target points (Nx3)
 * @param mode 0=rigid,1=similarity,2=affine
 * @param Output the output Matrix Transformation
 * @return 1 if success 0 if failed
 */
// BIS: { 'computeLandmarkTransformWASM', 'bisLinearTransformation', [ 'bisMatrix', 'bisMatrix', 'ParamObj', 'debug' ] }
unsigned char* computeLandmarkTransformWASM(unsigned char* source_ptr, unsigned char* target_ptr,const char* jsonstring,int debug) {

  std::unique_ptr<bisJSONParameterList> params(new bisJSONParameterList());
  if (!params->parseJSONString(jsonstring))
    return 0;

  int mode=params->getIntValue("mode",2);
  if (debug)
    std::cout << "___ mode=" << mode << std::endl;

  std::unique_ptr<bisSimpleMatrix<float> > source(new bisSimpleMatrix<float>("source_points_json"));
  if (!source->linkIntoPointer(source_ptr))
    return 0;

  if (debug)
    std::cout << "___ Ref Allocated = " << source->getNumRows() << "*" << source->getNumCols() << std::endl;

  std::unique_ptr<bisSimpleMatrix<float> > target(new bisSimpleMatrix<float>("target_points_json"));
  if (!target->linkIntoPointer(target_ptr))
    return 0;

  if (debug)
    std::cout << "___ Target Allocated = " << target->getNumRows() << "*" << target->getNumCols() << std::endl;

  std::unique_ptr<bisMatrixTransformation> output(new bisMatrixTransformation("output_matrix"));

  int result=bisPointRegistrationUtils::computeLandmarkTransformation(source.get(),
                                                                      target.get(),
                                                                      mode,
                                                                      output.get(),
                                                                      NULL,
                                                                      debug);

  if (debug)
    std::cout << "___ Computed ok=" << result << std::endl;

  if (result==0) {
    return 0;
  }

  std::unique_ptr< bisSimpleMatrix<float> > raw_output (output->getSimpleMatrix());
  return raw_output->releaseAndReturnRawArray();

}

/** Test Point Locator
 * @param RawPoints the source points (Nx3)
 * @param paramobj { mode 0=nearest,1=threshold, threshold = 5.0, x,y,z = point, l=length }
 * @param Output the output points
 */
// BIS: { 'testPointLocatorWASM', 'Matrix', [ 'Matrix', 'ParamObj', 'debug' ] }
unsigned char* testPointLocatorWASM(unsigned char* source_ptr,const char* jsonstring,int debug) {

  std::unique_ptr<bisJSONParameterList> params(new bisJSONParameterList());
  if (!params->parseJSONString(jsonstring))
    return 0;

  int mode=params->getIntValue("mode",0);
  float threshold=params->getFloatValue("threshold",5.0);
  float x[3];
  x[0]=params->getFloatValue("x",0.0);
  x[1]=params->getFloatValue("y",0.0);
  x[2]=params->getFloatValue("z",0.0);

  float length=params->getFloatValue("length",10.0);
  if (debug)
    std::cout << "mode=" << mode << ", threshold=" << threshold << " target=(" << x[0] << "," << x[1] << "," << x[2] << ")" << " length=" << length <<  std::endl;

  std::shared_ptr<bisSimpleMatrix<float> > source(new bisSimpleMatrix<float>("source_points_json"));
  if (!source->linkIntoPointer(source_ptr))
    return 0;

  bisPointLocator* locator=new bisPointLocator();
  locator->initialize(source,length,debug);

  if (mode<1)
    {
      std::unique_ptr<bisSimpleMatrix<float> > output_points(new bisSimpleMatrix<float>("output_points"));
      float y[3];
      if (debug)
        std::cout << "Looking for nearest point to " << x[0] << "," << x[1] << "," << x[2] << std::endl;

      locator->getNearestPoint(x,y,debug);
      output_points->zero(1,3);
      float* out=output_points->getData();
      out[0]=y[0];
      out[1]=y[1];
      out[2]=y[2];
      delete locator;
      return output_points->releaseAndReturnRawArray();
    }

  std::vector<int> plist;
  if (debug)
    std::cout << "Looking for point closer than " << threshold << " to " << x[0] << "," << x[1] << "," << x[2] << std::endl;

  int np=locator->getPointsWithinRadius(x,threshold,plist,debug);
  if (mode==1)
    {
      std::unique_ptr<bisSimpleMatrix<float> > output_points(new bisSimpleMatrix<float>("output_points"));
      if (np>0) {
        output_points->zero(np,3);
        float* inp=source->getData();
        float* out=output_points->getData();
        for (int i=0;i<np;i++) {
          int index=plist[i];
          for (int ia=0;ia<=2;ia++)
            out[i*3+ia]=inp[index*3+ia];
        }
      } else {
        output_points->zero(1,1);
      }
      delete locator;
      return output_points->releaseAndReturnRawArray();
    }

  std::unique_ptr<bisSimpleMatrix<int> > output_indices(new bisSimpleMatrix<int>("output_indices"));
  if (np>0) {
    output_indices->zero(np,1);
    int* out=output_indices->getData();
    for (int i=0;i<np;i++)
      out[i]=plist[i];
  } else {
    output_indices->zero(1,1);
    int* out=output_indices->getData();
    out[0]=-1;
  }
  delete locator;
  return output_indices->releaseAndReturnRawArray();
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 3, 3> test_long_3_3(const Eigen::Matrix<boost::int64_t, 3, 3> & M)
{
	return M;
}
void export_long_3_3()
{
	boost::python::def("test_long_3_3",test_long_3_3);
}


#ifndef SM_RANDOM_HPP
#define SM_RANDOM_HPP

#include <boost/cstdint.hpp>

namespace sm {
  namespace random {

    /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1.
    double normal();
    /// \brief Return a sample from a normally distributed random distribution with zero mean and standard deviation 1.
    double randn();

    /// \brief Return a sample from a uniform distribution between 0.0 and 1.0
    double uniform();

    /// \brief Return a sample from a uniform distribution between 0.0 and 1.0
    double rand();

    /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound)
    double randLU(double lowerBoundInclusive, double upperBoundExclusive);

    /// \brief Return a sample from a uniform distribution in the half-open range [lowerBound, upperBound)
    int randLUi(int lowerBoundInclusive, int upperBoundExclusive);

    /// \brief Seed the random number generator.
    void seed(boost::uint64_t s);

  } // namespace random
} // namespace sm

#endif /* SM_RANDOM_HPP */


//  Copyright (c) 2007-2013 Hartmut Kaiser
//
//  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)

#include <hpx/hpx_init.hpp>
#include <hpx/include/actions.hpp>
#include <hpx/include/util.hpp>
#include <hpx/include/lcos.hpp>
#include <hpx/util/unwrapped.hpp>

#include <iostream>

#include <boost/cstdint.hpp>
#include <boost/format.hpp>

///////////////////////////////////////////////////////////////////////////////
boost::uint64_t threshold = 2;
boost::uint64_t distribute_at = 2;
int num_repeats = 1;

///////////////////////////////////////////////////////////////////////////////
boost::atomic<std::size_t> serial_execution_count(0);

std::size_t get_serial_execution_count()
{
    return serial_execution_count.load();
}
HPX_PLAIN_ACTION(get_serial_execution_count);

///////////////////////////////////////////////////////////////////////////////
boost::atomic<std::size_t> next_locality(0);
std::vector<hpx::id_type> localities;
hpx::id_type here;

struct when_all_wrapper
{
    typedef boost::uint64_t result_type;
    typedef HPX_STD_TUPLE<
            hpx::lcos::future<boost::uint64_t>
          , hpx::lcos::future<boost::uint64_t> > data_type;

    boost::uint64_t operator()(
        hpx::lcos::future<data_type> data
    ) const
    {
        data_type v = data.get();
        return HPX_STD_GET(0, v).get() + HPX_STD_GET(1, v).get();
    }
};

///////////////////////////////////////////////////////////////////////////////
BOOST_NOINLINE boost::uint64_t fibonacci_serial_sub(boost::uint64_t n)
{
    if (n < 2)
        return n;
    return fibonacci_serial_sub(n-1) + fibonacci_serial_sub(n-2);
}

boost::uint64_t fibonacci_serial(boost::uint64_t n)
{
    ++serial_execution_count;
    return fibonacci_serial_sub(n);
}

///////////////////////////////////////////////////////////////////////////////
hpx::id_type const& get_next_locality(boost::uint64_t next)
{
    return localities[next % localities.size()];
}

///////////////////////////////////////////////////////////////////////////////
hpx::future<boost::uint64_t> fibonacci_future(boost::uint64_t n);
HPX_PLAIN_ACTION(fibonacci_future);

hpx::future<boost::uint64_t> fibonacci_future(boost::uint64_t n)
{
    // if we know the answer, we return a future encapsulating the final value
    if (n < 2)
        return hpx::make_ready_future(n);
    if (n < threshold)
        return hpx::make_ready_future(fibonacci_serial(n));

    fibonacci_future_action fib;
    hpx::id_type loc1 = here;
    hpx::id_type loc2 = here;

    if (n == distribute_at) {
        loc2 = get_next_locality(++next_locality);
    }
    else if (n-1 == distribute_at) {
        boost::uint64_t next = next_locality += 2;
        loc1 = get_next_locality(next-1);
        loc2 = get_next_locality(next);
    }

    hpx::future<boost::uint64_t> f = hpx::async(fib, loc1, n-1);
    hpx::future<boost::uint64_t> r = fib(loc2, n-2);

    return hpx::when_all(f, r).then(when_all_wrapper());
}

///////////////////////////////////////////////////////////////////////////////
int hpx_main(boost::program_options::variables_map& vm)
{
    // extract command line argument, i.e. fib(N)
    boost::uint64_t n = vm["n-value"].as<boost::uint64_t>();
    std::string test = vm["test"].as<std::string>();
    boost::uint64_t max_runs = vm["n-runs"].as<boost::uint64_t>();

    if (max_runs == 0) {
        std::cerr << "fibonacci_futures_distributed: wrong command "
            "line argument value for "
            "option 'n-runs', should not be zero" << std::endl;
        return hpx::finalize(); // Handles HPX shutdown
    }

    bool executed_one = false;
    boost::uint64_t r = 0;

    if (test == "all" || test == "0")
    {
        // Keep track of the time required to execute.
        boost::uint64_t start = hpx::util::high_resolution_clock::now();

        // Synchronous execution, use as reference only.
        r = fibonacci_serial(n);

//        double d = double(hpx::util::high_resolution_clock::now() - start) / 1.e9;
        boost::uint64_t d = hpx::util::high_resolution_clock::now() - start;
        char const* fmt = "fibonacci_serial(%1%) == %2%,"
            "elapsed time:,%3%,[s]\n";
        std::cout << (boost::format(fmt) % n % r % d);

        executed_one = true;
    }

    if (test == "all" || test == "1")
    {
        // Keep track of the time required to execute.
        boost::uint64_t start = hpx::util::high_resolution_clock::now();

        for (std::size_t i = 0; i != max_runs; ++i)
        {
            // Create a Future for the whole calculation and wait for it.
            next_locality.store(0);
            r = fibonacci_future(n).get();
        }

//        double d = double(hpx::util::high_resolution_clock::now() - start) / 1.e9;
        boost::uint64_t d = hpx::util::high_resolution_clock::now() - start;
        char const* fmt = "fibonacci_future(%1%) == %2%,elapsed time:,%3%,[s],%4%\n";
        std::cout << (boost::format(fmt) % n % r % (d / max_runs) % next_locality.load());

        get_serial_execution_count_action serial_count;
        BOOST_FOREACH(hpx::id_type const& loc, hpx::find_all_localities())
        {
            std::size_t count = serial_count(loc);
            std::cout << (boost::format("  serial-count,%1%,%2%\n") %
                loc % (count / max_runs));
        }

        executed_one = true;
    }

    if (!executed_one)
    {
        std::cerr << "fibonacci_futures_distributed: wrong command line argument "
            "value for option 'tests', should be either 'all' or a number between "
            "zero and 1, value specified: " << test << std::endl;
    }

    return hpx::finalize(); // Handles HPX shutdown
}

///////////////////////////////////////////////////////////////////////////////
boost::program_options::options_description get_commandline_options()
{
    // Configure application-specific options
    boost::program_options::options_description
       desc_commandline("Usage: " HPX_APPLICATION_STRING " [options]");

    using boost::program_options::value;
    desc_commandline.add_options()
        ( "n-value", value<boost::uint64_t>()->default_value(10),
          "n value for the Fibonacci function")
        ( "n-runs", value<boost::uint64_t>()->default_value(1),
          "number of runs to perform")
        ( "threshold", value<unsigned int>()->default_value(2),
          "threshold for switching to serial code")
        ( "distribute-at", value<unsigned int>()->default_value(2),
          "threshold for distribution to other nodes")
        ( "test", value<std::string>()->default_value("all"),
          "select tests to execute (0-7, default: all)")
        ( "loc-repeat", value<int>()->default_value(1),
          "how often should a locality > 0 be used")
    ;
    return desc_commandline;
}

///////////////////////////////////////////////////////////////////////////////
void init_globals()
{
    // Retrieve command line using the Boost.ProgramOptions library.
    boost::program_options::variables_map vm;
    if (!hpx::util::retrieve_commandline_arguments(get_commandline_options(), vm))
    {
        HPX_THROW_EXCEPTION(hpx::commandline_option_error,
            "fibonacci_futures_distributed",
            "failed to handle command line options");
        return;
    }

    boost::uint64_t n = vm["n-value"].as<boost::uint64_t>();

    threshold = vm["threshold"].as<unsigned int>();
    if (threshold < 2 || threshold > n) {
        HPX_THROW_EXCEPTION(hpx::commandline_option_error,
            "fibonacci_futures_distributed",
            "wrong command line argument value for option 'threshold', "
            "should be in between 2 and n-value, value specified: " +
                boost::lexical_cast<std::string>(threshold));
        return;
    }

    distribute_at = vm["distribute-at"].as<unsigned int>();
    if (distribute_at < 2 || distribute_at > n) {
        HPX_THROW_EXCEPTION(hpx::commandline_option_error,
            "fibonacci_futures_distributed",
            "wrong command line argument value for option 'distribute-at', "
            "should be in between 2 and n-value, value specified: " +
                boost::lexical_cast<std::string>(distribute_at));
        return;
    }

    here = hpx::find_here();
    next_locality.store(0);
    serial_execution_count.store(0);

    // try to more evenly distribute the work over the participating localities
    std::vector<hpx::id_type> locs = hpx::find_all_localities();
    std::size_t num_repeats = vm["loc-repeat"].as<int>();

    localities.push_back(here);      // add ourselves
    for (std::size_t j = 0; j != num_repeats; ++j)
    {
        for (std::size_t i = 0; i != locs.size(); ++i)
        {
            if (here == locs[i])
                continue;
            localities.push_back(locs[i]);
        }
    }
}

int main(int argc, char* argv[])
{
    // Initialize and run HPX
    hpx::register_startup_function(&init_globals);
    return hpx::init(get_commandline_options(), argc, argv);
}