/* Process all images in the directory specified in the argument.
Xiang Xiang (eglxiang@gmail.com), May 2016, MIT license.
Main functionality: compute VGG_Face features for each image.
First you need to create the saving directory yourself.
It writes the feature vector of each face image into a txt file.
*/

#define CPU_ONLY
#define CORR_METRIC
#include "Classifier.h"
#include <caffe/caffe.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <math.h>
#include <dirent.h>
#include <string.h>
//#include <boost/python.hpp>

using namespace caffe;
using namespace cv;
using namespace std;
//using namespace boost::python

int main(int argc, char** argv)
{
    // load image
    if (argc != 4)
    {
        cerr<<"Run a simple test sample using pretrained VGG face model. " << endl
            << "Usage: " << argv[0]
            << "deploy.prototxt network.caffemodel folderpath" << endl;
        return -1;
    }
    string model_file = argv[1];
    string trained_file = argv[2];
    Classifier classifier(model_file, trained_file);
    string foldername = argv[3];
    string rootpath = "/mnt/localsata/selected_faces/";
    string folderpath = rootpath + foldername;

    // list files in the folder
    DIR *dir;
    dir = opendir(folderpath.c_str());
    cout << dir << endl;
    string imgName;
    struct dirent *ent;

    if (dir != NULL) {
        while ( (ent = readdir(dir)) != NULL ) {
            imgName = ent->d_name;
            if (imgName.compare(".")!=0 && imgName.compare("..")!=0)
            {
                string aux;
                aux.append(folderpath);
                aux.append(imgName);
                cout << aux << endl;
                Mat img = imread(aux, -1);
                if (! img.data) {
                    cout << "Could not open or find the image" << endl;
                    return -1;
                }
                int height = img.rows;
                int width = img.cols;

                // subtract the average face
                Mat avg(height, width, CV_8UC3, Scalar(93.5940,104.7624,129.1863));
                // Mat mean_img(height, width, CV_8UC3, Scalar(90,100,120)); //also works
                Mat image = img - avg;
                vector<float> output = classifier.Predict(image);

                //for (vector<float>::const_iterator i = output.begin(); i != output.end(); ++i) {
                //    cout << *i << ' ';
                //}
            }
        }
    }

    return 0;
}


/*!
@file
Forward declares `boost::hana::minus`.

@copyright Louis Dionne 2013-2016
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_FWD_MINUS_HPP
#define BOOST_HANA_FWD_MINUS_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! Subtract two elements of a group.
    //! @ingroup group-Group
    //!
    //! Specifically, this performs the `Monoid` operation on the first
    //! argument and on the inverse of the second argument, thus being
    //! equivalent to:
    //! @code
    //!     minus(x, y) == plus(x, negate(y))
    //! @endcode
    //!
    //!
    //! Cross-type version of the method
    //! --------------------------------
    //! The `minus` method is "overloaded" to handle distinct data types
    //! with certain properties. Specifically, `minus` is defined for
    //! _distinct_ data types `A` and `B` such that
    //! 1. `A` and `B` share a common data type `C`, as determined by the
    //!    `common` metafunction
    //! 2. `A`, `B` and `C` are all `Group`s when taken individually
    //! 3. `to<C> : A -> B` and `to<C> : B -> C` are `Group`-embeddings, as
    //!    determined by the `is_embedding` metafunction.
    //!
    //! The definition of `minus` for data types satisfying the above
    //! properties is obtained by setting
    //! @code
    //!     minus(x, y) = minus(to<C>(x), to<C>(y))
    //! @endcode
    //!
    //!
    //! Example
    //! -------
    //! @include example/minus.cpp
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto minus = [](auto&& x, auto&& y) -> decltype(auto) {
        return tag-dispatched;
    };
#else
    template <typename T, typename U, typename = void>
    struct minus_impl : minus_impl<T, U, when<true>> { };

    struct minus_t {
        template <typename X, typename Y>
        constexpr decltype(auto) operator()(X&& x, Y&& y) const;
    };

    constexpr minus_t minus{};
#endif
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_MINUS_HPP


#include <fc/fwd_impl.hpp>
#include <boost/config.hpp>

#include "_elliptic_impl_pub.hpp"

/* used by mixed + openssl */

namespace fc { namespace ecc {
namespace detail {

public_key_impl::public_key_impl() BOOST_NOEXCEPT {
    _init_lib();
}

public_key_impl::public_key_impl(const public_key_impl& cpy) BOOST_NOEXCEPT {
    _init_lib();
    *this = cpy;
}

public_key_impl::public_key_impl(public_key_impl&& cpy) BOOST_NOEXCEPT {
    _init_lib();
    *this = cpy;
}

public_key_impl::~public_key_impl() BOOST_NOEXCEPT {
    free_key();
}

public_key_impl&
public_key_impl::operator=(const public_key_impl& pk) BOOST_NOEXCEPT {
    if(pk._key == nullptr) {
        free_key();
    }
    else if(_key == nullptr) {
        _key = EC_KEY_dup(pk._key);
    }
    else {
        EC_KEY_copy(_key, pk._key);
    }
    return *this;
}

public_key_impl&
public_key_impl::operator=(public_key_impl&& pk) BOOST_NOEXCEPT {
    if(this != &pk) {
        free_key();
        _key    = pk._key;
        pk._key = nullptr;
    }
    return *this;
}

void
public_key_impl::free_key() BOOST_NOEXCEPT {
    if(_key != nullptr) {
        EC_KEY_free(_key);
        _key = nullptr;
    }
}

// Perform ECDSA key recovery (see SEC1 4.1.6) for curves over (mod p)-fields
// recid selects which key is recovered
// if check is non-zero, additional checks are performed
int
public_key_impl::ECDSA_SIG_recover_key_GFp(EC_KEY* eckey, ECDSA_SIG* ecsig,
                                           const unsigned char* msg,
                                           int msglen, int recid, int check) {
    if(!eckey)
        FC_THROW_EXCEPTION(exception, "null key");

    int     ret = 0;
    BN_CTX* ctx = NULL;

    BIGNUM*   x     = NULL;
    BIGNUM*   e     = NULL;
    BIGNUM*   order = NULL;
    BIGNUM*   sor   = NULL;
    BIGNUM*   eor   = NULL;
    BIGNUM*   field = NULL;
    EC_POINT* R     = NULL;
    EC_POINT* O     = NULL;
    EC_POINT* Q     = NULL;
    BIGNUM*   rr    = NULL;
    BIGNUM*   zero  = NULL;
    int       n     = 0;
    int       i     = recid / 2;

    const EC_GROUP* group = EC_KEY_get0_group(eckey);
    if((ctx = BN_CTX_new()) == NULL) {
        ret = -1;
        goto err;
    }
    BN_CTX_start(ctx);
    order = BN_CTX_get(ctx);
    if(!EC_GROUP_get_order(group, order, ctx)) {
        ret = -2;
        goto err;
    }
    x = BN_CTX_get(ctx);
    if(!BN_copy(x, order)) {
        ret = -1;
        goto err;
    }
    if(!BN_mul_word(x, i)) {
        ret = -1;
        goto err;
    }
    if(!BN_add(x, x, ecsig->r)) {
        ret = -1;
        goto err;
    }
    field = BN_CTX_get(ctx);
    if(!EC_GROUP_get_curve_GFp(group, field, NULL, NULL, ctx)) {
        ret = -2;
        goto err;
    }
    if(BN_cmp(x, field) >= 0) {
        ret = 0;
        goto err;
    }
    if((R = EC_POINT_new(group)) == NULL) {
        ret = -2;
        goto err;
    }
    if(!EC_POINT_set_compressed_coordinates_GFp(group, R, x, recid % 2, ctx)) {
        ret = 0;
        goto err;
    }
    if(check) {
        if((O = EC_POINT_new(group)) == NULL) {
            ret = -2;
            goto err;
        }
        if(!EC_POINT_mul(group, O, NULL, R, order, ctx)) {
            ret = -2;
            goto err;
        }
        if(!EC_POINT_is_at_infinity(group, O)) {
            ret = 0;
            goto err;
        }
    }
    if((Q = EC_POINT_new(group)) == NULL) {
        ret = -2;
        goto err;
    }
    n = EC_GROUP_get_degree(group);
    e = BN_CTX_get(ctx);
    if(!BN_bin2bn(msg, msglen, e)) {
        ret = -1;
        goto err;
    }
    if(8 * msglen > n)
        BN_rshift(e, e, 8 - (n & 7));
    zero = BN_CTX_get(ctx);
    if(!BN_zero(zero)) {
        ret = -1;
        goto err;
    }
    if(!BN_mod_sub(e, zero, e, order, ctx)) {
        ret = -1;
        goto err;
    }
    rr = BN_CTX_get(ctx);
    if(!BN_mod_inverse(rr, ecsig->r, order, ctx)) {
        ret = -1;
        goto err;
    }
    sor = BN_CTX_get(ctx);
    if(!BN_mod_mul(sor, ecsig->s, rr, order, ctx)) {
        ret = -1;
        goto err;
    }
    eor = BN_CTX_get(ctx);
    if(!BN_mod_mul(eor, e, rr, order, ctx)) {
        ret = -1;
        goto err;
    }
    if(!EC_POINT_mul(group, Q, eor, R, sor, ctx)) {
        ret = -2;
        goto err;
    }
    if(!EC_KEY_set_public_key(eckey, Q)) {
        ret = -2;
        goto err;
    }

    ret = 1;

err:
    if(ctx) {
        BN_CTX_end(ctx);
        BN_CTX_free(ctx);
    }
    if(R != NULL)
        EC_POINT_free(R);
    if(O != NULL)
        EC_POINT_free(O);
    if(Q != NULL)
        EC_POINT_free(Q);
    return ret;
}
}  // namespace detail

public_key::public_key() {}

public_key::public_key(const public_key& pk)
    : my(pk.my) {}

public_key::public_key(public_key&& pk)
    : my(std::move(pk.my)) {}

public_key::~public_key() {}

public_key&
public_key::operator=(public_key&& pk) {
    my = std::move(pk.my);
    return *this;
}

public_key&
public_key::operator=(const public_key& pk) {
    my = pk.my;
    return *this;
}

bool
public_key::valid() const {
    return my->_key != nullptr;
}

/* WARNING! This implementation is broken, it is actually equivalent to
     * public_key::add()!
     */
//    public_key public_key::mult( const fc::sha256& digest ) const
//    {
//        // get point from this public key
//        const EC_POINT* master_pub   = EC_KEY_get0_public_key( my->_key );
//        ec_group group(EC_GROUP_new_by_curve_name(NID_secp256k1));
//
//        ssl_bignum z;
//        BN_bin2bn((unsigned char*)&digest, sizeof(digest), z);
//
//        // multiply by digest
//        ssl_bignum one;
//        BN_one(one);
//        bn_ctx ctx(BN_CTX_new());
//
//        ec_point result(EC_POINT_new(group));
//        EC_POINT_mul(group, result, z, master_pub, one, ctx);
//
//        public_key rtn;
//        rtn.my->_key = EC_KEY_new_by_curve_name( NID_secp256k1 );
//        EC_KEY_set_public_key(rtn.my->_key,result);
//
//        return rtn;
//    }
public_key
public_key::add(const fc::sha256& digest) const {
    try {
        ec_group group(EC_GROUP_new_by_curve_name(NID_secp256k1));
        bn_ctx   ctx(BN_CTX_new());

        fc::bigint digest_bi((char*)&digest, sizeof(digest));

        ssl_bignum order;
        EC_GROUP_get_order(group, order, ctx);
        if(digest_bi > fc::bigint(order)) {
            FC_THROW_EXCEPTION(exception, "digest > group order");
        }

        public_key      digest_key   = private_key::regenerate(digest).get_public_key();
        const EC_POINT* digest_point = EC_KEY_get0_public_key(digest_key.my->_key);

        // get point from this public key
        const EC_POINT* master_pub = EC_KEY_get0_public_key(my->_key);

        //        ssl_bignum z;
        //        BN_bin2bn((unsigned char*)&digest, sizeof(digest), z);

        // multiply by digest
        //        ssl_bignum one;
        //        BN_one(one);

        ec_point result(EC_POINT_new(group));
        EC_POINT_add(group, result, digest_point, master_pub, ctx);

        if(EC_POINT_is_at_infinity(group, result)) {
            FC_THROW_EXCEPTION(exception, "point at  infinity");
        }

        public_key rtn;
        rtn.my->_key = EC_KEY_new_by_curve_name(NID_secp256k1);
        EC_KEY_set_public_key(rtn.my->_key, result);
        return rtn;
    }
    FC_RETHROW_EXCEPTIONS(debug, "digest: ${digest}", ("digest", digest));
}

std::string
public_key::to_base58() const {
    public_key_data key = serialize();
    return to_base58(key);
}

//    signature private_key::sign( const fc::sha256& digest )const
//    {
//        unsigned int buf_len = ECDSA_size(my->_key);
////        fprintf( stderr, "%d  %d\n", buf_len, sizeof(sha256) );
//        signature sig;
//        assert( buf_len == sizeof(sig) );
//
//        if( !ECDSA_sign( 0,
//                    (const unsigned char*)&digest, sizeof(digest),
//                    (unsigned char*)&sig, &buf_len, my->_key ) )
//        {
//            FC_THROW_EXCEPTION( exception, "signing error" );
//        }
//
//
//        return sig;
//    }
//    bool       public_key::verify( const fc::sha256& digest, const fc::ecc::signature& sig )
//    {
//      return 1 == ECDSA_verify( 0, (unsigned char*)&digest, sizeof(digest), (unsigned char*)&sig, sizeof(sig), my->_key );
//    }

public_key_data
public_key::serialize() const {
    public_key_data dat;
    if(!my->_key)
        return dat;
    EC_KEY_set_conv_form(my->_key, POINT_CONVERSION_COMPRESSED);
    /*size_t nbytes = i2o_ECPublicKey( my->_key, nullptr ); */
    /*assert( nbytes == 33 )*/
    char* front = &dat.data[0];
    i2o_ECPublicKey(my->_key, (unsigned char**)&front);  // FIXME: questionable memory handling
    return dat;
    /*
       EC_POINT* pub   = EC_KEY_get0_public_key( my->_key );
       EC_GROUP* group = EC_KEY_get0_group( my->_key );
       EC_POINT_get_affine_coordinates_GFp( group, pub, self.my->_pub_x.get(), self.my->_pub_y.get(), nullptr );
       */
}
public_key_point_data
public_key::serialize_ecc_point() const {
    public_key_point_data dat;
    if(!my->_key)
        return dat;
    EC_KEY_set_conv_form(my->_key, POINT_CONVERSION_UNCOMPRESSED);
    char* front = &dat.data[0];
    i2o_ECPublicKey(my->_key, (unsigned char**)&front);  // FIXME: questionable memory handling
    return dat;
}

public_key::public_key(const public_key_point_data& dat) {
    const char* front = &dat.data[0];
    if(*front == 0) {
    }
    else {
        my->_key = EC_KEY_new_by_curve_name(NID_secp256k1);
        my->_key = o2i_ECPublicKey(&my->_key, (const unsigned char**)&front, sizeof(dat));
        if(!my->_key) {
            FC_THROW_EXCEPTION(exception, "error decoding public key", ("s", ERR_error_string(ERR_get_error(), nullptr)));
        }
    }
}
public_key::public_key(const public_key_data& dat) {
    const char* front = &dat.data[0];
    if(*front == 0) {
    }
    else {
        my->_key = EC_KEY_new_by_curve_name(NID_secp256k1);
        my->_key = o2i_ECPublicKey(&my->_key, (const unsigned char**)&front, sizeof(public_key_data));
        if(!my->_key) {
            FC_THROW_EXCEPTION(exception, "error decoding public key", ("s", ERR_error_string(ERR_get_error(), nullptr)));
        }
    }
}

//    bool       private_key::verify( const fc::sha256& digest, const fc::ecc::signature& sig )
//    {
//      return 1 == ECDSA_verify( 0, (unsigned char*)&digest, sizeof(digest), (unsigned char*)&sig, sizeof(sig), my->_key );
//    }

public_key::public_key(const compact_signature& c, const fc::sha256& digest, bool check_canonical) {
    int nV = c.data[0];
    if(nV < 27 || nV >= 35)
        FC_THROW_EXCEPTION(exception, "unable to reconstruct public key from signature");

    ECDSA_SIG* sig = ECDSA_SIG_new();
    BN_bin2bn(&c.data[1], 32, sig->r);
    BN_bin2bn(&c.data[33], 32, sig->s);

    if(check_canonical) {
        FC_ASSERT(is_canonical(c), "signature is not canonical");
    }

    my->_key = EC_KEY_new_by_curve_name(NID_secp256k1);

    if(nV >= 31) {
        EC_KEY_set_conv_form(my->_key, POINT_CONVERSION_COMPRESSED);
        nV -= 4;
        //            fprintf( stderr, "compressed\n" );
    }

    if(detail::public_key_impl::ECDSA_SIG_recover_key_GFp(my->_key, sig, (unsigned char*)&digest, sizeof(digest), nV - 27, 0) == 1) {
        ECDSA_SIG_free(sig);
        return;
    }
    ECDSA_SIG_free(sig);
    FC_THROW_EXCEPTION(exception, "unable to reconstruct public key from signature");
}
}}  // namespace fc::ecc


#pragma once

#include "util.hxx"
#include "vec3f.hxx"
#include "box.hxx"
#include "span.hxx"
#include "json_fwd.hxx"
#include "ray.hxx"

#include <boost/container/static_vector.hpp>
#include <numeric>

namespace guiding
{

namespace kdtree
{

inline static constexpr int MAX_DEPTH = 20;

struct AdaptParams
{
  std::uint64_t max_num_points = std::numeric_limits<std::uint64_t>::max();
};

static constexpr int MAX_NODES = (1<<30)-1;

struct Node
{
  double split_pos = std::numeric_limits<double>::quiet_NaN();
  std::uint64_t
    split_axis : 2,
    left_is_leaf : 1,
    right_is_leaf : 1,
    left_idx : 30,
    right_idx : 30;
};

static_assert(sizeof(Node) == 16);

struct Handle
{
  int idx = -1;
  bool is_leaf = false;
};

class Tree
{
public:
  using Handle = guiding::kdtree::Handle;

private:
  friend class TreeAdaptor;
  template <class, class> friend class Builder;
  ToyVector<Node> storage;
  Handle root;
  int num_leafs = 0;

  Handle AllocateLeaf()
  {
    assert (num_leafs < MAX_NODES);
    return Handle{ num_leafs++, true };
  }

  Handle AllocateBranch(Handle left, Handle right, int axis, double pos)
  {
    Node nd;
    nd.split_pos = pos;
    nd.split_axis = axis;
    nd.left_is_leaf = left.is_leaf;
    nd.right_is_leaf = right.is_leaf;
    nd.left_idx = left.idx;
    nd.right_idx = right.idx;
    storage.push_back(nd);
    return { static_cast<int>(storage.size()-1), false };
  }

  struct TagUninitialized {};

  explicit Tree(TagUninitialized)
  {
  }

public:
  Tree() :
    Tree(TagUninitialized{})
  {
    root = AllocateLeaf();
  }

  Tree(const Tree &) = delete;
  Tree& operator=(const Tree&) = delete;

  Tree(Tree &&) = default;
  Tree& operator=(Tree &&) = default;

  int NumLeafs() const { return num_leafs; }

  Handle GetRoot() const
  {
    return root;
  }

  auto Children(Handle node) const
  {
    assert (!node.is_leaf);
    const auto& b = storage[node.idx];
    return std::make_pair(Handle{static_cast<int>(b.left_idx), static_cast<bool>(b.left_is_leaf)},
                          Handle{static_cast<int>(b.right_idx), static_cast<bool>(b.right_is_leaf)});
  }

  auto Split(Handle node) const
  {
    assert (!node.is_leaf);
    const auto& b = storage[node.idx];
    return std::make_pair(static_cast<int>(b.split_axis), b.split_pos);
  }

  int Lookup(const Double3 &p)
  {
    Handle current = root;
    while (true)
    {
      if (current.is_leaf)
      {
        return current.idx;
      }
      else
      {
        const auto& b = storage[current.idx];
        const bool go_left = p[b.split_axis] < b.split_pos;
        auto [left, right] = Children(current);
        current = go_left ? left : right;
      }
    }
  }

  int Lookup(const Double3 &p) const
  {
    return const_cast<Tree*>(this)->Lookup(p);
  }

#ifdef HAVE_JSON
  void DumpTo(rapidjson::Document &doc, rapidjson::Value & parent) const;
#endif
};

class TreeAdaptor
{
public:
  using SplitDecision = std::function<std::pair<int, double>(int)>;

  TreeAdaptor(SplitDecision split_decision_) :
    split_decision{split_decision_}
  {
  }

  Tree Adapt(Tree &tree)
  {
    this->tree = &tree;
    new_tree.root = AdaptRecursive(tree.root, 1);
    Tree tmp{Tree::TagUninitialized()};
    std::swap(tmp, new_tree);
    return tmp;
  }

  struct OldToNew
  {
    //int old = -1;
    int new_first = -1;
    int new_second = -1;
  };

  Span<const OldToNew> GetNodeMappings() const
  {
    return AsSpan(node_mappings);
  }

private:
  SplitDecision split_decision;
  Tree *tree = nullptr;
  Tree new_tree{Tree::TagUninitialized()};
  ToyVector<OldToNew> node_mappings;

  void AddToTranslationMap(Tree::Handle old, Tree::Handle new_first, Tree::Handle new_second)
  {
    node_mappings.push_back({
      /*old.idx,*/ new_first.idx, new_second.idx
    });
    assert(node_mappings.size() == old.idx+1);
    assert(old.is_leaf);
  }

  // Axis and location
  std::pair<int, double> DetermineSplit(Handle p)
  {
    assert (p.is_leaf);
    return split_decision(p.idx);
  }

  Handle AdaptLeafRecursive(Tree::Handle node, int depth)
  {
    if (auto [axis, pos] = DetermineSplit(node); axis>=0 && depth<MAX_DEPTH)
    {
      auto left = new_tree.AllocateLeaf();
      auto right = new_tree.AllocateLeaf();
      auto branch = new_tree.AllocateBranch(left, right, axis, pos);
      AddToTranslationMap(node, left, right);
      return branch;
    }
    else
    {
      auto clone = new_tree.AllocateLeaf();
      AddToTranslationMap(node, clone, {});
      return clone;
    }
  }

  Handle AdaptBranchRecursive(Tree::Handle node, int depth)
  {
    auto [old_left, old_right] = tree->Children(node);
    auto [axis, pos] = tree->Split(node);
    auto left = AdaptRecursive(old_left, depth+1);
    auto right = AdaptRecursive(old_right, depth+1);
    auto branch = new_tree.AllocateBranch(left, right, axis, pos);
    return branch;
  }

  Handle AdaptRecursive(Tree::Handle node, int depth)
  {
    return node.is_leaf ? AdaptLeafRecursive(node, depth) : AdaptBranchRecursive(node, depth);
  }
};

class LeafIterator
{
  using H = Tree::Handle;
  const Tree* tree;
  Ray ray;

  struct Entry
  {
    H node;
    double tnear;
    double tfar;
  };

  boost::container::static_vector<Entry, MAX_DEPTH> stack;

public:
  LeafIterator(const Tree &tree_, const Ray &ray_, double tnear_init, double tfar_init)  noexcept
    : tree{&tree_}, ray{ray_}
    {
      stack.push_back({tree->GetRoot(), tnear_init, tfar_init});
      DecentToNextLeaf();
    }

  void operator++()  noexcept
  {
    stack.pop_back();
    if (!stack.empty())
      DecentToNextLeaf();
  }

  operator bool() const  noexcept
  {
    return !stack.empty();
  }

  struct ReturnValue {
    int idx;
    double tnear, tfar;
  };

  ReturnValue operator*() const  noexcept
  {
    auto e = stack.back();
    return ReturnValue{e.node.idx, e.tnear, e.tfar};
  }

  std::pair<double, double> Interval() const  noexcept
  {
    const auto& e = stack.back();
    return std::make_pair(e.tnear, e.tfar);
  }

  int Payload() const  noexcept
  {
    const auto& e = stack.back();
    return e.node.idx;
  }

private:

  void DecentToNextLeaf() noexcept;
};

template<class Point, class GetCoordinates>
class Builder
{
  int max_depth, min_num_points;
  Tree new_tree{ Tree::TagUninitialized() };
  GetCoordinates get_coordinates;
  ToyVector <Span<Point>> leaf_ranges;

  auto MakeCompareCoordinates(int axis) const
  {
    auto compare = [this, axis](const Point &a, const Point &b)
    {
      return get_coordinates(a)[axis] < get_coordinates(b)[axis];
    };

    return compare;
  }

  Eigen::Array<double, 3, 2> ComputeBounds(const Span<Point> pts) const
  {
    Eigen::Array<double, 3, 2> result;
    for (int axis = 0; axis < 3; ++axis)
    {
      auto[imin, imax] = std::minmax_element(pts.begin(), pts.end(), MakeCompareCoordinates(axis));
      const double pmin = get_coordinates(*imin)[axis];
      const double pmax = get_coordinates(*imax)[axis];
      result(axis, 0) = pmin;
      result(axis, 1) = pmax;
    }
    return result;
  }

  Handle BuildRecursive(Span<Point> points, int depth)
  {
    if (points.size() < 2*min_num_points || depth >= max_depth)
    {
      return BuildLeaf(points);
    }
    else
    {
      return TryBuildBranchRecursive(points, depth);
    }
  }

  Handle BuildLeaf(Span<Point> points)
  {
      Handle h = new_tree.AllocateLeaf();
      assert(h.idx == isize(leaf_ranges));
      leaf_ranges.push_back(points);
      return h;
  }

  static double AverageIfNonDegenerate(double a, double b)
  {
    assert (a < b);
    double x = 0.5*(a+b);
    // Use b because taking a would be an error.
    return x==a ? b : x;
  }

  /* Returns
       Coordinate for which all elements in the left interval are smaller.
       Size of the left interval.
  */
  std::tuple<double, int> SplitRange(Span<Point> pts, int axis)
  {
    const auto n = pts.size();
    if (n <= 0)
      return { NaN, 0 };

    const auto n_split_guess = n/2;
    std::nth_element(
      pts.begin(),
      pts.begin()+n_split_guess,
      pts.end(),
      MakeCompareCoordinates(axis));

    return {
      get_coordinates(pts[n_split_guess])[axis],
      n_split_guess
    };
  }

  Handle TryBuildBranchRecursive(Span<Point> branch_points, int depth)
  {
    const Eigen::Array<double, 3, 2> bounds = ComputeBounds(branch_points);

    int axis = -1;
    (bounds.col(1) - bounds.col(0)).maxCoeff(&axis);
    assert(axis >= 0);

    const auto n = branch_points.size();
    assert(n >= 2*min_num_points);

    auto [pos, count_left] = SplitRange(branch_points, axis);
    const auto count_right = n - count_left;

    //std::cout << "split axis " << axis << " range " << bounds.row(axis) << " count " << branch_points.size() << " nleft " << count_left << std::endl;

    if (count_left >= min_num_points && count_right >= min_num_points)
    {
      auto left_range = Subspan(branch_points, 0, count_left);
      auto right_range = Subspan(branch_points, count_left, count_right);
      Handle left = BuildRecursive(left_range, depth+1);
      Handle right = BuildRecursive(right_range, depth+1);
      auto branch = new_tree.AllocateBranch(left, right, axis, pos);
      return branch;
    }
    else
    {
      Handle leaf = BuildLeaf(branch_points);
      return leaf;
    }
  }

public:
  Builder(int max_depth, int min_num_points, GetCoordinates get_coordinates)
    : max_depth{ max_depth }, min_num_points{ min_num_points }, get_coordinates{ get_coordinates }
  {
    assert (max_depth > 0);
  }

  Tree Build(Span<Point> points)
  {
    new_tree.root = BuildRecursive(points, 1);
    return std::move(new_tree);
  }

  Span<Point> DataRangeOfLeaf(int idx) const
  {
    return leaf_ranges[idx];
  }
};

template<class Point, class GetCoordinates>
inline Builder<Point, GetCoordinates> MakeBuilder(int max_depth, int min_num_points, GetCoordinates get_coordinates)
{
  return Builder<Point, GetCoordinates>(max_depth, min_num_points, get_coordinates);
}

} // kdtree

} // guiding


/*
 Copyright (C) 2018 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <ored/marketdata/commoditycurve.hpp>
#include <ored/utilities/conventionsbasedfutureexpiry.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/wildcard.hpp>
#include <qle/termstructures/averagefuturepricehelper.hpp>
#include <qle/termstructures/averageoffpeakpowerhelper.hpp>
#include <qle/termstructures/averagespotpricehelper.hpp>
#include <qle/termstructures/commodityaveragebasispricecurve.hpp>
#include <qle/termstructures/commoditybasispricecurve.hpp>
#include <qle/termstructures/crosscurrencypricetermstructure.hpp>
#include <qle/termstructures/futurepricehelper.hpp>
#include <qle/termstructures/iterativebootstrap.hpp>
#include <qle/termstructures/piecewisepricecurve.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/time/date.hpp>

using QuantExt::AverageFuturePriceHelper;
using QuantExt::AverageOffPeakPowerHelper;
using QuantExt::AverageSpotPriceHelper;
using QuantExt::FutureExpiryCalculator;
using QuantExt::FuturePriceHelper;
using QuantExt::CommodityAverageBasisPriceCurve;
using QuantExt::CommodityBasisPriceCurve;
using QuantExt::CommodityIndex;
using QuantExt::CrossCurrencyPriceTermStructure;
using QuantExt::CubicFlat;
using QuantExt::InterpolatedPriceCurve;
using QuantExt::LinearFlat;
using QuantExt::LogLinearFlat;
using QuantExt::PriceTermStructure;
using QuantExt::PiecewisePriceCurve;
using QuantLib::BootstrapHelper;
using std::make_pair;
using std::map;
using std::string;

// Explicit template instantiation to avoid "error C2079: ... uses undefined class ..."
// Explained in the answer to the SO question here:
// https://stackoverflow.com/a/57666066/1771882
// Needs to be in global namespace also i.e. not under ore::data
// https://stackoverflow.com/a/25594741/1771882
template class QuantExt::PiecewisePriceCurve<QuantLib::Linear, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantLib::LogLinear, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantLib::Cubic, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantExt::LinearFlat, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantExt::LogLinearFlat, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantExt::CubicFlat, QuantExt::IterativeBootstrap>;
template class QuantExt::PiecewisePriceCurve<QuantLib::BackwardFlat, QuantExt::IterativeBootstrap>;

namespace {

using ore::data::Convention;
using ore::data::Conventions;
using QuantLib::Date;
using QuantLib::Error;
using QuantLib::io::iso_date;
using QuantLib::Real;

void addMarketFixing(const string& idxConvId, const Date& expiry, Real value, const Conventions& conventions) {
    auto p = conventions.get(idxConvId, Convention::Type::CommodityFuture);
    if (p.first) {
        auto idx = parseCommodityIndex(idxConvId, conventions, false);
        idx = idx->clone(expiry);
        if (idx->isValidFixingDate(expiry)) {
            try {
                idx->addFixing(expiry, value);
                TLOG("Added fixing (" << iso_date(expiry) << "," << idx->name() << "," << value << ").");
            } catch (const Error& e) {
                TLOG("Failed to add fixing (" << iso_date(expiry) << "," << idx->name() << "," <<
                    value << "): " << e.what());
            }
        } else {
            TLOG("Failed to add fixing (" << iso_date(expiry) << "," << idx->name() << "," <<
                value << ") because " << iso_date(expiry) << " is not a valid fixing date.");
        }
    } else {
        TLOG("Failed to add fixing because no commodity future convention for " << idxConvId << ".");
    }
}

}

namespace ore {
namespace data {

CommodityCurve::CommodityCurve()
    : commoditySpot_(Null<Real>()), onValue_(Null<Real>()), tnValue_(Null<Real>()), regexQuotes_(false) {}

CommodityCurve::CommodityCurve(const Date& asof, const CommodityCurveSpec& spec, const Loader& loader,
                               const CurveConfigurations& curveConfigs, const Conventions& conventions,
                               const FXTriangulation& fxSpots,
                               const map<string, boost::shared_ptr<YieldCurve>>& yieldCurves,
                               const map<string, boost::shared_ptr<CommodityCurve>>& commodityCurves)
    : spec_(spec), commoditySpot_(Null<Real>()), onValue_(Null<Real>()), tnValue_(Null<Real>()), regexQuotes_(false) {

    try {

        boost::shared_ptr<CommodityCurveConfig> config = curveConfigs.commodityCurveConfig(spec_.curveConfigID());

        dayCounter_ = config->dayCountId() == "" ? Actual365Fixed() : parseDayCounter(config->dayCountId());
        interpolationMethod_ = config->interpolationMethod() == "" ? "Linear" : config->interpolationMethod();

        if (config->type() == CommodityCurveConfig::Type::Direct) {

            // Populate the raw price curve data
            map<Date, Handle<Quote>> data;
            populateData(data, asof, config, loader, conventions);

            // Create the commodity price curve
            buildCurve(asof, data, config);

        } else if (config->type() == CommodityCurveConfig::Type::Basis) {

            // We have a commodity basis configuration

            // Look up the required base price curve in the commodityCurves map
            CommodityCurveSpec ccSpec(config->currency(), config->basePriceCurveId());
            DLOG("Looking for base price curve with id, " << config->basePriceCurveId() << ", and spec, " << ccSpec
                                                          << ".");
            auto itCc = commodityCurves.find(ccSpec.name());
            QL_REQUIRE(itCc != commodityCurves.end(), "Can't find price curve with id " << config->basePriceCurveId());
            auto pts = Handle<PriceTermStructure>(itCc->second->commodityPriceCurve());

            buildBasisPriceCurve(asof, *config, conventions, pts, loader);

        } else if (config->type() == CommodityCurveConfig::Type::Piecewise) {

            // We have a piecewise commodity configuration
            buildPiecewiseCurve(asof, *config, conventions, loader, commodityCurves);

        } else {

            // We have a cross currency type commodity curve configuration
            boost::shared_ptr<CommodityCurveConfig> baseConfig =
                curveConfigs.commodityCurveConfig(config->basePriceCurveId());

            buildCrossCurrencyPriceCurve(asof, config, baseConfig, fxSpots, yieldCurves, commodityCurves);
        }

        // Apply extrapolation from the curve configuration
        commodityPriceCurve_->enableExtrapolation(config->extrapolation());

        // Ask for price now so that errors are thrown during the build, not later.
        commodityPriceCurve_->price(asof + 1 * Days);

    } catch (std::exception& e) {
        QL_FAIL("commodity curve building failed: " << e.what());
    } catch (...) {
        QL_FAIL("commodity curve building failed: unknown error");
    }
}

void CommodityCurve::populateData(map<Date, Handle<Quote>>& data, const Date& asof,
                                  const boost::shared_ptr<CommodityCurveConfig>& config, const Loader& loader,
                                  const Conventions& conventions) {

    // Some default conventions for building the commodity curve
    Period spotTenor = 2 * Days;
    Real pointsFactor = 1.0;
    Calendar cal = parseCalendar(config->currency());
    bool spotRelative = true;
    BusinessDayConvention bdc = Following;
    bool outright = true;

    // Overwrite the default conventions if the commodity curve config provides explicit conventions
    if (!config->conventionsId().empty()) {
        QL_REQUIRE(conventions.has(config->conventionsId()),
                   "Commodity conventions " << config->conventionsId() << " requested by commodity config "
                                            << config->curveID() << " not found");
        auto convention =
            boost::dynamic_pointer_cast<CommodityForwardConvention>(conventions.get(config->conventionsId()));
        QL_REQUIRE(convention, "Convention " << config->conventionsId() << " not of expected type CommodityConvention");

        spotTenor = convention->spotDays() * Days;
        pointsFactor = convention->pointsFactor();
        if (convention->advanceCalendar() != NullCalendar())
            cal = convention->advanceCalendar();
        spotRelative = convention->spotRelative();
        bdc = convention->bdc();
        outright = convention->outright();
    }

    // Commodity spot quote if provided by the configuration
    Date spotDate = cal.advance(asof, spotTenor);
    if (!config->commoditySpotQuoteId().empty()) {
        auto spot = loader.get(config->commoditySpotQuoteId(), asof)->quote();
        commoditySpot_ = spot->value();
        data[spotDate] = spot;
    } else {
        QL_REQUIRE(outright, "If the commodity forward quotes are not outright,"
                                 << " a commodity spot quote needs to be configured");
    }

    // Add the forward quotes to the curve data
    for (auto& q : getQuotes(asof, config->curveID(), config->fwdQuotes(), loader)) {

        // We add ON and TN quotes after this loop if they are given and not outright quotes
        TLOG("Commodity Forward Price found for quote: " << q->name());
        Date expiry;
        Real value = q->quote()->value();
        if (!q->tenorBased()) {
            expiry = q->expiryDate();
            add(asof, expiry, value, data, outright, pointsFactor);
        } else {
            if (q->startTenor() == boost::none) {
                expiry = cal.advance(spotRelative ? spotDate : asof, q->tenor(), bdc);
                add(asof, expiry, value, data, outright, pointsFactor);
            } else {
                if (*q->startTenor() == 0 * Days && q->tenor() == 1 * Days) {
                    onValue_ = q->quote()->value();
                    if (outright)
                        add(asof, asof, value, data, outright);
                } else if (*q->startTenor() == 1 * Days && q->tenor() == 1 * Days) {
                    tnValue_ = q->quote()->value();
                    if (outright) {
                        expiry = cal.advance(asof, 1 * Days, bdc);
                        add(asof, expiry, value, data, outright);
                    }
                } else {
                    expiry = cal.advance(cal.advance(asof, *q->startTenor(), bdc), q->tenor(), bdc);
                    add(asof, expiry, value, data, outright, pointsFactor);
                }
            }
        }
    }

    // Deal with ON and TN if quotes are not outright quotes
    if (spotTenor == 2 * Days && tnValue_ != Null<Real>() && !outright) {
        add(asof, cal.advance(asof, 1 * Days, bdc), -tnValue_, data, outright, pointsFactor);
        if (onValue_ != Null<Real>()) {
            add(asof, asof, -onValue_ - tnValue_, data, outright, pointsFactor);
        }
    }

    // Some logging and checks
    LOG("Read " << data.size() << " quotes for commodity curve " << config->curveID());
    if (!regexQuotes_) {
        QL_REQUIRE(data.size() == config->quotes().size(), "Found " << data.size() << " quotes, but "
                                                                    << config->quotes().size()
                                                                    << " quotes given in config " << config->curveID());
    } else {
        QL_REQUIRE(data.size() > 0,
                   "Regular expression specified in commodity config " << config->curveID() << " but no quotes read");
    }
}

void CommodityCurve::add(const Date& asof, const Date& expiry, Real value, map<Date, Handle<Quote>>& data, bool outright,
                         Real pointsFactor) {

    if (expiry < asof)
        return;

    QL_REQUIRE(data.find(expiry) == data.end(), "Duplicate quote for expiry " << io::iso_date(expiry) << " found.");

    if (!outright) {
        QL_REQUIRE(commoditySpot_ != Null<Real>(), "Can't use forward points without a commodity spot value");
        value = commoditySpot_ + value / pointsFactor;
    }

    data[expiry] = Handle<Quote>(boost::make_shared<SimpleQuote>(value));
}

void CommodityCurve::buildCurve(const Date& asof, const map<Date, Handle<Quote>>& data,
                                const boost::shared_ptr<CommodityCurveConfig>& config) {

    vector<Date> curveDates;
    curveDates.reserve(data.size());
    vector<Handle<Quote>> curvePrices;
    curvePrices.reserve(data.size());
    for (auto const& datum : data) {
        curveDates.push_back(datum.first);
        curvePrices.push_back(datum.second);
    }

    // Build the curve using the data
    populateCurve<InterpolatedPriceCurve>(asof, curveDates, curvePrices, dayCounter_,
                                          parseCurrency(config->currency()));
}

void CommodityCurve::buildCrossCurrencyPriceCurve(
    const Date& asof, const boost::shared_ptr<CommodityCurveConfig>& config,
    const boost::shared_ptr<CommodityCurveConfig>& baseConfig, const FXTriangulation& fxSpots,
    const map<string, boost::shared_ptr<YieldCurve>>& yieldCurves,
    const map<string, boost::shared_ptr<CommodityCurve>>& commodityCurves) {

    // Look up the required base price curve in the commodityCurves map
    // We pass in the commodity curve ID only in the member basePriceCurveId of config e.g. PM:XAUUSD.
    // But, the map commodityCurves is keyed on the spec name e.g. Commodity/USD/PM:XAUUSD
    auto commIt = commodityCurves.find(CommodityCurveSpec(baseConfig->currency(), baseConfig->curveID()).name());
    QL_REQUIRE(commIt != commodityCurves.end(), "Could not find base commodity curve with id "
                                                    << baseConfig->curveID()
                                                    << " required in the building of commodity curve with id "
                                                    << config->curveID());

    // Look up the two yield curves in the yieldCurves map
    auto baseYtsIt = yieldCurves.find(YieldCurveSpec(baseConfig->currency(), config->baseYieldCurveId()).name());
    QL_REQUIRE(baseYtsIt != yieldCurves.end(),
               "Could not find base yield curve with id "
                   << config->baseYieldCurveId() << " and currency " << baseConfig->currency()
                   << " required in the building of commodity curve with id " << config->curveID());

    auto ytsIt = yieldCurves.find(YieldCurveSpec(config->currency(), config->yieldCurveId()).name());
    QL_REQUIRE(ytsIt != yieldCurves.end(), "Could not find yield curve with id "
                                               << config->yieldCurveId() << " and currency " << config->currency()
                                               << " required in the building of commodity curve with id "
                                               << config->curveID());

    // Get the FX spot rate, number of units of this currency per unit of base currency
    Handle<Quote> fxSpot = fxSpots.getQuote(baseConfig->currency() + config->currency());

    // Populate the commodityPriceCurve_ member
    commodityPriceCurve_ = boost::make_shared<CrossCurrencyPriceTermStructure>(
        asof, QuantLib::Handle<PriceTermStructure>(commIt->second->commodityPriceCurve()), fxSpot,
        baseYtsIt->second->handle(), ytsIt->second->handle(), parseCurrency(config->currency()));
}

void CommodityCurve::buildBasisPriceCurve(const Date& asof, const CommodityCurveConfig& config,
                                          const Conventions& conventions, const Handle<PriceTermStructure>& basePts,
                                          const Loader& loader) {

    LOG("CommodityCurve: start building commodity basis curve.");

    // We need to have commodity future conventions for both the base curve and the basis curve
    QL_REQUIRE(conventions.has(config.conventionsId()), "Commodity conventions " << config.conventionsId()
                                                                                << " requested by commodity config "
                                                                                << config.curveID() << " not found");
    auto basisConvention =
        boost::dynamic_pointer_cast<CommodityFutureConvention>(conventions.get(config.conventionsId()));
    QL_REQUIRE(basisConvention,
               "Convention " << config.conventionsId() << " not of expected type CommodityFutureConvention");
    auto basisFec = boost::make_shared<ConventionsBasedFutureExpiry>(*basisConvention);

    QL_REQUIRE(conventions.has(config.baseConventionsId()),
               "Commodity conventions " << config.baseConventionsId() << " requested by commodity config "
                                        << config.curveID() << " not found");
    auto baseConvention =
        boost::dynamic_pointer_cast<CommodityFutureConvention>(conventions.get(config.baseConventionsId()));
    QL_REQUIRE(baseConvention,
               "Convention " << config.baseConventionsId() << " not of expected type CommodityFutureConvention");
    auto baseFec = boost::make_shared<ConventionsBasedFutureExpiry>(*baseConvention);

    // Construct the commodity index.
    auto index = parseCommodityIndex(baseConvention->id(), conventions, false, basePts);

    // Sort the configured quotes on expiry dates
    // Ignore tenor based quotes i.e. we expect an explicit expiry date and log a warning if the expiry date does not
    // match our own calculated expiry date based on the basis conventions.
    map<Date, Handle<Quote>> basisData;
    for (auto& q : getQuotes(asof, config.curveID(), config.fwdQuotes(), loader, true)) {

        QL_REQUIRE(basisData.find(q->expiryDate()) == basisData.end(), "Found duplicate quote, "
                                                                           << q->name() << ", for expiry date "
                                                                           << io::iso_date(q->expiryDate()) << ".");

        basisData[q->expiryDate()] = q->quote();
        TLOG("Using quote " << q->name() << " in commodity basis curve.");

        // We expect the expiry date in the quotes to match our calculated expiry date. The code will work if it does
        // not but we log a warning in this case.
        Date calcExpiry = basisFec->nextExpiry(true, q->expiryDate());
        if (calcExpiry != q->expiryDate()) {
            WLOG("Calculated expiry date, " << io::iso_date(calcExpiry) << ", does not equal quote's expiry date "
                                            << io::iso_date(q->expiryDate()) << ".");
        }
    }

    if (basisConvention->isAveraging()) {
        // We are building a curve that will be used to return an average price.
        if (!baseConvention->isAveraging() && config.averageBase()) {
            DLOG("Creating a CommodityAverageBasisPriceCurve.");
            populateCurve<CommodityAverageBasisPriceCurve>(asof, basisData, basisFec, index,
                basePts, baseFec, config.addBasis());
        } else {
            // Either 1) base convention is not averaging and config.averageBase() is false or 2) the base convention
            // is averaging. Either way, we build a CommodityBasisPriceCurve.
            DLOG("Creating a CommodityBasisPriceCurve for an average price curve.");
            populateCurve<CommodityBasisPriceCurve>(asof, basisData, basisFec, index, basePts,
                baseFec, config.addBasis(), config.monthOffset());
        }
    } else {
        // We are building a curve that will be used to return a price on a single date.
        QL_REQUIRE(!baseConvention->isAveraging(), "A commodity basis curve with non-averaging" <<
            " basis and averaging base is not valid.");

        DLOG("Creating a CommodityBasisPriceCurve.");
        populateCurve<CommodityBasisPriceCurve>(asof, basisData, basisFec, index, basePts,
            baseFec, config.addBasis(), config.monthOffset());
    }

    LOG("CommodityCurve: finished building commodity basis curve.");
}

// Allow for more readable code in method below.
template<class I> using Crv = QuantExt::PiecewisePriceCurve<I, QuantExt::IterativeBootstrap>;
template<class C> using BS = QuantExt::IterativeBootstrap<C>;

void CommodityCurve::buildPiecewiseCurve(const Date& asof, const CommodityCurveConfig& config,
    const Conventions& conventions, const Loader& loader,
    const map<string, boost::shared_ptr<CommodityCurve>>& commodityCurves) {

    LOG("CommodityCurve: start building commodity piecewise curve.");

    // We store the instruments in a map. The key is the instrument's pillar date. The segments are ordered in
    // priority so if we encounter the same pillar date later, we ignore it with a debug log.
    map<Date, boost::shared_ptr<Helper>> mpInstruments;
    const auto& priceSegments = config.priceSegments();
    QL_REQUIRE(!priceSegments.empty(), "CommodityCurve: need at least one price segment to build piecewise curve.");
    for (const auto& kv : priceSegments) {
        if (kv.second.type() != PriceSegment::Type::OffPeakPowerDaily) {
            addInstruments(asof, loader, config.curveID(), config.currency(), kv.second,
                conventions, commodityCurves, mpInstruments);
        } else {
            addOffPeakPowerInstruments(asof, loader, config.curveID(), kv.second, conventions, mpInstruments);
        }
    }

    // Populate the vector of helpers.
    vector<boost::shared_ptr<Helper>> instruments;
    instruments.reserve(mpInstruments.size());
    for (const auto& kv : mpInstruments) {
        instruments.push_back(kv.second);
    }

    // Use bootstrap configuration if provided.
    BootstrapConfig bc;
    if (config.bootstrapConfig()) {
        bc = *config.bootstrapConfig();
    }
    Real acc = bc.accuracy();
    Real globalAcc = bc.globalAccuracy();
    bool noThrow = bc.dontThrow();
    Size maxAttempts = bc.maxAttempts();
    Real maxF = bc.maxFactor();
    Real minF = bc.minFactor();
    Size noThrowSteps = bc.dontThrowSteps();

    // Create curve based on interpolation method provided.
    Currency ccy = parseCurrency(config.currency());
    if (interpolationMethod_ == "Linear") {
        BS<Crv<Linear>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<Linear>>(asof, instruments, dayCounter_, ccy, Linear(), bs);
    } else if (interpolationMethod_ == "LogLinear") {
        BS<Crv<QuantLib::LogLinear>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<QuantLib::LogLinear>>(asof, instruments,
            dayCounter_, ccy, QuantLib::LogLinear(), bs);
    } else if (interpolationMethod_ == "Cubic") {
        BS<Crv<QuantLib::Cubic>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<QuantLib::Cubic>>(asof, instruments,
            dayCounter_, ccy, QuantLib::Cubic(), bs);
    } else if (interpolationMethod_ == "LinearFlat") {
        BS<Crv<QuantExt::LinearFlat>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<QuantExt::LinearFlat>>(asof, instruments,
            dayCounter_, ccy, QuantExt::LinearFlat(), bs);
    } else if (interpolationMethod_ == "LogLinearFlat") {
        BS<Crv<QuantExt::LogLinearFlat>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<QuantExt::LogLinearFlat>>(asof, instruments,
            dayCounter_, ccy, QuantExt::LogLinearFlat(), bs);
    } else if (interpolationMethod_ == "CubicFlat") {
        BS<Crv<QuantExt::CubicFlat>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<QuantExt::CubicFlat>>(asof, instruments,
            dayCounter_, ccy, QuantExt::CubicFlat(), bs);
    } else if (interpolationMethod_ == "BackwardFlat") {
        BS<Crv<BackwardFlat>> bs(acc, globalAcc, noThrow, maxAttempts, maxF, minF, noThrowSteps);
        commodityPriceCurve_ = boost::make_shared<Crv<BackwardFlat>>(asof, instruments,
            dayCounter_, ccy, BackwardFlat(), bs);
    } else {
        QL_FAIL("The interpolation method, " << interpolationMethod_ << ", is not supported.");
    }

    LOG("CommodityCurve: finished building commodity piecewise curve.");
}

vector<boost::shared_ptr<CommodityForwardQuote>>
CommodityCurve::getQuotes(const Date& asof, const string& configId, const vector<string>& quotes,
    const Loader& loader, bool filter) {

    LOG("CommodityCurve: start getting configured commodity quotes.");

    // Check if we are using a regular expression to select the quotes for the curve. If we are, the quotes should
    // contain exactly one element.
    auto wildcard = getUniqueWildcard(quotes);
    regexQuotes_ = wildcard != boost::none;

    // Add the relevant forward quotes to the result vector
    vector<boost::shared_ptr<CommodityForwardQuote>> result;
    for (auto& md : loader.loadQuotes(asof)) {

        // Only looking for quotes on asof date, with quote type PRICE and instrument type commodity forward
        if (md->asofDate() == asof && md->quoteType() == MarketDatum::QuoteType::PRICE &&
            md->instrumentType() == MarketDatum::InstrumentType::COMMODITY_FWD) {

            boost::shared_ptr<CommodityForwardQuote> q = boost::dynamic_pointer_cast<CommodityForwardQuote>(md);

            // Check if the quote is requested by the config and if it isn't continue to the next quote
            if (!wildcard) {
                vector<string>::const_iterator it =
                    find(quotes.begin(), quotes.end(), q->name());
                if (it == quotes.end())
                    continue;
            } else {
                if (!(*wildcard).matches(q->name()))
                    continue;
            }

            // If filter is true, remove tenor based quotes and quotes with expiry before asof.
            if (filter) {
                if (q->tenorBased()) {
                    TLOG("Skipping tenor based quote, " << q->name() << ".");
                    continue;
                }
                if (q->expiryDate() < asof) {
                    TLOG("Skipping quote because its expiry date, " << io::iso_date(q->expiryDate())
                        << ", is before the market date " << io::iso_date(asof));
                    continue;
                }
            }

            // If we make it here, the quote is relevant.
            result.push_back(q);
            TLOG("Added quote " << q->name() << ".");
        }
    }

    LOG("CommodityCurve: finished getting configured commodity quotes.");

    return result;
}

void CommodityCurve::addInstruments(const Date& asof, const Loader& loader, const string& configId,
    const string& currency, const PriceSegment& priceSegment, const Conventions& conventions,
    const map<string, boost::shared_ptr<CommodityCurve>>& commodityCurves,
    map<Date, boost::shared_ptr<Helper>>& instruments) {

    using PST = PriceSegment::Type;
    using AD = CommodityFutureConvention::AveragingData;
    PST type = priceSegment.type();

    // Pre-populate some variables if averaging segment.
    boost::shared_ptr<CommodityFutureConvention> convention;
    AD ad;
    boost::shared_ptr<CommodityIndex> index;
    boost::shared_ptr<FutureExpiryCalculator> uFec;
    if (type == PST::AveragingFuture || type == PST::AveragingSpot || type == PST::AveragingOffPeakPower) {

        // Get the associated averaging commodity future convention.
        convention = boost::dynamic_pointer_cast<CommodityFutureConvention>(
            conventions.get(priceSegment.conventionsId()));
        QL_REQUIRE(convention, "Convention " << priceSegment.conventionsId() <<
            " not of expected type CommodityFutureConvention.");

        ad = convention->averagingData();
        QL_REQUIRE(!ad.empty(), "CommodityCurve: convention " << convention->id() <<
            " should have non-empty averaging data for piecewise price curve construction.");

        // The commodity index for which we are building a price curve.
        index = parseCommodityIndex(ad.commodityName(), conventions, false);

        // If referencing a future, we need conventions for the underlying future that is being averaged.
        if (type == PST::AveragingFuture || type == PST::AveragingOffPeakPower) {

            auto uConvention = boost::dynamic_pointer_cast<CommodityFutureConvention>(
                conventions.get(ad.conventionsId()));
            QL_REQUIRE(uConvention, "Convention " << priceSegment.conventionsId() <<
                " not of expected type CommodityFutureConvention.");
            uFec = boost::make_shared<ConventionsBasedFutureExpiry>(*uConvention);

            if (ad.dailyExpiryOffset() != Null<Natural>() && ad.dailyExpiryOffset() > 0) {
                QL_REQUIRE(uConvention->contractFrequency() == Daily, "CommodityCurve: the averaging data has" <<
                    " a positive DailyExpiryOffset (" << ad.dailyExpiryOffset() << ") but the underlying future" <<
                    " contract frequency is not daily (" << uConvention->contractFrequency() << ").");
            }
        }

    }

    // Pre-populate some variables if the price segment is AveragingOffPeakPower.
    boost::shared_ptr<CommodityIndex> peakIndex;
    Natural peakHoursPerDay = 16;
    Calendar peakCalendar;
    if (type == PST::AveragingOffPeakPower) {

        // Look up the peak price curve in the commodityCurves map
        const string& ppId = priceSegment.peakPriceCurveId();
        QL_REQUIRE(!ppId.empty(), "CommodityCurve: AveragingOffPeakPower segment in " <<
            " curve configuration " << configId << " does not provide a peak price curve ID.");
        CommodityCurveSpec ccSpec(currency, ppId);
        DLOG("Looking for peak price curve with id, " << ppId << ", and spec, " << ccSpec << ".");
        auto itCc = commodityCurves.find(ccSpec.name());
        QL_REQUIRE(itCc != commodityCurves.end(), "Can't find peak price curve with id " << ppId);
        auto peakPts = Handle<PriceTermStructure>(itCc->second->commodityPriceCurve());

        // Create the daily peak price index linked to the peak price term structure.
        peakIndex = parseCommodityIndex(ppId, conventions, false, peakPts);

        // Calendar defining the peak business days.
        peakCalendar = parseCalendar(priceSegment.peakPriceCalendar());

        // Look up the conventions for the peak price commodity to determine peak hours per day.
        if (conventions.has(ppId)) {
            auto peakConvention = boost::dynamic_pointer_cast<CommodityFutureConvention>(conventions.get(ppId));
            if (peakConvention && peakConvention->hoursPerDay() != Null<Natural>()) {
                peakHoursPerDay = peakConvention->hoursPerDay();
            }
        }
    }

    // Get the relevant quotes
    auto quotes = getQuotes(asof, configId, priceSegment.quotes(), loader, true);

    // Add an instrument for each relevant quote.
    for (const auto& quote : quotes) {

        const Date& expiry = quote->expiryDate();
        switch (type) {
        case PST::Future:
            if (expiry == asof) {
                TLOG("Quote " << quote->name() << " has expiry date " << io::iso_date(expiry) << " equal to asof" <<
                    " so not adding to instruments. Attempt to add as fixing instead.");
                addMarketFixing(priceSegment.conventionsId(), expiry, quote->quote()->value(), conventions);
            } else if (instruments.count(expiry) == 0) {
                instruments[expiry] = boost::make_shared<FuturePriceHelper>(quote->quote(), expiry);
            } else {
                TLOG("Skipping quote, " << quote->name() << ", because its expiry date, " <<
                    io::iso_date(expiry) << ", is already in the instrument set.");
            }
            break;

        // An averaging future referencing an underlying future or spot. Setup is similar.
        case PST::AveragingFuture:
        case PST::AveragingSpot:
        case PST::AveragingOffPeakPower: {

            // Determine the calculation period.
            using ADCP = AD::CalculationPeriod;
            Date start;
            Date end;
            if (ad.period() == ADCP::ExpiryToExpiry) {

                auto fec = boost::make_shared<ConventionsBasedFutureExpiry>(*convention);
                end = fec->nextExpiry(true, expiry);
                if (end != expiry) {
                    WLOG("Calculated expiry date, " << io::iso_date(end) << ", does not equal quote's expiry date "
                        << io::iso_date(expiry) << ". Proceed with quote's expiry.");
                }
                start = fec->priorExpiry(false, end) + 1;

            } else if (ad.period() == ADCP::PreviousMonth) {

                end = Date::endOfMonth(expiry - 1 * Months);
                start = Date(1, end.month(), end.year());
            }

            boost::shared_ptr<Helper> helper;
            if (type == PST::AveragingOffPeakPower) {
                TLOG("Building average off-peak power helper from quote, " << quote->name() << ".");
                helper = boost::make_shared<AverageOffPeakPowerHelper>(quote->quote(), index, start,
                    end, uFec, peakIndex, peakCalendar, peakHoursPerDay);
            } else {
                TLOG("Building average future price helper from quote, " << quote->name() << ".");
                helper = boost::make_shared<AverageFuturePriceHelper>(quote->quote(), index, start, end, uFec,
                    ad.pricingCalendar(), ad.deliveryRollDays(), ad.futureMonthOffset(), ad.useBusinessDays(),
                    ad.dailyExpiryOffset());
            }

            // Only add to instruments if an instrument with the same pillar date is not there already.
            Date pillar = helper->pillarDate();
            if (instruments.count(pillar) == 0) {
                instruments[pillar] = helper;
            } else {
                TLOG("Skipping quote, " << quote->name() << ", because an instrument with its pillar date, " <<
                    io::iso_date(pillar) << ", is already in the instrument set.");
            }
            break;
        }

        default:
            QL_FAIL("CommodityCurve: unrecognised price segment type.");
            break;
        }
    }
}

void CommodityCurve::addOffPeakPowerInstruments(const Date& asof, const Loader& loader, const string& configId,
    const PriceSegment& priceSegment, const Conventions& conventions,
    map<Date, boost::shared_ptr<Helper>>& instruments) {

    // Check that we have been called with the expected segment type.
    using PST = PriceSegment::Type;
    QL_REQUIRE(priceSegment.type() == PST::OffPeakPowerDaily, "Expecting a price segment type of OffPeakPowerDaily.");

    // Check we have a commodity future convention for the price segment.
    const string& convId = priceSegment.conventionsId();
    auto p = conventions.get(convId, Convention::Type::CommodityFuture);
    QL_REQUIRE(p.first, "Could not get conventions with id " << convId << " for OffPeakPowerDaily price segment" <<
        " in curve configuration " << configId << ".");
    auto convention = boost::dynamic_pointer_cast<CommodityFutureConvention>(p.second);

    // Check that the commodity future convention has off-peak information for the name.
    const auto& oppIdxData = convention->offPeakPowerIndexData();
    QL_REQUIRE(oppIdxData, "Conventions with id " << convId << " for OffPeakPowerDaily price segment" <<
        " should have an OffPeakPowerIndexData section.");
    Real offPeakHours = oppIdxData->offPeakHours();
    TLOG("Off-peak hours is " << offPeakHours);
    const Calendar& peakCalendar = oppIdxData->peakCalendar();

    // Check that the price segment has off-peak daily section.
    const auto& opd = priceSegment.offPeakDaily();
    QL_REQUIRE(opd, "The OffPeakPowerDaily price segment for curve configuration " << configId <<
        " should have an OffPeakDaily section.");

    // Get all the peak and off-peak quotes that we have and store them in a map. The map key is the expiry date and
    // the map value is a pair of values the first being the off-peak value for that expiry and the second being the
    // peak value for that expiry. We only need the peak portion to form the quote on peakCalendar holidays. We need
    // the off-peak portion always.
    map<Date, pair<Real, Real>> quotes;

    auto opqs = getQuotes(asof, configId, opd->offPeakQuotes(), loader, true);
    for (const auto& q : opqs) {
        Real value = q->quote()->value();
        Date expiry = q->expiryDate();
        if (quotes.count(expiry) != 0) {
            TLOG("Already have off-peak quote with expiry " << io::iso_date(expiry) << " so skipping " << q->name());
        } else {
            TLOG("Adding off-peak quote " << q->name() << ": " << io::iso_date(expiry) << "," << value);
            quotes[expiry] = make_pair(value, Null<Real>());
        }
    }

    auto pqs = getQuotes(asof, configId, opd->peakQuotes(), loader, true);
    for (const auto& q : pqs) {
        Real value = q->quote()->value();
        Date expiry = q->expiryDate();
        auto it = quotes.find(expiry);
        if (it == quotes.end()) {
            TLOG("Have no off-peak quote with expiry " << io::iso_date(expiry) << " so skipping " << q->name());
        } else if (it->second.second != Null<Real>()) {
            TLOG("Already have a peak quote with expiry " << io::iso_date(expiry) << " so skipping " << q->name());
        } else {
            TLOG("Adding peak quote " << q->name() << ": " << io::iso_date(expiry) << "," << value);
            it->second.second = value;
        }
    }

    // Now, use the quotes to create the future instruments in the curve.
    for (const auto& kv : quotes) {

        // If the expiry is already in the instrument set, we skip it.
        const Date& expiry = kv.first;
        if (instruments.count(expiry) != 0) {
            TLOG("Skipping expiry " << io::iso_date(expiry) << " because it is already in the instrument set.");
            continue;
        }

        // If the expiry is equal to the asof, we add fixings.
        if (expiry == asof) {
            TLOG("The off-peak power expiry date " << io::iso_date(expiry) << " is equal to asof" <<
                " so not adding to instruments. Attempt to add fixing(s) instead.");
            if (peakCalendar.isHoliday(expiry) && kv.second.second == Null<Real>()) {
                DLOG("The peak portion of the quote on holiday " << io::iso_date(expiry) <<
                    " is missing so can't add fixings.");
            } else {
                // Add the off-peak and if necessary peak fixing
                addMarketFixing(oppIdxData->offPeakIndex(), expiry, kv.second.first, conventions);
                if (peakCalendar.isHoliday(expiry))
                    addMarketFixing(oppIdxData->peakIndex(), expiry, kv.second.second, conventions);
            }
            continue;
        }

        // Determine the quote that we will use in the future instrument for this expiry.
        Real quote = 0.0;
        if (peakCalendar.isHoliday(expiry)) {
            Real peakValue = kv.second.second;
            if (peakValue == Null<Real>()) {
                DLOG("The peak portion of the quote on holiday " << io::iso_date(expiry) << " is missing so skip.");
                continue;
            } else {
                Real offPeakValue = kv.second.first;
                quote = (offPeakHours * offPeakValue + (24.0 - offPeakHours) * peakValue) / 24.0;
                TLOG("The quote on holiday " << io::iso_date(expiry) << " is " << quote << ". (off-peak,peak) is" <<
                    " (" << offPeakValue << "," << peakValue << ").");
            }
        } else {
            quote = kv.second.first;
            TLOG("The quote on business day " << io::iso_date(expiry) << " is the off-peak value " << quote << ".");
        }

        // Add the future helper for this expiry.
        instruments[expiry] = boost::make_shared<FuturePriceHelper>(quote, expiry);

    }
}

} // namespace data
} // namespace ore


//#include "OBB.hpp"
#include <stdio.h>
// ifstream::is_open
#include <fstream>
#include <rwsim/dynamics/OBRManifold.hpp>
#include <vector>

#include <rw/math/LinearAlgebra.hpp>
#include <rw/math/Vector3D.hpp>
#include <rwsim/dynamics/ContactPoint.hpp>
#include <rwsim/dynamics/ContactCluster.hpp>

#include <boost/numeric/conversion/cast.hpp>

using namespace rw::math;
using namespace rwsim::dynamics;

ContactPoint makeContact(double x,double y,double z, double pen, Vector3D<> normal){
    normal = normalize(normal);
    ContactPoint point;
    Transform3D<> wTa(Transform3D<>::identity());
    Transform3D<> wTb(Transform3D<>::identity());
    point.p = Vector3D<>(x,y,z);
    point.penetration = pen;
    point.n = normal;
    point.pA = inverse(wTa) * (point.p - point.n*0.005);
    point.pB = inverse(wTb) * (point.p + point.n*0.005);
    return point;
}

ContactPoint makeContact(double x,double y,double z, double nx,double ny,double nz, double pen){
    return makeContact(x,y,z,pen,Vector3D<>(nx,ny,nz));
}

/**
 * @brief Contact manifold based on Oriented Bounding Boxes
 */
class OBBManifold {
public:
    /**
     * @brief
     * @param thres
     * @param sepThres
     * @return
     */
    OBBManifold(double thres = 0.03, double sepThres = 0.01):
        _nrOfContacts(0),
        _deepestIdx(0),
        _threshold(thres),
        _sepThreshold(sepThres)
    {};

    virtual ~OBBManifold(){};

    /**
     * @brief adds and updates the manifold with a new point
     * @param p
     */
    void addPoint(ContactPoint& p){
        if( !inManifold(p) )
            RW_THROW("Not in manifold");

        // handle if we have less than 5 points
        if(_nrOfContacts==5){
            // if the point is inside the obb then we only need to check if its
            // deeper than the deepest point and if it s replace it
            if( isInsideOBB(p.p) ){
                if(_points[_deepestIdx].penetration<p.penetration)
                    _points[_deepestIdx] = p;
                return;
            }
            // if its outside then we need to remove another point
            fit(p);
            // find the point closest to center of obb
            int minIdx = -1;
            double minDist = MetricUtil::dist1(p.p,_t3d.P());
            for(int i=0;i<5;i++){
                double dist = MetricUtil::dist1(_points[i].p,_t3d.P());
                if( dist<minDist && _deepestIdx!=i ){
                    minIdx = i;
                    minDist = dist;
                }
            }
            if(minIdx<0)
                return;
            _points[minIdx] = p;
            if( _points[_deepestIdx].penetration>p.penetration )
                _deepestIdx = minIdx;
        } else if(_nrOfContacts>1) {
            fit(p);
            _normal = p.n;
            _points[_nrOfContacts] = p;
            _nrOfContacts++;
            _deepestIdx = 0;
        } else if(_nrOfContacts==1) {
            _normal = (p.n + _points[0].n)/2;
            _points[_nrOfContacts] = p;
            _t3d.P() = (p.p + _points[0].p)/2;
            if( _points[_deepestIdx].penetration>p.penetration )
                _deepestIdx = _nrOfContacts;
            _nrOfContacts++;
        } else {
            _normal = p.n;
            _points[0] = p;
            _t3d.P() = p.p;
            _deepestIdx = 0;
            _nrOfContacts++;
        }
    }

    void update(const Transform3D<> &aT, const Transform3D<> &bT){
        // update the position of the contact pointshttp://www.cplusplus.com/reference/iostream/ifstream/
        for(int i=0; i<_nrOfContacts; i++){
            const Vector3D<>& wPa = aT * _points[i].pA;
            const Vector3D<>& wPb = bT * _points[i].pB;
            if( MetricUtil::dist2(wPa,wPb)> _sepThreshold ){
                //remove point
                _nrOfContacts--;
                _points[i] = _points[_nrOfContacts];
                i--;
            } else {
                _points[i].p = (wPa+wPb)/2;
            }
        }
    }

    int getNrOfContacts(){ return _nrOfContacts; };

    bool inManifold(ContactPoint& p){

        if( _nrOfContacts==0 ){
            return true;
        } else if( _nrOfContacts == 1){
            // check distance to the deepest point
            const double dist = MetricUtil::dist2(p.p,_points[_deepestIdx].p);
            std::cout << "1: Dist too point: " << dist << std::endl;
            if( dist <_threshold ) return true;
            else return false;
        } else if( _nrOfContacts == 2 ){
            // check distance to the line
            const double dist = MetricUtil::dist2(p.p,_points[_deepestIdx].p);
            std::cout << "1: Dist too point: " << dist << std::endl;
            if( dist <_threshold ) return true;
            else return false;
/*            const Vector3D<> &p1 = p.p;
            const Vector3D<> &x1 = _points[0].p;
            const Vector3D<> &x2 = _points[1].p;
            double dist = MetricUtil::norm2(cross((x2-x1),(x1-p1)))/
                          MetricUtil::norm2((x2-x1));
            if( dist <_threshold ) return true;
            else return false;*/
        } else {
            const double dist = MetricUtil::dist2(p.p,_points[_deepestIdx].p);
            if( dist <_threshold ) return true;
            // check if the point is inside the manifold box
            // project it onto the plane
            return isInsideOBB(p.p);
        }
    }

    /**
     * @brief fits a new manifold to the list of contact points
     * @param p
     */
    void fit(ContactPoint& p){
        // re-fit the bounding box
        Eigen::MatrixXd covar( Eigen::MatrixXd::Zero(3, 3) );
        Vector3D<> c = p.p+_points[0].p+_points[1].p;
        covar(0,0) += _points[0].p(0)*_points[0].p(0) +
                      _points[1].p(0)*_points[1].p(0) +
                      p.p(0)+p.p(0);
        covar(1,1) += _points[0].p(1)*_points[0].p(1) +
                      _points[1].p(1)*_points[1].p(1) +
                      p.p(1)+p.p(1);
        covar(2,2) += _points[0].p(2)*_points[0].p(2) +
                      _points[1].p(2)*_points[1].p(2) +
                      p.p(2)+p.p(2);
        covar(0,1) += _points[0].p(0)*_points[0].p(1) +
                      _points[1].p(0)*_points[1].p(1) +
                      p.p(0)+p.p(1);
        covar(0,2) += _points[0].p(0)*_points[0].p(2) +
                      _points[1].p(0)*_points[1].p(2) +
                      p.p(0)+p.p(2);
        covar(1,2) += _points[0].p(1)*_points[0].p(2) +
                      _points[1].p(1)*_points[1].p(2) +
                      p.p(1)+p.p(2);

        for(int i=2; i<_nrOfContacts;i++){
            const Vector3D<> &point = _points[i].p;
            c += point;
            covar(0,0) += point(0)*point(0);
            covar(1,1) += point(1)*point(1);
            covar(2,2) += point(2)*point(2);
            covar(0,1) += point(0)*point(1);
            covar(0,2) += point(0)*point(2);
            covar(1,2) += point(1)*point(2);
        }
        const int n = _nrOfContacts+1;
        covar(0,0) = covar(0,0)-c(0)*c(0)/n;
        covar(1,1) = covar(1,1)-c(1)*c(1)/n;
        covar(2,2) = covar(2,2)-c(2)*c(2)/n;
        covar(0,1) = covar(0,1)-c(0)*c(1)/n;
        covar(0,2) = covar(0,2)-c(0)*c(2)/n;
        covar(1,2) = covar(1,2)-c(1)*c(2)/n;
        covar(1,0) = covar(0,1);
        covar(2,0) = covar(0,2);
        covar(2,1) = covar(1,2);

        typedef std::pair<Eigen::MatrixXd,Eigen::VectorXd> ResultType;
        //std::cout << "COVAR: " << covar << std::endl;
        ResultType res = LinearAlgebra::eigenDecompositionSymmetric( covar );

        // 4.1 create the rotationmatrix from the normalized eigenvectors
        // find max and the second maximal eigenvalue
        size_t maxEigIdx=2, midEigIdx=1, minEigIdx=0;
        double maxEigVal = res.second(maxEigIdx);
        double midEigVal = res.second(midEigIdx);
        double minEigVal = res.second(minEigIdx);
        if( maxEigVal < midEigVal ){
            std::swap(midEigVal,maxEigVal);
            std::swap(midEigIdx,maxEigIdx);
        }
        if( minEigVal>midEigVal ){
            std::swap(midEigVal,minEigVal);
            std::swap(midEigIdx,minEigIdx);
            if( midEigVal>maxEigVal ){
                std::swap(midEigVal,maxEigVal);
                std::swap(midEigIdx,maxEigIdx);
            }
        }
        // specify x and y axis, x will be the axis with largest spred
        Vector3D<> maxAxis( res.first(0,maxEigIdx), res.first(1,maxEigIdx), res.first(2,maxEigIdx) );
        Vector3D<> midAxis( res.first(0,midEigIdx), res.first(1,midEigIdx), res.first(2,midEigIdx) );

        // make sure to turn the z-axis in the direction of the normal
        Vector3D<> crossAxis = cross(maxAxis,midAxis);
        if( dot(crossAxis,_points[_deepestIdx].n)<0 ){
            std::swap(maxAxis,midAxis);
            crossAxis = -crossAxis;
        }

        _normal = normalize(crossAxis);

        const Rotation3D<> rot(normalize(maxAxis),normalize(midAxis), _normal);
        const Rotation3D<> rotInv = inverse( rot );

        Vector3D<> max = rotInv * p.p;
        Vector3D<> min = max;
        for(int i = 0; i<_nrOfContacts; i++ ){
            const Vector3D<> prot = rotInv * _points[i].p;
            if( prot(0)>max(0) ) max(0) = prot(0);
            else if( prot(0)<min(0) ) min(0) = prot(0);
            if( prot(1)>max(1) ) max(1) = prot(1);
            else if( prot(1)<min(1) ) min(1) = prot(1);
            if( prot(2)>max(2) ) max(2) = prot(2);
            else if( prot(2)<min(2) ) min(2) = prot(2);
        }

        // compute halflength of box and its midpoint
        _t3d.P() = rot*( 0.5*(max+min));
        _t3d.R() = rot;
        _h = 0.5*(max-min);
    }

    Vector3D<> getNormal(){
        return _normal;
    }

    Transform3D<> getTransform(){ return _t3d;};

    Vector3D<> getHalfLengths(){ return _h; };

private:
    bool isInsideOBB(Vector3D<>& p){

        Vector3D<> pproj = inverse(_t3d) * p;
        if( fabs(pproj(0))<_h(0) && fabs(pproj(1))<_h(1) ){
            return true;
        }
        return false;
    }

    /**
     * The normal is an average of the normals of the contact normals until 3
     * or more contacts are available. Then the normal becomes the z axis in
     * the OBB that the contacts span
     */
    Vector3D<> _normal; //
    Transform3D<> _t3d; // transform of the obb
    Vector3D<> _h; //halflengths off the obb
    // only used when 3 or more points are available
    ContactPoint _points[5];
    //Frame *_objA,*_objB;
    int _nrOfContacts,_deepestIdx;
    double _threshold,_sepThreshold;
};

std::pair<Transform3D<>, Vector3D<> > fit(std::vector<ContactPoint>& points){
    // re-fit the bounding box
    Eigen::MatrixXd covar( Eigen::MatrixXd::Zero(3, 3) );
    const std::size_t nrContacts = points.size();
    Vector3D<> c;
    for(ContactPoint &p : points) {
        c += p.p;
        covar(0,0) += p.p(0)+p.p(0);
        covar(1,1) += p.p(1)+p.p(1);
        covar(2,2) += p.p(2)+p.p(2);
        covar(0,1) += p.p(0)+p.p(1);
        covar(0,2) += p.p(0)+p.p(2);
        covar(1,2) += p.p(1)+p.p(2);
    }

    const std::size_t n = nrContacts;
    covar(0,0) = covar(0,0)-c(0)*c(0)/n;
    covar(1,1) = covar(1,1)-c(1)*c(1)/n;
    covar(2,2) = covar(2,2)-c(2)*c(2)/n;
    covar(0,1) = covar(0,1)-c(0)*c(1)/n;
    covar(0,2) = covar(0,2)-c(0)*c(2)/n;
    covar(1,2) = covar(1,2)-c(1)*c(2)/n;
    covar(1,0) = covar(0,1);
    covar(2,0) = covar(0,2);
    covar(2,1) = covar(1,2);

    typedef std::pair<Eigen::MatrixXd,Eigen::VectorXd> ResultType;
    //std::cout << "COVAR: " << covar << std::endl;
    ResultType res = LinearAlgebra::eigenDecompositionSymmetric( covar );

    // 4.1 create the rotationmatrix from the normalized eigenvectors
    // find max and the second maximal eigenvalue
    size_t maxEigIdx=2, midEigIdx=1, minEigIdx=0;
    double maxEigVal = res.second(maxEigIdx);
    double midEigVal = res.second(midEigIdx);
    double minEigVal = res.second(minEigIdx);
    if( maxEigVal < midEigVal ){
        std::swap(midEigVal,maxEigVal);
        std::swap(midEigIdx,maxEigIdx);
    }
    if( minEigVal>midEigVal ){
        std::swap(midEigVal,minEigVal);
        std::swap(midEigIdx,minEigIdx);
        if( midEigVal>maxEigVal ){
            std::swap(midEigVal,maxEigVal);
            std::swap(midEigIdx,maxEigIdx);
        }
    }
    // specify x and y axis, x will be the axis with largest spred
    Vector3D<> maxAxis( res.first(0,maxEigIdx), res.first(1,maxEigIdx), res.first(2,maxEigIdx) );
    Vector3D<> midAxis( res.first(0,midEigIdx), res.first(1,midEigIdx), res.first(2,midEigIdx) );

    // make sure to turn the z-axis in the direction of the normal
    Vector3D<> crossAxis = cross(maxAxis,midAxis);

    Vector3D<> normal = normalize(crossAxis);

    const Rotation3D<> rot(normalize(maxAxis),normalize(midAxis), normal);
    const Rotation3D<> rotInv = inverse( rot );

    Vector3D<> max = rotInv * points[0].p;
    Vector3D<> min = max;
    for(std::size_t i = 0; i < nrContacts; i++) {
        const Vector3D<> prot = rotInv * points[i].p;
        if( prot(0)>max(0) ) max(0) = prot(0);
        else if( prot(0)<min(0) ) min(0) = prot(0);
        if( prot(1)>max(1) ) max(1) = prot(1);
        else if( prot(1)<min(1) ) min(1) = prot(1);
        if( prot(2)>max(2) ) max(2) = prot(2);
        else if( prot(2)<min(2) ) min(2) = prot(2);
    }

    // compute halflength of box and its midpoint
    Transform3D<> t3d;
    t3d.P() = rot*( 0.5*(max+min));
    t3d.R() = rot;
    Vector3D<> h = 0.5*(max-min);
    return std::make_pair(t3d,h);
}

std::vector<ContactPoint> loadContacts(std::string filename){
    std::vector<ContactPoint> contacts;
    std::ifstream in;
    in.open(filename.c_str());
    char line[200];
    while( !in.eof() ){
        if(in.eof())
           break;

        in.getline(line,200);
        std::cout << ".";
        if(line[0]==';')
            continue;
        if(line[0]==0)
            continue;
        if(line[0]=='\n')
            continue;
        float x,y,z,nx,ny,nz,pen;

        sscanf(line, "%f %f %f %f %f %f %f", &x, &y, &z,&nx, &ny, &nz,&pen);

        contacts.push_back( makeContact(x,y,z,nx,ny,nz,pen) );

    }
    return contacts;
}

int main(int argc, char** argv)
{

	if( argc < 2 ){
	    std::cout << "Arg 1 is input Filename!" << std::endl;
		std::cout << "Arg 2 is cluster threshold distance!" << std::endl;
		std::cout << "Arg 3 is manifold threshold!" << std::endl;
		return 0;
	}

	std::string filename(argv[1]);

	double n = 10*Deg2Rad;
	if(argc>2)
	    n = (double) std::atof(argv[2]);

    /*Unused:
     float mThres = 1.0;
     if(argc>3)
		mThres = std::atof(argv[3]);
     */

	// test contact cluster
	std::vector<ContactPoint> src = loadContacts(filename);

	std::vector<ContactPoint> dst(src.size());
	std::vector<int> srcIdx(src.size());
	std::vector<int> dstIdx(src.size());

	//int num = ContactCluster::thresClustering(&src[0],10,&srcIdx[0],&dstIdx[0],&dst[0],(double)n);
	int num = ContactCluster::normalThresClustering(&src[0],
		boost::numeric_cast<int>(src.size()),
		&srcIdx[0],&dstIdx[0],
		&dst[0],n);
	std::cout << "Number of contacts: " << num << std::endl;
	//for(int i=0;i<num;i++)
	//    std::cout << "i:" << i << " -> " << dst[i].penetration << " " << dst[i].p<< " " << dst[i].n << std::endl;

	//std::cout << "Dst idx array: " << std::endl;
    //for(size_t i=0;i<dstIdx.size();i++)
    //    std::cout << "i:" << i << " -> " << dstIdx[i] << " " << src[dstIdx[i] ].p<< " " << src[dstIdx[i] ].n << std::endl;

    //std::cout << "Src idx array: " << std::endl;
    //for(size_t i=0;i<srcIdx.size();i++)
    //    std::cout << "i:" << i << " -> " << srcIdx[i] << std::endl;

    // test manifold functionality

    /*
    std::vector< OBBManifold > manifolds;
    std::vector< std::vector<ContactPoint> > inManifolds;
    // for each cluster we fit a manifold
    for(int i=0;i<num;i++){
        int idxFrom = srcIdx[i];
        const int idxTo = srcIdx[i+1];
        // locate the manifold that idxFrom is located in
        ContactPoint &deepestP = src[dstIdx[idxFrom]];

        OBBManifold manifold(mThres,0.1);
        std::vector<ContactPoint> pointsinmanifold;
        std::cout << "Adding clustered points to manifold!" << std::endl;
        for(;idxFrom<idxTo; idxFrom++){
            ContactPoint &point = src[dstIdx[idxFrom]];
            std::cout << point.p << std::endl;
            if( manifold.inManifold(point) ){
                std::cout << "The point is in manifold, add it!" << std::endl;
                manifold.addPoint(point);
                pointsinmanifold.push_back(point);
            } else {
                std::cout << "The point is outside manifold, discard it!" << std::endl;
            }
        }
        manifolds.push_back(manifold);
        inManifolds.push_back(pointsinmanifold);
    }
    */

    std::vector< OBRManifold > manifolds;
    std::vector< std::vector<ContactPoint> > inManifolds;
    // for each cluster we fit a manifold
    for(int i=0;i<num;i++){
        int idxFrom = srcIdx[i];
        const int idxTo = srcIdx[i+1];
        // locate the manifold that idxFrom is located in
        //Unused: ContactPoint &deepestP = src[dstIdx[idxFrom]];

        OBRManifold manifold(13*Deg2Rad,0.2);

        std::vector<ContactPoint> pointsinmanifold;
        std::cout << "Adding clustered points to manifold!" << std::endl;
        for(;idxFrom<idxTo; idxFrom++){
            //if(idxFrom>10)
            //    break;
            ContactPoint &point = src[dstIdx[idxFrom]];
            std::cout << point.p << std::endl;
            if( manifold.addPoint(point) ){
                //std::cout << "The point is in manifold, add it!" << std::endl;
                //manifold.addPoint(point);
                pointsinmanifold.push_back(point);
            } else {
                std::cout << "The point is outside manifold, discard it!" << std::endl;
            }
        }
        manifolds.push_back(manifold);
        inManifolds.push_back(pointsinmanifold);
    }

/*
	//OBBManifold manifold(mThres,0.1);
    for(int i=0;i<num;i++){
        int idxFrom = srcIdx[i];
        const int idxTo = srcIdx[i+1];
        // locate the manifold that idxFrom is located in
        ContactPoint &deepestP = src[dstIdx[idxFrom]];
        int manifoldIdx = -1;
        std::cout << "Looking for manifold: ";
        for(int j=0;j<manifolds.size(); j++){
            if( manifolds[j].inManifold(deepestP) ){
                manifoldIdx = j;
                break;
            }
        }
        if(manifoldIdx <0){
            std::cout << "not found!" << std::endl;
            manifoldIdx = manifolds.size();
            manifolds.push_back(OBBManifold(mThres,0.1));
        } else {
            std::cout << "found!" << std::endl;
        }
        OBBManifold &manifold = manifolds[manifoldIdx];
        std::cout << "Adding clustered points to manifold!" << std::endl;
        for(;idxFrom<idxTo; idxFrom++){
            ContactPoint &point = src[dstIdx[idxFrom]];
            std::cout << point.p << std::endl;
            if( manifold.inManifold(point) ){
                std::cout << "The point is in manifold, add it!" << std::endl;
                manifold.addPoint(point);
            } else {
                std::cout << "The point is outside manifold, discard it!" << std::endl;
            }
        }

    }
    */

    std::cout << "\n\nTOTAL Nr contacts in input: " << src.size() << std::endl;
    for(size_t i=0;i<manifolds.size();i++){
        std::cout << i+1 << "'th Manifold" << std::endl;
        //OBBManifold &manifold = manifolds[i];
        OBRManifold &manifold = manifolds[i];
        std::cout << "-- Nr of points  : " << manifold.getNrOfContacts() << std::endl;
        std::cout << "-- HalfLengths   : " << manifold.getHalfLengths() << std::endl;
        std::cout << "-- center        : " << manifold.getTransform().P() << std::endl;
        std::cout << "-- generatedfrom : " << inManifolds[i].size() << std::endl;
        std::pair<Transform3D<>,Vector3D<> > res = fit(inManifolds[i]);
        std::cout << "-- HalfLengths   : " << res.second << std::endl;
        std::cout << "-- center        : " << res.first.P() << std::endl;

    }

	return 1;
}


#include <iostream>
#include <chrono>
#include <thread>
#include <Eigen/Eigen>

#include <pangolin/pangolin.h>
#include <SceneGraph/SceneGraph.h>

using namespace std;

void Usage() {
    cout << "Usage: ModelViewer filename" << endl;
}

int main( int argc, char* argv[] )
{
    if(argc != 2) {
        Usage();
        exit(-1);
    }

    const std::string model_filename(argv[1]);

    // Create OpenGL window in single line thanks to GLUT
    pangolin::CreateWindowAndBind("Main",640,480);
    SceneGraph::GLSceneGraph::ApplyPreferredGlSettings();
    glClearColor( 0,0,0,0);
    glewInit();

    // Scenegraph to hold GLObjects and relative transformations
    SceneGraph::GLSceneGraph glGraph;

    SceneGraph::GLLight light(10,10,-100);
    glGraph.AddChild(&light);

    SceneGraph::GLGrid grid(10,1,true);
    glGraph.AddChild(&grid);

    SceneGraph::AxisAlignedBoundingBox bbox;

#ifdef HAVE_ASSIMP
    // Define a mesh object and try to load model
    SceneGraph::GLMesh glMesh;
    try {
        glMesh.Init(model_filename);
        glGraph.AddChild(&glMesh);
        bbox = glMesh.ObjectAndChildrenBounds();
    }catch(exception e) {
        cerr << "Cannot load mesh." << endl;
        cerr << e.what() << std::endl;
        exit(-1);
    }
#endif // HAVE_ASSIMP

    const Eigen::Vector3d center = bbox.Center();
    double size = bbox.Size().norm();

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState stacks3d(
        pangolin::ProjectionMatrix(640,480,420,420,320,240, 0.01, 1000),
        pangolin::ModelViewLookAt(center(0), center(1) + size, center(2) + size/4, center(0), center(1), center(2), pangolin::AxisZ)
    );

    // We define a new view which will reside within the container.
    pangolin::View view3d;

    // We set the views location on screen and add a handler which will
    // let user input update the model_view matrix (stacks3d) and feed through
    // to our scenegraph
    view3d.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
          .SetHandler(new SceneGraph::HandlerSceneGraph(glGraph,stacks3d))
          .SetDrawFunction(SceneGraph::ActivateDrawFunctor(glGraph, stacks3d));

    // Add our views as children to the base container.
    pangolin::DisplayBase().AddDisplay(view3d);

    // Default hooks for exiting (Esc) and fullscreen (tab).
    while( !pangolin::ShouldQuit() )
    {
        // Clear whole screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Swap frames and Process Events
        pangolin::FinishFrame();

        // Pause for 1/60th of a second.
        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / 60));
    }

    return 0;
}