/**
 * @file test_distributed_mapper.cpp
 * @author Siddharth Choudhary
 * @brief unit tests for test multi robot case simulated by loading the datasets
 */

#include <distributed_mapper/distributed_mapper.h>
#include <distributed_mapper/distributed_mapper_utils.h>
#include <distributed_mapper/evaluation_utils.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include "distributed_pcm/distributed_pcm.h"

using namespace std;
using namespace gtsam;
using namespace distributed_mapper;
using namespace evaluation_utils;

noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Variance(12, 1);

// Prior
noiseModel::Diagonal::shared_ptr priorModel = //
        noiseModel::Isotropic::Variance(6, 1e-12);

string robotNames_("abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ");
static const Matrix zero33= Matrix::Zero(3,3);

////////////////////////////////////////////////////////////////////////////////////////////
vector<Values>
distributedEstimation(size_t nrRobots, string dataPath, string traceFile, Values centralized, size_t maxIter = 5000,
                      bool useFlaggedInit = true, double rotationEstimateChangeThreshold = 1e-7,
                      double poseEstimateChangeThreshold = 1e-7, bool useBetweenNoise = false,
                      bool contains_odometry = false){

    ////////////////////////////////////////////////////////////////////////////////
    // Read Graph
    ////////////////////////////////////////////////////////////////////////////////
    string dataFile = dataPath + "/fullGraph.g2o";
    GraphAndValues graphAndValuesWithoutPrior = readG2o(dataFile, true);
    NonlinearFactorGraph graphCentralized = *(graphAndValuesWithoutPrior.first);

    ////////////////////////////////////////////////////////////////////////////////
    // Distributed Estimation
    ////////////////////////////////////////////////////////////////////////////////
    vector< boost::shared_ptr<DistributedMapper> > distMappers;

    // Load subgraph and construct distMapper optimizers
    for(size_t robot = 0; robot < nrRobots; robot++){
        boost::shared_ptr<DistributedMapper> distMapper(new DistributedMapper(robotNames_[robot], true));

        // read robot subgraphs
        string dataFile_i = dataPath + boost::lexical_cast<string>(robot) + ".g2o";
        GraphAndValues graphAndValues = readG2o(dataFile_i, true);
        distMapper->loadSubgraphAndCreateSubgraphEdge(graphAndValues);

        // Add prior
        if(robot==0){
            Symbol sym(robotNames_[robot],0);
            distMapper->addPrior(sym, Pose3(), priorModel);
        }

        // Verbosity level
        distMapper->setVerbosity(DistributedMapper::ERROR);

        distMapper->setUseBetweenNoiseFlag(useBetweenNoise);

        // Push to the set of optimizers
        distMappers.push_back(distMapper);
    }

    // Get estimates
    vector < Values > rotationTrace;
    vector < Values > poseTrace;
    vector < Values > subgraphRotationTrace;
    vector < Values > subgraphPoseTrace;
    vector < VectorValues > rotationVectorValuesTrace;

    double gamma = 1.0f;
    int max_clique_size = 0;
    vector< Values > estimates = distributedOptimizer(distMappers, maxIter, max_clique_size, DistributedMapper::incUpdate,
                                                      gamma, rotationEstimateChangeThreshold, poseEstimateChangeThreshold,
                                                      useFlaggedInit, false, false, contains_odometry, 0.99, false, true,
                                                      boost::none, rotationTrace, poseTrace,
                                                      subgraphRotationTrace, subgraphPoseTrace, rotationVectorValuesTrace);

    // Aggregate distributed estimates into values
    Values distributed;
    for(size_t i = 0; i< estimates.size(); i++){
        for(const Values::ConstKeyValuePair& key_value: estimates[i]){
            Key key = key_value.key;
            size_t keyIndex = symbolIndex(key);
            distributed.insert(keyIndex, estimates[i].at<Pose3>(key));
        }
    }

    // Save it
    string distributed_file = traceFile + "_distributed.g2o";
    writeG2o(graphCentralized, distributed, distributed_file);

    ////////////////////////////////////////////////////////////////////////////////
    // Centralized Rotation Estimation for Trace Comparisons
    ////////////////////////////////////////////////////////////////////////////////
    NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(graphCentralized);
    GaussianFactorGraph centralizedLinearGraph;
    noiseModel::Unit::shared_ptr rotationGraphModel = noiseModel::Unit::Create(9);

    for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
        Matrix3 Rij;

        boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
                boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
        if (pose3Between)
            Rij = pose3Between->measured().rotation().matrix();
        else
            std::cout << "Error in buildLinearOrientationGraph" << std::endl;

        const FastVector<Key>& keys = factor->keys();
        Key key1 = keys[0], key2 = keys[1];
        Matrix M9 = Matrix::Zero(9,9);
        M9.block(0,0,3,3) = Rij;
        M9.block(3,3,3,3) = Rij;
        M9.block(6,6,3,3) = Rij;
        centralizedLinearGraph.add(key1, -I9, key2, M9, zero9, rotationGraphModel);
    }
    GaussianFactorGraph centralizedLinearGraphWithoutPrior = centralizedLinearGraph.clone();

    pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
    NonlinearFactorGraph graph = (graphAndValues.first);
    pose3Graph = InitializePose3::buildPose3graph(graph);
    centralizedLinearGraph = InitializePose3::buildLinearOrientationGraph(pose3Graph);
    VectorValues rotEstCentralized = centralizedLinearGraph.optimize();

    ////////////////////////////////////////////////////////////////////////////////
    // Log overall error w.r.t centralized graph
    ////////////////////////////////////////////////////////////////////////////////

    // Log distributed rotation error
    std::string traceFileOverallError = traceFile+ "_overall_error.txt";
    fstream traceStreamOverallError(traceFileOverallError.c_str(), fstream::out);
    for(size_t i=0; i<rotationVectorValuesTrace.size(); i++){
        traceStreamOverallError << centralizedLinearGraphWithoutPrior.error(rotationVectorValuesTrace[i]) << " ";
    }
    traceStreamOverallError << "-1" << endl;

    // Log distributed pose error
    for(size_t i=0; i<poseTrace.size(); i++){
        traceStreamOverallError << graphCentralized.error(poseTrace[i]) << " ";
    }
    traceStreamOverallError << "-1" << endl;

    // Log centralized pose error
    traceStreamOverallError << graphCentralized.error(centralized) << endl;
    traceStreamOverallError.close();

    // Log centralized rotation error
    double centralizedRotationError = centralizedLinearGraphWithoutPrior.error(rotEstCentralized);
    std::string traceFileCentralizedRotation = traceFile+ "_centralizedRotation.txt";
    fstream traceStreamCentralizedRotation(traceFileCentralizedRotation.c_str(), fstream::out);
    traceStreamCentralizedRotation << centralizedRotationError << endl;
    traceStreamCentralizedRotation.close();

    // Log each robot errors
    logResults(nrRobots, traceFile, centralized, distMappers);

    return estimates;
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_2robots) {

    // Read centralized graph
    string dataFile("../../../test_data/distributed_mapper/blocks_world/2robots/fullGraph.g2o");
    pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
    NonlinearFactorGraph graph = (graphAndValues.first);
    Values centralized = centralizedEstimation(graph, model, priorModel);

    size_t nrRobots = 2;
    string dataPath = "../../../test_data/distributed_mapper/blocks_world/2robots/";
    string traceFile = "../../../log/testdistributedEstimation_2robots";
    vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized, 1000, true, 1e-3, 1e-3);

    // Compare centralized and distributed pose estimates
    COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-1);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_4robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/4robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 4;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/4robots/";
  string traceFile = "../../../log/testdistributedEstimation_4robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized, 1000, true, 1e-3, 1e-3);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-1);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_9robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/9robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 9;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/9robots/";
  string traceFile = "../../../log/testdistributedEstimation_9robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized, 1000, true, 1e-3, 1e-3);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-1);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_16robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/16robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 16;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/16robots/";
  string traceFile = "../../../log/testdistributedEstimation_16robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized, 1000);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-1);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_25robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/25robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 25;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/25robots/";
  string traceFile = "../../../log/testdistributedEstimation_25robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-1);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_36robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/36robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 36;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/36robots/";
  string traceFile = "../../../log/testdistributedEstimation_36robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-0);
}

/******************************************************************************/
TEST(DistributedMapper, testdistributedEstimation_49robots) {

  // Read centralized graph
  string dataFile("../../../test_data/distributed_mapper/blocks_world/49robots/fullGraph.g2o");
  pair<NonlinearFactorGraph, Values> graphAndValues = loadGraphWithPrior(dataFile, priorModel);
  NonlinearFactorGraph graph = (graphAndValues.first);
  Values centralized = centralizedEstimation(graph, model, priorModel);

  // Distributed Mapper
  size_t nrRobots = 49;
  string dataPath = "../../../test_data/distributed_mapper/blocks_world/49robots/";
  string traceFile = "../../../log/testdistributedEstimation_49robots";
  vector<Values> distributed = distributedEstimation(nrRobots, dataPath, traceFile, centralized);

  // TEST
  COMPARE_VALUES_DATASET(nrRobots, centralized, distributed, 1e-0);
}

/****************************************************************************** */
int main() {
    TestResult tr;
    return TestRegistry::runAllTests(tr);
}
//******************************************************************************


/**
 * @file RectTree.hpp
 * @author bwu
 * @brief Model of R tree and related algorithms
 * @note This is a boost R(ectangle) tree wapper for the convenient usage
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_TREE_RECTTREE_HPP
#define GENERIC_TREE_RECTTREE_HPP
#include "generic/geometry/Box.hpp"
#include <boost/geometry.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <iostream>
#include <utility>
#include <list>
namespace generic{
namespace tree {
using generic::geometry::Box2D;
using generic::geometry::Point2D;
namespace bg = boost::geometry;
namespace bgid = bg::index::detail;

/**
 * @brief model of R tree node concept
 * @tparam num_type coordinate type of R tree
 * @tparam object the object that R tree node holes it's pointer
 */
template <typename num_type, typename object>
class RectNode
{
public:
    typedef std::list<RectNode<num_type, object> *> RectChildren;
    int m_level = 0;
    Box2D<num_type > m_bbox;
    std::list<object * > m_objs;
    RectNode * m_parent = nullptr;
    RectChildren m_children;

    virtual ~RectNode(){ Clear(); }
    ///@brief build tree node with input `objs`
    void Build(std::list<object* > && objs);

    ///@brief gets node level, the top level is 0, and sub node increasing
    int GetLevel() const { return m_level; }
    ///@brief sets bounding box of this node
    void SetBBox(const Box2D<num_type > & bbox) { m_bbox = bbox; }
    ///@brief gets bounding box of this node
    const Box2D<num_type > & GetBBox() const { return m_bbox; }

    ///@brief gets objects contains in this node
    std::list<object * > & GetObjs() { return m_objs; }
    const std::list<object * > & GetObjs() const { return m_objs; }

    ///@brief checks if this node is the root of tree
    bool isRoot() const { return nullptr == m_parent; }
    ///@brief gets parent node of this node
    RectNode * GetParent() { return m_parent; }
    ///@brief checks if this node has chiled
    bool hasChild() const { return m_children.size() != 0 ; }
    ///@brief gets reference of children container
    RectChildren & GetChildren() { return m_children; }
    ///@brief gets const reference of children container
    const RectChildren & GetChildren() const { return m_children; }
    ///@brief gets all objects contains in this node and it's sub nodes
    void GetAllObjects(std::list<object * > & allObjs) const;
    void GetAllObjects(std::list<const object * > & allObjs) const;
    ///@brief gets all leaf nodes from input node
    static void GetAllNodes(RectNode<num_type, object>  * node, std::list<RectNode<num_type, object> * > & allNodes);
    ///@brief gets all leaf nodes from input node by level
    static void GetAllNodesByLevel(RectNode<num_type, object>  * node, std::map<int, std::list<RectNode<num_type, object> * > > & allNodes);
    ///@brief gets all top nodes that bbox area less the given `arae` from input `node`
    static void GetTopNodesAreaLessThan(num_type area, RectNode<num_type, object> * node, std::list<RectNode<num_type, object> * > & allNodes);

    ///@brief clear all nodes and object pointers
    void Clear();
};

template <typename num_type, typename object>
void RectNode<num_type, object>::Build(std::list<object * > && objs)
{
    Clear();
    m_objs = objs;
}

template <typename num_type, typename object>
void RectNode<num_type, object>::GetAllObjects(std::list<object * > & allObjs) const
{
    typename std::list<object* >::const_iterator iter1 = m_objs.begin();
    for(; iter1 != m_objs.end(); ++iter1) allObjs.push_back(*iter1);

    if(hasChild()){
        auto iter = m_children.begin();
        for(; iter != m_children.end(); ++iter)
            (*iter)->GetAllObjects(allObjs);
    }
}

template <typename num_type, typename object>
void RectNode<num_type, object>::GetAllObjects(std::list<const object * > & allObjs) const
{
    typename std::list<object * >::const_iterator iter1 = m_objs.begin();
    for(; iter1 != m_objs.end(); ++iter1) allObjs.push_back(*iter1);

    if(hasChild()){
        auto iter = m_children.begin();
        for(; iter != m_children.end(); ++iter)
            (*iter)->GetAllObjects(allObjs);
    }
}

template <typename num_type, typename object>
void RectNode<num_type, object>::GetAllNodes(RectNode<num_type, object>  * node, std::list<RectNode<num_type, object> * > & allNodes)
{
    allNodes.push_back(node);
    if(!node->hasChild()) return;
    const auto & children = node->GetChildren();
    for(auto & child : children) GetAllNodes(child, allNodes);
}

template <typename num_type, typename object>
void RectNode<num_type, object>::GetAllNodesByLevel(RectNode<num_type, object> * node, std::map<int, std::list<RectNode<num_type, object> * > > & allNodes)
{
    int topLevel = node->GetLevel();
    allNodes[topLevel].push_back(node);
    if(!node->hasChild()) return;
    const auto & children = node->GetChildren();
    for(auto & child : children) GetAllNodesByLevel(child, allNodes);
}

template <typename num_type, typename object>
void RectNode<num_type, object>::GetTopNodesAreaLessThan(num_type area, RectNode<num_type, object> * node, std::list<RectNode<num_type, object> * > & allNodes)
{
    if(node->m_bbox.Area() >= area){
        if(!node->hasChild()) return;
        const auto & children = node->GetChildren();
        for(auto & child : children) GetTopNodesAreaLessThan(area, child, allNodes);
    }
    else allNodes.push_back(node);
}

template <typename num_type, typename object>
void RectNode<num_type, object>::Clear()
{
    auto iter = m_children.begin();
    for(; iter != m_children.end(); ++iter){
        if(*iter) delete *iter;
    }
    m_children.clear();
    m_objs.clear();
}

/**
 * @brief model of R tree concept
 * @tparam num_type coordinate type of R tree
 * @tparam object the object that R tree node holes it's pointer
 * @tparam extent functor to calculate bbox of object
 */
template <typename num_type, typename object, typename extent>
class RectTree : public RectNode<num_type, object>
{
    typedef bg::model::d2::point_xy<num_type, bg::cs::cartesian> RtPoint;
    typedef bg::model::box<RtPoint> RtBox;
    typedef std::pair<RtBox, object * > RtValue;
    typedef bg::index::rtree<RtValue, bg::index::dynamic_rstar> RtreeProxy;
    typedef bgid::rtree::utilities::view<RtreeProxy> RtProxyView;

    template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
    class RtProxyVisitor
            : public bgid::rtree::visitor<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag, true>::type
    {
        typedef typename bgid::rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type RtProxyNode;
        typedef typename bgid::rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type RtProxyLeaf;

    public:
        ///@brief a visitor to contructs rtree node from boost rtree structure
        RtProxyVisitor(RectNode<num_type, object> * root) : m_root(root) { m_currNode = m_root; }

        void operator()(const RtProxyNode & proxyNode)
        {
            typedef typename bgid::rtree::elements_type<RtProxyNode>::type RtProxyNodeEle;
            const RtProxyNodeEle & elements = bgid::rtree::elements(proxyNode);

            RectNode<num_type, object> * pareNode = m_currNode;
            auto iter = elements.begin();
            for(; iter != elements.end(); ++iter){
                RectNode<num_type, object> * node = new RectNode<num_type, object>;
                HandleBox(node, pareNode, iter->first);
                m_currNode = node;
                bgid::rtree::apply_visitor(*this, *(iter->second));
            }
        }

        void operator()(const RtProxyLeaf & leaf)
        {
            typedef typename bgid::rtree::elements_type<RtProxyLeaf>::type RtProxyLeafEle;
            const RtProxyLeafEle & elements = bgid::rtree::elements(leaf);

            auto iter = elements.begin();
            for(; iter != elements.end(); ++iter){
                HandleValue(m_currNode, iter->second);
            }
        }

        void HandleBox(RectNode<num_type, object> * node, RectNode<num_type, object> * parent, const Box & box)
        {
            node->m_level = parent->m_level + 1;
            node->m_parent = parent;
            parent->m_children.push_back(node);
            node->m_bbox[0][0] = bg::get<bg::min_corner, 0>(box);
            node->m_bbox[0][1] = bg::get<bg::min_corner, 1>(box);
            node->m_bbox[1][0] = bg::get<bg::max_corner, 0>(box);
            node->m_bbox[1][1] = bg::get<bg::max_corner, 1>(box);
        }

        void HandleValue(RectNode<num_type, object> * node, object * obj)
        {
            node->m_objs.push_back(obj);
        }

        RectNode<num_type, object> * m_root;
        RectNode<num_type, object> * m_currNode;
    };

public:
    RectTree() : m_proxy(nullptr) {}
    ~RectTree()
    {
        if(m_proxy) delete m_proxy;
        m_proxy = nullptr;
        RectNode<num_type, object>::Clear();
    }

    /**
     * @brief build tree with input objects and threshold
     * @param[in] objs input objects
     * @param[in] max_objs_to_build_sub tree nodes will continue spliting if the objects numbers it holds greater than `max_objs_to_build_sub`
     * @note if `max_objs_to_build_sub`=0, this node will take all objects and stop building
     */
    void Build(std::list<object* > objs, size_t max_objs_to_build_sub);
    ///@brief gets all top nodes that bbox area less the given `arae`
    void GetTopNodesAreaLessThan(num_type area, std::list<RectNode<num_type, object> * > & nodes);

private:
    void BuildProxy_(const std::list<object* > & objs, size_t max_objs_to_build_sub);
    void BuildSelf_();

    RtreeProxy * m_proxy;
};

template <typename num_type, typename object, typename extent>
void RectTree<num_type, object, extent>::Build(std::list<object* > objs, size_t max_objs_to_build_sub)
{
    RectNode<num_type, object>::Clear();

    if(0 == max_objs_to_build_sub){
        RectNode<num_type, object>::Build(std::move(objs));
        return;
    }

    BuildProxy_(objs, max_objs_to_build_sub);
    BuildSelf_();
}

template <typename num_type, typename object, typename extent>
void RectTree<num_type, object, extent>::BuildProxy_(const std::list<object* > & objs, size_t max_objs_to_build_sub)
{
    extent ext;
    if(m_proxy) delete m_proxy;
    bg::index::dynamic_rstar para(max_objs_to_build_sub);
    m_proxy = new RtreeProxy(para);

    for(object * obj : objs){
        Box2D<num_type> bbox = ext(*obj);
        RtBox rtBox(RtPoint(bbox[0][0], bbox[0][1]), RtPoint(bbox[1][0], bbox[1][1]));
        m_proxy->insert(std::make_pair(rtBox, obj));
    }
}

template <typename num_type, typename object, typename extent>
void RectTree<num_type, object, extent>::BuildSelf_()
{
    RtProxyView rtv(*m_proxy);
    RtProxyVisitor<
            typename RtProxyView::value_type,
            typename RtProxyView::options_type,
            typename RtProxyView::translator_type,
            typename RtProxyView::box_type,
            typename RtProxyView::allocators_type
            > visitor(this);
    rtv.apply_visitor(visitor);
}

template <typename num_type, typename object, typename extent>
void RectTree<num_type, object, extent>::GetTopNodesAreaLessThan(num_type area, std::list<RectNode<num_type, object> * > & nodes)
{
    RectNode<num_type, object>::GetTopNodesAreaLessThan(area, this, nodes);
}

}//namespace tree
}//namespace generic
#endif//GENERIC_TREE_RECTTREE_HPP


//  (C) Copyright Raffi Enficiaud 2014.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE boost_test_macro_overview
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE( test_macro_overview )
{
  namespace tt = boost::test_tools;
  int a = 1;
  int b = 2;
  BOOST_TEST(a != b - 1);
  BOOST_TEST(a + 1 < b);
  BOOST_TEST(b -1 > a, a << " < " << b - 1 << " does not hold");
  BOOST_TEST(a == b, tt::bitwise());
  BOOST_TEST(a + 0.1 == b - 0.8, tt::tolerance(0.01));
}
//]


#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <utility>
#include <vector>
#include <map>
#include <algorithm>
#include <stdint.h>

#include "gflags/gflags.h"
#include "glog/logging.h"
#include <boost/shared_ptr.hpp>

#include "caffe/proto/caffe.pb.h"
#include "caffe/blob.hpp"
#include "caffe/util/benchmark.hpp"
#include "caffe/util/io.hpp"
#include "caffe/util/math_functions.hpp"
#include "caffe/util/rng.hpp"
#include "caffe/layer.hpp"
#include "caffe/filler.hpp"

#include "caffe/nhwc/deconv_nhwc_layer.hpp"

#include "gtest/gtest.h"
#include "caffe/test/test_caffe_main.hpp"
#include "caffe/test/test_gradient_check_util.hpp"

using namespace std;
using namespace caffe;

int main(int nargc, char** args) {
  // set caffe::GPU
  caffe::Caffe::SetDevice(0);
  caffe::Caffe::set_mode(caffe::Caffe::CPU);

  int input_channels = 4;
  int output_channels = 4;
  int input_dim = 4;
  int kernel = 4;
  int stride = 4;
  int pad = 0;
  // define input & output blobs
  Blob<float>* input = new Blob<float>(1, input_dim, input_dim, input_channels);
  Blob<float>* output = new Blob<float>(1, input_dim, input_dim, output_channels);
  vector<Blob<float>*> bottom_vec;
  vector<Blob<float>*> top_vec;

  // set the bottom blob data
  FillerParameter filler_param;
  filler_param.set_std(0.5);
  GaussianFiller<float> filler(filler_param);
  filler.Fill(input);

  // set the bottom and top data-pointer
  bottom_vec.push_back(input);
  top_vec.push_back(output);

  // create reorg layer
  LayerParameter layer_param;
  layer_param.set_name("DeconvNHWC");
  layer_param.set_type("DeconvolutionNHWC");
  ConvolutionParameter* conv_param = layer_param.mutable_convolution_param();
  conv_param->set_kernel_h(kernel);
  conv_param->set_kernel_w(kernel);
  conv_param->set_stride_h(stride);
  conv_param->set_stride_w(stride);
  conv_param->set_pad_h(pad);
  conv_param->set_pad_w(pad);
  conv_param->set_group(1);
  conv_param->set_num_output(output_channels);
  conv_param->set_engine(caffe::ConvolutionParameter_Engine_CAFFE);
  FillerParameter* weight_filler = conv_param->mutable_weight_filler();
  weight_filler->set_type("gaussian");
  weight_filler->set_std(0.01);
  FillerParameter* bias_filler = conv_param->mutable_bias_filler();
  bias_filler->set_type("constant");
  bias_filler->set_std(0);

  boost::shared_ptr<Layer<float> > deconvNHWC_layer = LayerRegistry<float>::CreateLayer(layer_param);
  deconvNHWC_layer->SetUp(bottom_vec, top_vec);
  deconvNHWC_layer->Forward(bottom_vec, top_vec);

  GradientChecker<float> checker(1e-4, 1e-3);
  checker.CheckGradientExhaustive(deconvNHWC_layer.get(), bottom_vec, top_vec);

  LOG(INFO) << "Gradient Check Finished.";
  return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cmath>
#include <random>

#include "Domain/CoordinateMaps/Wedge3D.hpp"
#include "Domain/OrientationMap.hpp"
#include "ErrorHandling/Error.hpp"
#include "Utilities/StdArrayHelpers.hpp"
#include "Utilities/TypeTraits.hpp"
#include "tests/Unit/Domain/CoordinateMaps/TestMapHelpers.hpp"

namespace {
void test_wedge3d_all_directions() {
  // Set up random number generator
  const auto seed = std::random_device{}();
  std::mt19937 gen(seed);
  INFO("seed = " << seed);
  std::uniform_real_distribution<> unit_dis(0, 1);
  std::uniform_real_distribution<> inner_dis(1, 3);
  std::uniform_real_distribution<> outer_dis(5.2, 7);
  const double inner_radius = inner_dis(gen);
  CAPTURE_PRECISE(inner_radius);
  const double outer_radius = outer_dis(gen);
  CAPTURE_PRECISE(outer_radius);
  const double inner_sphericity = unit_dis(gen);
  CAPTURE_PRECISE(inner_sphericity);
  const double outer_sphericity = unit_dis(gen);
  CAPTURE_PRECISE(outer_sphericity);

  using WedgeHalves = CoordinateMaps::Wedge3D::WedgeHalves;
  const std::array<WedgeHalves, 3> halves_array = {
      {WedgeHalves::UpperOnly, WedgeHalves::LowerOnly, WedgeHalves::Both}};
  for (const auto& halves : halves_array) {
    for (const auto& direction : all_wedge_directions()) {
      for (const auto& with_equiangular_map : {true, false}) {
        for (const auto& with_logarithmic_map : {true, false}) {
          const CoordinateMaps::Wedge3D wedge_map(
              inner_radius, outer_radius, direction,
              with_logarithmic_map ? 1.0 : inner_sphericity,
              with_logarithmic_map ? 1.0 : outer_sphericity,
              with_equiangular_map, halves, with_logarithmic_map);
          test_suite_for_map_on_unit_cube(wedge_map);
        }
      }
    }
  }
}

void test_wedge3d_alignment() {
  // This test tests that the logical axes point along the expected directions
  // in physical space

  const double inner_r = sqrt(3.0);
  const double outer_r = 2.0 * sqrt(3.0);

  using WedgeHalves = CoordinateMaps::Wedge3D::WedgeHalves;
  const auto wedge_directions = all_wedge_directions();

  for (const auto& with_equiangular_map : {true, false}) {
    for (const auto& with_logarithmic_map : {true, false}) {
      const double inner_sphericity = with_logarithmic_map ? 1.0 : 0.0;
      const CoordinateMaps::Wedge3D map_upper_zeta(
          inner_r, outer_r, wedge_directions[0], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Upper Z wedge
      const CoordinateMaps::Wedge3D map_upper_eta(
          inner_r, outer_r, wedge_directions[2], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Upper Y wedge
      const CoordinateMaps::Wedge3D map_upper_xi(
          inner_r, outer_r, wedge_directions[4], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Upper X Wedge
      const CoordinateMaps::Wedge3D map_lower_zeta(
          inner_r, outer_r, wedge_directions[1], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Lower Z wedge
      const CoordinateMaps::Wedge3D map_lower_eta(
          inner_r, outer_r, wedge_directions[3], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Lower Y wedge
      const CoordinateMaps::Wedge3D map_lower_xi(
          inner_r, outer_r, wedge_directions[5], inner_sphericity, 1.0,
          with_equiangular_map, WedgeHalves::Both,
          with_logarithmic_map);  // Lower X wedge
      const std::array<double, 3> lowest_corner{{-1.0, -1.0, -1.0}};
      const std::array<double, 3> along_xi{{1.0, -1.0, -1.0}};
      const std::array<double, 3> along_eta{{-1.0, 1.0, -1.0}};
      const std::array<double, 3> along_zeta{{-1.0, -1.0, 1.0}};

      const std::array<double, 3> lowest_physical_corner_upper_zeta{
          {-1.0, -1.0, 1.0}};
      const std::array<double, 3> lowest_physical_corner_upper_eta{
          {-1.0, 1.0, -1.0}};
      const std::array<double, 3> lowest_physical_corner_upper_xi{
          {1.0, -1.0, -1.0}};
      const std::array<double, 3> lowest_physical_corner_lower_zeta{
          {-1.0, 1.0, -1.0}};
      const std::array<double, 3> lowest_physical_corner_lower_eta{
          {-1.0, -1.0, 1.0}};
      const std::array<double, 3> lowest_physical_corner_lower_xi{
          {-1.0, 1.0, -1.0}};

      // Test that this map's logical axes point along +X, +Y, +Z:
      CHECK(map_upper_zeta(along_xi)[0] == approx(1.0));
      CHECK(map_upper_zeta(along_eta)[1] == approx(1.0));
      CHECK(map_upper_zeta(along_zeta)[2] == approx(2.0));
      CHECK_ITERABLE_APPROX(map_upper_zeta(lowest_corner),
                            lowest_physical_corner_upper_zeta);

      // Test that this map's logical axes point along +Z, +X, +Y:
      CHECK(map_upper_eta(along_xi)[2] == approx(1.0));
      CHECK(map_upper_eta(along_eta)[0] == approx(1.0));
      CHECK(map_upper_eta(along_zeta)[1] == approx(2.0));
      CHECK_ITERABLE_APPROX(map_upper_eta(lowest_corner),
                            lowest_physical_corner_upper_eta);

      // Test that this map's logical axes point along +Y, +Z, +X:
      CHECK(map_upper_xi(along_xi)[1] == approx(1.0));
      CHECK(map_upper_xi(along_eta)[2] == approx(1.0));
      CHECK(map_upper_xi(along_zeta)[0] == approx(2.0));
      CHECK_ITERABLE_APPROX(map_upper_xi(lowest_corner),
                            lowest_physical_corner_upper_xi);

      // Test that this map's logical axes point along +X, -Y, -Z:
      CHECK(map_lower_zeta(along_xi)[0] == approx(1.0));
      CHECK(map_lower_zeta(along_eta)[1] == approx(-1.0));
      CHECK(map_lower_zeta(along_zeta)[2] == approx(-2.0));
      CHECK_ITERABLE_APPROX(map_lower_zeta(lowest_corner),
                            lowest_physical_corner_lower_zeta);

      // Test that this map's logical axes point along -Z, +X, -Y:
      CHECK(map_lower_eta(along_xi)[2] == approx(-1.0));
      CHECK(map_lower_eta(along_eta)[0] == approx(1.0));
      CHECK(map_lower_eta(along_zeta)[1] == approx(-2.0));
      CHECK_ITERABLE_APPROX(map_lower_eta(lowest_corner),
                            lowest_physical_corner_lower_eta);

      // Test that this map's logical axes point along -Y, +Z, -X:
      CHECK(map_lower_xi(along_xi)[1] == approx(-1.0));
      CHECK(map_lower_xi(along_eta)[2] == approx(1.0));
      CHECK(map_lower_xi(along_zeta)[0] == approx(-2.0));
      CHECK_ITERABLE_APPROX(map_lower_xi(lowest_corner),
                            lowest_physical_corner_lower_xi);
    }
  }
}

void test_wedge3d_random_radii() {
  // Set up random number generator:
  const auto seed = std::random_device{}();
  std::mt19937 gen(seed);
  INFO("seed = " << seed);
  std::uniform_real_distribution<> real_dis(-1, 1);
  std::uniform_real_distribution<> inner_dis(1, 3);
  std::uniform_real_distribution<> outer_dis(4, 7);

  // Check that points on the corners of the reference cube map to the correct
  // corners of the wedge.
  const std::array<double, 3> inner_corner{{-1.0, -1.0, -1.0}};
  const std::array<double, 3> outer_corner{{1.0, 1.0, 1.0}};
  const double random_inner_radius_lower_xi = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_lower_xi);
  const double random_inner_radius_lower_eta = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_lower_eta);
  const double random_inner_radius_lower_zeta = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_lower_zeta);
  const double random_inner_radius_upper_xi = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_upper_xi);
  const double random_inner_radius_upper_eta = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_upper_eta);
  const double random_inner_radius_upper_zeta = inner_dis(gen);
  CAPTURE_PRECISE(random_inner_radius_upper_zeta);

  const double random_outer_radius_lower_xi = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_lower_xi);
  const double random_outer_radius_lower_eta = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_lower_eta);
  const double random_outer_radius_lower_zeta = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_lower_zeta);
  const double random_outer_radius_upper_xi = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_upper_xi);
  const double random_outer_radius_upper_eta = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_upper_eta);
  const double random_outer_radius_upper_zeta = outer_dis(gen);
  CAPTURE_PRECISE(random_outer_radius_upper_zeta);

  using WedgeHalves = CoordinateMaps::Wedge3D::WedgeHalves;
  const auto wedge_directions = all_wedge_directions();
  for (const auto& with_equiangular_map : {true, false}) {
    for (const auto& with_logarithmic_map : {true, false}) {
      const double inner_sphericity = with_logarithmic_map ? 1.0 : 0.0;
      const CoordinateMaps::Wedge3D map_lower_xi(
          random_inner_radius_lower_xi, random_outer_radius_lower_xi,
          wedge_directions[5], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      const CoordinateMaps::Wedge3D map_lower_eta(
          random_inner_radius_lower_eta, random_outer_radius_lower_eta,
          wedge_directions[3], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      const CoordinateMaps::Wedge3D map_lower_zeta(
          random_inner_radius_lower_zeta, random_outer_radius_lower_zeta,
          wedge_directions[1], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      const CoordinateMaps::Wedge3D map_upper_xi(
          random_inner_radius_upper_xi, random_outer_radius_upper_xi,
          wedge_directions[4], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      const CoordinateMaps::Wedge3D map_upper_eta(
          random_inner_radius_upper_eta, random_outer_radius_upper_eta,
          wedge_directions[2], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      const CoordinateMaps::Wedge3D map_upper_zeta(
          random_inner_radius_upper_zeta, random_outer_radius_upper_zeta,
          wedge_directions[0], inner_sphericity, 1.0, with_equiangular_map,
          WedgeHalves::Both, with_logarithmic_map);
      CHECK(map_lower_xi(outer_corner)[0] ==
            approx(-random_outer_radius_lower_xi / sqrt(3.0)));
      CHECK(map_lower_eta(outer_corner)[1] ==
            approx(-random_outer_radius_lower_eta / sqrt(3.0)));
      CHECK(map_lower_zeta(outer_corner)[2] ==
            approx(-random_outer_radius_lower_zeta / sqrt(3.0)));
      CHECK(map_upper_xi(inner_corner)[0] ==
            approx(random_inner_radius_upper_xi / sqrt(3.0)));
      CHECK(map_upper_eta(inner_corner)[1] ==
            approx(random_inner_radius_upper_eta / sqrt(3.0)));
      CHECK(map_upper_zeta(inner_corner)[2] ==
            approx(random_inner_radius_upper_zeta / sqrt(3.0)));

      // Check that random points on the edges of the reference cube map to the
      // correct edges of the wedge.
      const std::array<double, 3> random_outer_face{
          {real_dis(gen), real_dis(gen), 1.0}};
      const std::array<double, 3> random_inner_face{
          {real_dis(gen), real_dis(gen), -1.0}};
      CAPTURE_PRECISE(random_outer_face);
      CAPTURE_PRECISE(random_inner_face);

      if (not with_logarithmic_map) {
        CHECK(map_lower_xi(random_inner_face)[0] ==
              approx(-random_inner_radius_lower_xi / sqrt(3.0)));
        CHECK(map_lower_eta(random_inner_face)[1] ==
              approx(-random_inner_radius_lower_eta / sqrt(3.0)));
        CHECK(map_upper_xi(random_inner_face)[0] ==
              approx(random_inner_radius_upper_xi / sqrt(3.0)));
        CHECK(map_upper_eta(random_inner_face)[1] ==
              approx(random_inner_radius_upper_eta / sqrt(3.0)));
      }
      CHECK(magnitude(map_lower_xi(random_outer_face)) ==
            approx(random_outer_radius_lower_xi));
      CHECK(magnitude(map_lower_eta(random_outer_face)) ==
            approx(random_outer_radius_lower_eta));
      CHECK(magnitude(map_upper_xi(random_outer_face)) ==
            approx(random_outer_radius_upper_xi));
      CHECK(magnitude(map_upper_eta(random_outer_face)) ==
            approx(random_outer_radius_upper_eta));
      CHECK(magnitude(map_lower_zeta(random_outer_face)) ==
            approx(random_outer_radius_lower_zeta));
      CHECK(magnitude(map_upper_zeta(random_outer_face)) ==
            approx(random_outer_radius_upper_zeta));
    }
  }
}

void test_wedge3d_fail() noexcept {
  const CoordinateMaps::Wedge3D map(0.2, 4.0, OrientationMap<3>{}, 0.0, 1.0,
                                    true);
  // Any point with z=0 should fail the inverse map.
  const std::array<double, 3> test_mapped_point1{{3.0, 3.0, 0.0}};
  const std::array<double, 3> test_mapped_point2{{-3.0, 3.0, 0.0}};

  // Any point with (x^2+y^2)/z^2 >= 1199 should fail the inverse map.
  const std::array<double, 3> test_mapped_point3{{sqrt(1198.0), 1.0, 1.0}};
  const std::array<double, 3> test_mapped_point4{{30.0, sqrt(299.0), 1.0}};
  const std::array<double, 3> test_mapped_point5{{30.0, sqrt(300.0), 1.0}};

  // These points are outside the mapped wedge. So inverse should either
  // return the correct inverse (which happens to be computable for
  // these points) or it should return boost::none.
  const std::array<double, 3> test_mapped_point6{{30.0, sqrt(298.0), 1.0}};
  const std::array<double, 3> test_mapped_point7{{2.0, 4.0, 6.0}};

  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point1)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point2)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point3)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point4)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point5)));
  if(map.inverse(test_mapped_point6)) {
    Approx my_approx = Approx::custom().epsilon(1.e-10).scale(1.0);
    CHECK_ITERABLE_CUSTOM_APPROX(map(map.inverse(test_mapped_point6).get()),
                                 test_mapped_point6, my_approx);
  }
  if(map.inverse(test_mapped_point7)) {
    CHECK_ITERABLE_APPROX(map(map.inverse(test_mapped_point7).get()),
                          test_mapped_point7);
  }
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Wedge3D.Fail", "[Domain][Unit]") {
  test_wedge3d_fail();
}

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Wedge3D.Map", "[Domain][Unit]") {
  test_wedge3d_all_directions();
  test_wedge3d_alignment();
  test_wedge3d_random_radii();
}

// [[OutputRegex, The radius of the inner surface must be greater than zero.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.RadiusInner",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(-0.2, 4.0, OrientationMap<3>{}, 0.0, 1.0, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, Sphericity of the inner surface must be between 0 and 1]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.SphericityInner",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(0.2, 4.0, OrientationMap<3>{}, -0.2, 1.0, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, Sphericity of the outer surface must be between 0 and 1]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.SphericityOuter",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(0.2, 4.0, OrientationMap<3>{}, 0.0, -0.2, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The radius of the outer surface must be greater than the
// radius of the inner surface.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.RadiusOuter",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(4.2, 4.0, OrientationMap<3>{}, 0.0, 1.0, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The arguments passed into the constructor for Wedge3D result
// in an object where the outer surface is pierced by the inner surface.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.PiercedSurface",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(3.0, 4.0, OrientationMap<3>{}, 1.0, 0.0, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The logarithmic map is only supported for spherical wedges.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.LogarithmicMap",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  auto failed_wedge3d =
      CoordinateMaps::Wedge3D(0.2, 4.0, OrientationMap<3>{}, 0.8, 0.9, true,
                              CoordinateMaps::Wedge3D::WedgeHalves::Both, true);
  static_cast<void>(failed_wedge3d);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "per_face_normals.h"
#include <Eigen/Geometry>

#define SQRT_ONE_OVER_THREE 0.57735026918962573
template <typename DerivedV, typename DerivedF, typename DerivedZ, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  const Eigen::MatrixBase<DerivedZ> & Z,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  N.resize(F.rows(),3);
  // loop over faces
  int Frows = F.rows();
#pragma omp parallel for if (Frows>10000)
  for(int i = 0; i < Frows;i++)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0));
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0));
    N.row(i) = v1.cross(v2);//.normalized();
    typename DerivedV::Scalar r = N.row(i).norm();
    if(r == 0)
    {
      N.row(i) = Z;
    }else
    {
      N.row(i) /= r;
    }
  }
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  Eigen::Matrix<typename DerivedN::Scalar,3,1> Z(0,0,0);
  return per_face_normals(V,F,Z,N);
}

template <typename DerivedV, typename DerivedF, typename DerivedN>
IGL_INLINE void igl::per_face_normals_stable(
  const Eigen::MatrixBase<DerivedV>& V,
  const Eigen::MatrixBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedN> & N)
{
  typedef Eigen::Matrix<typename DerivedV::Scalar,1,3> RowVectorV3;
  typedef typename DerivedV::Scalar Scalar;

  const size_t m = F.rows();

  N.resize(F.rows(),3);
  // Grad all points
  for(size_t f = 0;f<m;f++)
  {
    const RowVectorV3 p0 = V.row(F(f,0));
    const RowVectorV3 p1 = V.row(F(f,1));
    const RowVectorV3 p2 = V.row(F(f,2));
    const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0);
    const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1);
    const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2);

    // careful sum
    for(int d = 0;d<3;d++)
    {
      // This is a little _silly_ in terms of complexity, but its recursive
      // implementation is clean looking...
      const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 =
        [&sum3](Scalar a, Scalar b, Scalar c)->Scalar
      {
        if(fabs(c)>fabs(a))
        {
          return sum3(c,b,a);
        }
        // c < a
        if(fabs(c)>fabs(b))
        {
          return sum3(a,c,b);
        }
        // c < a, c < b
        if(fabs(b)>fabs(a))
        {
          return sum3(b,a,c);
        }
        return (a+b)+c;
      };

      N(f,d) = sum3(n0(d),n1(d),n2(d));
    }
    // sum better not be sure, or else NaN
    N.row(f) /= N.row(f).norm();
  }

}

#include "cotmatrix.h"

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename DerivedN,
  typename DerivedVV,
  typename DerivedFF,
  typename DerivedJ>
IGL_INLINE void igl::per_face_normals(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::PlainObjectBase<DerivedN> & N,
  Eigen::PlainObjectBase<DerivedVV> & VV,
  Eigen::PlainObjectBase<DerivedFF> & FF,
  Eigen::PlainObjectBase<DerivedJ> & J)
{
  assert(V.cols() == 3);
  typedef Eigen::Index Index;
  typedef typename DerivedN::Scalar Scalar;
  // Use Bunge et al. algorithm in igl::cotmatrix to insert a point for each
  // polygon which minimizes squared area.
  {
    Eigen::SparseMatrix<Scalar> _1,_2,P;
    igl::cotmatrix(V,I,C,_1,_2,P);
    VV = P*V;
  }
  // number of polygons
  const Eigen::Index m = C.size()-1;
  N.resize(m,3);
  FF.resize(C(m),3);
  J.resize(C(m));
  {
    Eigen::Index k = 0;
    for(Eigen::Index p = 0;p<m;p++)
    {
      N.row(p).setZero();
      // number of faces/vertices in this simple polygon
      const Index np = C(p+1)-C(p);
      for(Eigen::Index i = 0;i<np;i++)
      {
        FF.row(k) <<
          I(C(p)+((i+0)%np)),
          I(C(p)+((i+1)%np)),
          V.rows()+p;
        J(k) = p;
        k++;
        typedef Eigen::Matrix<Scalar,1,3> V3;
        N.row(p) +=
          V3(VV.row(I(C(p)+((i+0)%np)))-VV.row(V.rows()+p)).cross(
          V3(VV.row(I(C(p)+((i+1)%np)))-VV.row(V.rows()+p)));
      }
      // normalize to take average
      N.row(p) /= N.row(p).stableNorm();
    }
    assert(k == FF.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
// generated by autoexplicit.sh
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
// Nonsense template. Where'd this come from? AABB nonsense?
namespace igl{template<> void per_face_normals<Eigen::Matrix<double, -1, 2, 0, -1, 2>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 2, 0, -1, 2> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&){} }
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<float, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 1, 3, 1, 1, 3>, Eigen::Matrix<double, 1, -1, 1, 1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 1, 3, 1, 1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 1, -1, 1, 1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<double, -1, 3, 1, -1, 3>, Eigen::Matrix<int, -1, 3, 1, -1, 3>, Eigen::Matrix<double, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, -1, 1, -1, -1>, Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1>, Eigen::Matrix<float, -1, -1, 1, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, -1, 1, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, -1, 1, -1, -1> >&);
template void igl::per_face_normals<Eigen::Matrix<float, -1, 3, 1, -1, 3>, Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3>, Eigen::Matrix<float, -1, 3, 1, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<unsigned int, -1, 3, 1, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<float, -1, 3, 1, -1, 3> >&);
template void igl::per_face_normals<class Eigen::Matrix<double,-1,3,0,-1,3>,class Eigen::Matrix<int,-1,-1,0,-1,-1>,class Eigen::Matrix<double,-1,-1,0,-1,-1> >(class Eigen::MatrixBase<class Eigen::Matrix<double,-1,3,0,-1,3> > const &,class Eigen::MatrixBase<class Eigen::Matrix<int,-1,-1,0,-1,-1> > const &,class Eigen::PlainObjectBase<class Eigen::Matrix<double,-1,-1,0,-1,-1> > &);
template void igl::per_face_normals<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, 2, 3, 0, 2, 3>, Eigen::Matrix<double, 2, 3, 0, 2, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, 2, 3, 0, 2, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, 2, 3, 0, 2, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::per_face_normals_stable<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, Eigen::Matrix<double, -1, 3, 0, -1, 3> >(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2012 Klaus Spanderen

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file sparsematrix.hpp
    \brief typedef for boost sparse matrix class
*/

#ifndef quantlib_sparse_matrix_hpp
#define quantlib_sparse_matrix_hpp

#include <ql/qldefines.hpp>

#if !defined(QL_NO_UBLAS_SUPPORT)

#include <ql/math/array.hpp>

#if defined(QL_PATCH_MSVC)
#pragma warning(push)
#pragma warning(disable:4180)
#pragma warning(disable:4127)
#endif

#if defined(__clang__) && BOOST_VERSION > 105300
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#if BOOST_VERSION == 106400
#include <boost/serialization/array_wrapper.hpp>
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>

#if defined(QL_PATCH_MSVC)
#pragma warning(pop)
#endif

#if defined(__clang__) && BOOST_VERSION > 105300
#pragma clang diagnostic pop
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

namespace QuantLib {
    typedef boost::numeric::ublas::compressed_matrix<Real> SparseMatrix;
    typedef boost::numeric::ublas::matrix_reference<SparseMatrix>
        SparseMatrixReference;

    inline Disposable<Array> prod(const SparseMatrix& A, const Array& x) {
        Array b(x.size(), 0.0);

        for (Size i=0; i < A.filled1()-1; ++i) {
            const Size begin = A.index1_data()[i];
            const Size end   = A.index1_data()[i+1];
            Real t=0;
            for (Size j=begin; j < end; ++j) {
                t += A.value_data()[j]*x[A.index2_data()[j]];
            }

            b[i]=t;
        }
        return b;
    }
}

#endif
#endif


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

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

#ifndef _PIMATH_BOXALGO__H_
#define _PIMATH_BOXALGO__H_

#include <boost/python.hpp>
#include <ImathBoxAlgo.h>
#include "util.h"

/*
 * transform, affineTransform:
 * type of scalar must be the same for the box and the matrix
 * findEntryAndExitPoints:
 * replaced with entryAndExitPoints; returns points as a tuple (or None if none found)
 * intersects:
 * replaced by intersection; returns point of intersection or None
 */

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct BoxAlgoBind
	{
		typedef Imath::Vec2<T> 		vec2_type;
		typedef Imath::Vec3<T> 		vec3_type;
		BoxAlgoBind()
		{
			bp::def("closestPointInBox", &Imath::closestPointInBox<vec3_type>);
			bp::def("closestPointInBox", &Imath::closestPointInBox<vec2_type>);
			bp::def("closestPointOnBox", &Imath::closestPointOnBox<T>);
			bp::def("clip", &Imath::clip<vec2_type> );
			bp::def("clip", &Imath::clip<vec3_type> );
		}

/*		static vec2_type
		clip2( const vec2_type &p, const box2_type & b )
		{
			return Imath::clip<vec2_type>(p, b);
		}

		static vec3_type
		clip3( const vec3_type &p, const box3_type & b )
		{
			return Imath::clip<vec3_type>(p, b);
		}*/
	};

	template<typename T>
	struct BoxAlgoBindFloating
	{
		typedef Imath::Vec2<T> 		vec2_type;
		typedef Imath::Vec3<T> 		vec3_type;
		typedef Imath::Quat<T> 		quat_type;
		typedef Imath::Matrix33<T> 	mat33_type;
		typedef Imath::Matrix44<T> 	mat44_type;
		typedef Imath::Line3<T> 	line3_type;
		typedef Imath::Box< vec2_type > 	    box2_type;
		typedef Imath::Box< vec3_type > 	    box3_type;

		BoxAlgoBindFloating()
		{
			// TODO: Add different scalar combinations.
			bp::def("transform", &Imath::transform<T, T>);
			bp::def("affineTransform", &Imath::affineTransform<T, T>);
			bp::def("intersection", intersection);
			bp::def("entryAndExitPoints", entryAndExitPoints);
		}

		static bp::object
		intersection(const box3_type& box,
					 const line3_type& line)
		{
			vec3_type intersection;
			return ( Imath::intersects(box, line, intersection) ) ?
				bp::object( intersection ) : bp::object();
		}

		static bp::object
		entryAndExitPoints(const line3_type& line,
						   const box3_type box)
		{
			vec3_type entry, exit;
			return (Imath::findEntryAndExitPoints(line, box, entry, exit))?
				bp::make_tuple(entry, exit) : bp::object();
		}
	};
}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 exponential toolbox - pow_abs/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of exponential components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 08/12/2010
///
#include <nt2/exponential/include/functions/pow_abs.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/ones.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/table.hpp>

NT2_TEST_CASE_TPL ( pow_abs3,  NT2_REAL_TYPES)
{

  using nt2::pow_abs;
  using nt2::tag::pow_abs_;
  typedef std::complex<T> cT;
  nt2::table<cT> a = nt2::ones(3, 3, nt2::meta::as_<cT>());
  nt2::table<cT> b = nt2::ones(3, 3, nt2::meta::as_<cT>());
  NT2_DISPLAY(nt2::pow_abs(a, b));
  NT2_TEST_EQUAL(a, nt2::pow_abs(a, b));

}
NT2_TEST_CASE_TPL ( pow_abs4,  NT2_REAL_TYPES)
{

  typedef std::complex<T> cT;
  nt2::table<cT> a = nt2::ones(1, 3, nt2::meta::as_<cT>());

  for(int i=1; i <= 3; i++) a(i) =  cT(i, i);
  NT2_DISPLAY(a);

  nt2::table<T> b = T(2)*nt2::ones(1, 3, nt2::meta::as_<T>());
  NT2_DISPLAY(b);

  NT2_DISPLAY(nt2::pow_abs(a, b));
  NT2_TEST_ULP_EQUAL(nt2::pow_abs(a, b), T(2)*nt2::sqr(nt2::real(a)), T(2.0));
}


#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <ohm_igvc/waypoint.h>
#include <ohm_igvc/get_successors.h>
#include <ohm_igvc/cell_to_real.h>
#include <ohm_igvc/real_to_cell.h>
#include <ohm_igvc/coordinate_convert.h>
#include <ohm_igvc/robot_position.h>

#include <boost/heap/priority_queue>
#include <vector>
#include <array>
#include <cmath>

struct Node {
    Node *parent;
    int x, y, which_child;
    float f, g, h, t;

	bool operator<(const Node &lhs, const Node &rhs) { return lhs.f < rhs.f; };
	bool operator>(const Node &lhs, const Node &rhs) { return rhs < lhs; };
	bool operator<=(const Node &lhs, const Node &rhs) { return !(lhs > rhs); };
	bool operator>=(const Node &lhs, const Node &rhs) { return !(lhs < rhs); };

	bool operator==(const Node &lhs, const Node &rhs) { return (lhs.x == rhs.x && lhs.y == rhs.y); };

	Node() {
		parent = nullptr;
		x = y = which_child = 0;
		f = g = h = 0.0;
	};
}

class Planner {
    public:
        Planner(); //
        std::vector<geometry_msgs::Pose2D> plan(int x, int y); //
		void get_next_waypoint(int i); //
		int get_robot_x() { return robot_position.x; };
		int get_robot_y() { return robot_position.y; };
		int get_goal_x() { return goal.x; };
		int get_goal_y() { return goal.y; };
		void get_robot_position(); //
		double distance_to_goal() { return distance(robot_position, goal); };
		double distance_to_last() { return distance(robot_position, last_in_path); };
		geometry_msgs::Point get_real_robot_position() { return cell_to_world(robot_position.x, robot_position.y); };
		void replan_path(const ros::TimerEvent &e);

    private:
		std::array<Node, 8> get_successors(int x, int y, Node &parent); //
		geometry_msgs::Point cell_to_world(int x, int y);  //

		double distance(Node first, Node second) { return std::hypot((second.x - first.x), (second.y - first.y)); }; //
		double angular_distance(double a1, double a2) { return 180.0 - std::fabs(std::fabs(a1 - a2) - 180.0); }; //

        double permissible_distance;
		double robot_theta;
		int robot_x, robot_y;

		double child_angle[8];

		Node goal;
		Node last_in_path;
		Node robot_position;

        ros::NodeHandle node;
        ros::ServiceClient map_get_successors, map_cell_to_world, map_world_to_cell, map_robot_position, coord_convert, waypoint_service;
};

Planner::Planner() {
	permissible_distance = 25;
	node.param("plan_distance", permissible_distance, permissible_distance);

	map_get_successors = node.serviceClient<ohm_igvc::get_successors>("get_successors", true);
	map_cell_to_world = node.serviceClient<ohm_igvc::cell_to_real>("cell_to_real", true);
	map_world_to_cell = node.serviceClient<ohm_igvc::real_to_cell>("real_to_cell", true);
	map_robot_position = node.serviceClient<ohm_igvc::robot_position>("get_robot_position", true);
	coord_convert = node.serviceClient<ohm_igvc::coordinate_convert>("coordinate_convert", true);
	waypoint_service = node.serviceClient<ohm_igvc::waypoint>("waypoint");
}

std::vector<geometry_msgs::Pose2D> Planner::plan(int x = robot_position.x, int y = robot_position.y) {
    boost:heap::priority_queue<Node> open, closed;
	std::vector<Node> path;
	std::vector<geometry_msgs::Pose2D> final_path;

	path.reserve(100);
	final_path.reserve(40);

	Node start;
	start.parent = nullptr;
	start.f = 0;
	start.x = x;
	start.y = y;

	open.push_back(start);

	Node last_q = start;

	bool searching = true;

	while(!open.is_empty() && searching) {
		Node q = open.top();
		open.pop();

		std::array<Node, 8> successors = get_successors(q.x, q.y, Node &parent);

		for(auto child = successors.begin(); it != successors.end(); ++it) {
			if(child->parent == nullptr) continue;

			if(child == goal) {
				goal.parent = child.parent;
				last_q = goal;
				searching = false;
				break;
			}

			child->g = q.g + distance(q, *child);
			child->h = distance(*child, goal);
			child->f = child->g + child->h + child->t;

			if(child->g > permissible_distance) continue;

			// sorry for this.

			bool skip = false;

			for(auto it = open.begin(); it != open.end(); ++it) { if(child == *it && child > *it) skip = true; }
			if(skip) continue;

			skip = false;

			for(auto it = closed.begin(); it != closed.end(); ++it) { if(child == *it && child > *it) skip = true; }
			if(skip) continue;

			// end sorry

			open.push(*child);
		}

		closed.push(q);

		last_q = q;

	}

	for(Node n = last_q; n.parent != nullptr; n = (*n).parent) { path.push_back(n); }

	for(auto node = path.rbegin(), auto next = path.rbegin() + 1; next != path.rend(); ++node, ++next) {
		if(angular_distance(child_angle[node->which_child], child_angle[next->which_child]) >= 90.0) {
			geometry_msgs::Pose2D waypoint;
			geometry_msgs::Point real_coord = cell_to_world(node->x, node->y);

			waypoint.x = real_coord.x;
			waypoint.y = real_coord.y;
			waypoint.theta = child_angle[node->which_child];

			final_path.push_back(waypoint);
		}
	}

	last_in_path = path.front();

	return final_path;
}

void get_next_waypoint(int i) {
	ohm_igvc::waypoint req_wp;
	ohm_igvc::coordinate_convert req_conv;
	ohm_igvc::real_to_cell req_cell;

	req_wp.request.ID = i;

	waypoint_service.call(req_wp);

	req_conv.request.coordinate.latitiude = req_wp.response.waypoint.latitude;
	req_conv.request.coordinate.longitude = req_wp.response.waypoint.longitude;

	coord_convert.call(req_conv);

	real_to_cell.request.coordinate.x = req_conv.response.coordinate.x;
	real_to_cell.request.coordinate.y = req_conv.response.coordinate.y;

	map_real_to_cell.call(req_cell);

	goal.x = req_cell.response.x;
	goal.y = req_cell.response.y;
}

std::array<Node, 8> Planner::get_successors(int x, int y, Node &parent) {
	std::array<Node, 8> successors;

	if(map_get_successors) {
		ohm_igvc::get_successors req;
		req.x = x;
		req.y = y;

		map_get_successors.call(req);

		for(int i = 0; i < 8; i++) {
			if(req.nodes[i].t_cost >= 0) {
				successors[i].parent = &parent;
			}

			successors[i].x = req.nodes[i].x;
			successors[i].y = req.nodes[i].y;
			successors[i].which_child = i;
			successors[i].t = req.nodes[i].t;
		}
	}

	return successors;
}

geometry_msgs::Point Planner::cell_to_world(int x, int y){
	/*** TODO: Implement reconnection logic for service calls ***/
	// if(map_cell_to_world) {
	ohm_igvc::cell_to_real req;

	req.request.x = x;
	req.request.y = y;

	map_cell_to_world.call(req);

	return req.response.real_coordinate;
	// }
}

void Planner::get_robot_position() {
	ohm_igvc::robot_position req;

	map_robot_position.call(req);

	robot_position.x = req.response.x;
	robot_position.y = req.response.y;
}


#include "gtest/gtest.h"
#include "Intersection.h"
#include "MeshFactory.h"
#include <Eigen/Core>

using namespace Eigen;

namespace {
    using namespace Geotree;

    class IntersectionTest : public ::testing::Test {
    };

    TEST_F(IntersectionTest, basic)
    {
        MeshFactory factory;
        Mesh tetra1 = factory.makeTetra(1);
        Mesh tetra2 = factory.makeTetra(1);

        tetra2.translate(Vector(0.1, 0.1, 0.1));

        Intersection is(tetra1, tetra2);

        EXPECT_EQ(1, is.pathcount());

    }
}


/*
@copyright Louis Dionne 2014
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/hana/set.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/list.hpp>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

int main() {
    using L = detail::minimal::List<>;
    constexpr auto list = detail::minimal::list<>;
    BOOST_HANA_CONSTANT_ASSERT(to<L>(set()) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(to<L>(set(x<1>)) == list(x<1>));

    BOOST_HANA_CONSTEXPR_ASSERT(elem(permutations(list(x<1>, x<2>)), to<L>(set(x<1>, x<2>))));
    BOOST_HANA_CONSTEXPR_ASSERT(elem(permutations(list(x<1>, x<2>, x<3>)), to<L>(set(x<1>, x<2>, x<3>))));
}


/*
Copyright 2012, 2013 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test cost.hpp.
*/

#define BOOST_TEST_MODULE test_cost
#include "utility/test/boost_unit_test.hpp"

#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/test.hpp>

#include "math/cost.hpp"

#include <boost/mpl/assert.hpp>

#include "range/std/container.hpp"

#include "math/check/check_magma.hpp"
#include "math/check/check_hash.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_cost)

typedef math::cost <double> cost;

// Test "choose", or that operation by any other name.
template <class Choose> void test_choose (Choose const & choose) {
    BOOST_MPL_ASSERT ((math::has <Choose (cost, cost)>));
    BOOST_MPL_ASSERT_NOT ((math::is::approximate <Choose (cost, cost)>));
    BOOST_MPL_ASSERT ((math::has <math::callable::identity <cost, Choose>()>));
    BOOST_MPL_ASSERT ((math::has <math::callable::zero <cost>()>));
    BOOST_MPL_ASSERT ((math::is::commutative <Choose, cost>));
    BOOST_MPL_ASSERT ((math::is::associative <Choose, cost>));

    BOOST_MPL_ASSERT ((math::is::path_operation <Choose, cost>));
    BOOST_MPL_ASSERT ((math::is::idempotent <Choose, cost>));

    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::invert <Choose> (cost)>));
    BOOST_MPL_ASSERT_NOT ((math::has <math::callable::invert<>(Choose, cost)>));

    // Distributivity.
    BOOST_MPL_ASSERT ((math::is::distributive <math::either,
        math::callable::times, Choose, cost>));
    BOOST_MPL_ASSERT ((math::is::distributive <math::left,
        math::callable::times, Choose, cost>));
    BOOST_MPL_ASSERT ((math::is::distributive <math::right,
        math::callable::times, Choose, cost>));

    BOOST_CHECK_EQUAL (choose (cost (0), cost (0)).value(), 0.);
    BOOST_CHECK_EQUAL (choose (cost (-1), cost (0)).value(), -1.);
    BOOST_CHECK_EQUAL (choose (cost (0), cost (-1)).value(), -1.);
    BOOST_CHECK_EQUAL (choose (cost (5), cost (-1)).value(), -1.);
    BOOST_CHECK_EQUAL (choose (cost (-1), cost (5)).value(), -1.);
    BOOST_CHECK_EQUAL (choose (cost (2), cost (5)).value(), 2.);
}

BOOST_AUTO_TEST_CASE (test_cost) {
    BOOST_MPL_ASSERT ((math::has <math::callable::non_member <cost>()>));
    BOOST_MPL_ASSERT ((math::has <math::callable::is_member (cost)>));
    BOOST_MPL_ASSERT ((math::has <math::callable::equal (cost, cost)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::approximately_equal (cost, cost)>));

    // "choose" and "plus" are the same.
    test_choose (math::choose);
    test_choose (math::plus);

    // times.
    BOOST_MPL_ASSERT ((math::has <math::callable::times (cost, cost &)>));
    BOOST_MPL_ASSERT ((
        math::is::approximate <math::callable::times (cost, cost &)>));
    BOOST_MPL_ASSERT ((math::is::approximate <
        math::callable::times (math::callable::times (cost, cost &), cost &)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::identity <cost, math::callable::times>()>));
    BOOST_MPL_ASSERT ((math::has <math::callable::one <cost>()>));

    BOOST_MPL_ASSERT ((math::is::commutative <math::callable::times, cost>));
    BOOST_MPL_ASSERT ((math::is::associative <math::callable::times, cost>));

    BOOST_MPL_ASSERT_NOT ((
        math::is::path_operation <math::callable::times, cost>));
    BOOST_MPL_ASSERT_NOT ((math::is::idempotent <math::callable::times, cost>));

    BOOST_MPL_ASSERT ((math::has <math::callable::divide<> (cost, cost)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::invert <math::callable::times> (cost)>));
    BOOST_MPL_ASSERT ((
        math::has <math::callable::reverse <math::callable::times> (cost)>));

    BOOST_CHECK_EQUAL (math::zero <cost>().value(),
        std::numeric_limits <double>::infinity());
    BOOST_CHECK_EQUAL (math::one <cost>().value(), 0.);
    // Initialise as the additive identity, that is, with value "infinity".
    BOOST_CHECK_EQUAL (cost().value(),
        std::numeric_limits <double>::infinity());

    cost a (3.);
    cost b (5.);
    cost c (-2);

    // Multiplication: add costs.
    BOOST_CHECK_EQUAL ((a*b).value(), 8.);
    BOOST_CHECK_EQUAL (math::times (a, b).value(), 8.);
    BOOST_CHECK_EQUAL (math::times (c, b), a);
    // Division: subtract costs.
    BOOST_CHECK_EQUAL ((a/a).value(), 0.);
    BOOST_CHECK_EQUAL (math::divide (a, a).value(), 0.);
    BOOST_CHECK_EQUAL (math::divide (b, c).value(), 7.);
    // Invert element w.r.t. times: negate cost.
    BOOST_CHECK_EQUAL (math::invert <math::callable::times> (a).value(), -3.);
    BOOST_CHECK_EQUAL (math::invert (math::times, b).value(), -5.);
    BOOST_CHECK_EQUAL (math::invert <math::left> (math::times, c).value(), 2.);
    BOOST_CHECK_EQUAL (math::invert <math::right> (math::times, c).value(), 2.);

    // Check for consistency.
    std::vector <cost> examples;
    examples.push_back (cost (-5.));
    examples.push_back (cost (-2.3));
    examples.push_back (cost (-1.));
    examples.push_back (cost (-0.5));
    examples.push_back (cost (-0.2));
    examples.push_back (cost (0.));
    examples.push_back (cost (0.2));
    examples.push_back (cost (0.5));
    examples.push_back (cost (1.));
    examples.push_back (cost (2.3));
    examples.push_back (cost (5.));

    math::check_equal_on (examples);
    math::check_hash (examples);

    math::check_magma <cost> (math::times, math::plus, examples);

    math::check_semiring <cost, math::either> (
        math::times, math::plus, examples);
    math::check_semiring <cost, math::either> (
        math::times, math::choose, examples);
}

BOOST_AUTO_TEST_SUITE_END()


#include "snapconfig.hpp"

#include "util/parseangle.hpp"
#include "util/snapregex.hpp"
#include <boost/algorithm/string.hpp>
#include <exception>

using namespace std;
using namespace LINZ;
using namespace ParseAngle;

static RGX::regex dms_regex("^\\s*([-nsNSewEW]?)\\s*((?:3[0-5]|[0-2]?\\d?)\\d)\\s+([0-5]?\\d)\\s+([0-5]?\\d)(?:\\.(\\d*))?\\s*([nsNSewEW]?)\\s*$");
static RGX::regex hp_regex("^\\s*(\\-?)((?:3[0-5]|[0-2]?\\d?)\\d)\\.([0-5]\\d)([0-5]\\d)(\\d*)()\\s*$");

ParseAngle::AngleFormat ParseAngle::parseAngleFormat(const string &formatdef)
{
    AngleFormat format = AF_DEGREES;

    if (boost::iequals(formatdef, "deg_angles") || boost::iequals(formatdef, "degrees") || boost::iequals(formatdef, "deg"))
    {
        format = AF_DEGREES;
    }
    else if (boost::iequals(formatdef, "dms_angles") || boost::iequals(formatdef, "dms"))
    {
        format = AF_DMS;
    }
    else if (boost::iequals(formatdef, "hp_angles") || boost::iequals(formatdef, "hp"))
    {
        format = AF_HPFORMAT;
    }
    else
    {
        throw ParseAngleException(string("Invalid angle format ") + formatdef);
    }
    return format;
}

double ParseAngle::parseAngle(AngleFormat format, const string &value, const string &sign)
{
    double angle = 0.0;
    RGX::smatch match;
    string tvalue = value;
    if (format == AF_DEGREES)
    {
        angle = atof(value.c_str());
        return angle;
    }
    if (format == AF_HPFORMAT)
    {
        if (tvalue.find('.') == string::npos) tvalue += '.';
        tvalue += "0000";
    }
    const RGX::regex &formatre = (format == AF_HPFORMAT) ? hp_regex : dms_regex;

    if (!RGX::regex_match(tvalue, match, formatre))
    {
        throw ParseAngleException(string("Invalid angle degrees/minutes/seconds: ") + value);
    }
    else
    {
        int deg = atoi(match[2].str().c_str());
        int min = atoi(match[3].str().c_str());
        double sec = atof((match[4].str() + "." + match[5].str()).c_str());
        angle = deg + min / 60.0 + sec / 3600.0;
        string hem = match[1].str() + match[6].str();
        if (hem.length() > 0)
        {
            bool ok = false;
            if (sign.length() == 2 && hem.length() == 1)
            {
                ok = true;
                boost::to_upper(hem);
                if (hem[0] == '-' || hem[0] == sign[1])
                {
                    angle = -angle;
                }
                else if (hem[0] != sign[0])
                {
                    ok = false;
                }
            }
            if (!ok)
            {
                throw ParseAngleException(string("Badly formatted angle: " + value));
            }
        }
    }
    return angle;
}


#ifndef __terark_bitfield_array_hpp__
#define __terark_bitfield_array_hpp__

#include <assert.h>
#include <terark/stdtypes.hpp>
#include <terark/valvec.hpp>
#include <boost/preprocessor/cat.hpp>

namespace terark {

namespace detail {

template<int... ArgList> struct GetAllSum;
template<int Arg0, int... ArgList>
struct GetAllSum<Arg0, ArgList...> {
	enum { value = Arg0 + GetAllSum<ArgList...>::value };
};
template<> struct GetAllSum<> { enum { value = 0 }; };

template<int Arg0, int... ArgList>
struct LastArgValue {
	enum { value = LastArgValue<ArgList...>::value };
};
template<int Arg0>
struct LastArgValue<Arg0> { enum { value = Arg0 }; };

template<int Nth, int Arg0, int... ArgList>
struct PrefixSum {
	enum { value = Arg0 + PrefixSum<Nth-1, ArgList...>::value };
};
template<int Arg0, int... ArgList>
struct PrefixSum<0, Arg0, ArgList...> { enum { value = 0 }; };

template<bool Test, class Then, class Else> struct IF;
template<class Then, class Else> struct IF<1,Then,Else> {typedef Then type;};
template<class Then, class Else> struct IF<0,Then,Else> {typedef Else type;};

template<int Nth, int Arg0, int... ArgList>
struct GetNthArg {
	enum { value = GetNthArg<Nth-1, ArgList...>::value };
};
template<int Arg0, int... ArgList>
struct GetNthArg<0, Arg0, ArgList...> { enum { value = Arg0 }; };

template<int Arg0, int... ArgList>
struct MaxValue {
	enum { temp = MaxValue<ArgList...>::value };
	enum { value = Arg0 > temp ? Arg0 : temp };
};
template<int Arg0>
struct MaxValue<Arg0> { enum { value = Arg0 }; };

template<int Arg0, int... ArgList>
struct MinValue {
	enum { temp = MinValue<ArgList...>::value };
	enum { value = Arg0 < temp ? Arg0 : temp };
};
template<int Arg0>
struct MinValue<Arg0> { enum { value = Arg0 }; };

template<int Bits>
struct AllOne {
	static_assert(Bits >=  1, "Bits must >= 1");
	static_assert(Bits <= 64, "Bits must <= 64");
	typedef typename IF<(Bits<=8)
	  , unsigned char
	  ,	typename IF<(Bits<32), uint32_t, uint64_t>::type
	>::type type;
   	static const type value = (type(1) << Bits) - 1;
};
template<>struct AllOne<32> {
	typedef uint32_t type;
	static const type value = type(-1);
};
template<>struct AllOne<64> {
	typedef uint64_t type;
	static const type value = type(-1);
};

#if 1 && ( \
	defined(__i386__) || defined(__i386) || defined(_M_IX86) || \
	defined(__X86__) || defined(_X86_) || \
	defined(__THW_INTEL__) || defined(__I86__) || \
	defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || defined(__x86_64) || defined(_M_X64) \
	)
  #define BITFIELD_ARRAY_FAST_MISALIGN
#endif

template<int... BitFieldsBits>
class bitfield_array {
	static_assert(sizeof...(BitFieldsBits) > 0, "BitFieldsBits... Must Have At Least One BitField");
	static_assert(MaxValue<BitFieldsBits...>::value <= 64, "BitField bits must <= 64");
	static_assert(MinValue<BitFieldsBits...>::value >=  1, "BitField bits must >= 1");
	template<int NthField>
	struct BestUint {
		typedef typename
		AllOne<GetNthArg<NthField, BitFieldsBits...>::value>::type type;
	};
	valvec<unsigned char> m_bytes;
	size_t m_size;

public:
	enum { TotalBits = GetAllSum<BitFieldsBits...>::value };
	bitfield_array() : m_size(0) {}
	explicit bitfield_array(size_t num_tuples) : m_size(num_tuples) {
		m_bytes.resize(compute_mem_size(num_tuples));
	}

	const unsigned char* data() const { return m_bytes.data(); }
	size_t size() const { return m_size; }

	// align memory size to 16 bytes
	static size_t compute_mem_size(size_t num_tuples) {
		size_t  required = (num_tuples * TotalBits + 64 + 63) / 64 * 8;
		return (required + 15) & ~size_t(15); // align to 16
	}

	size_t mem_size() const { return m_bytes.size(); }

	void resize(size_t newsize) {
		m_bytes.resize(compute_mem_size(newsize));
		m_size = newsize;
	}
	void resize_no_init(size_t newsize) {
		m_bytes.resize_no_init(compute_mem_size(newsize));
		m_size = newsize;
	}

	void risk_release_ownership() {
		m_bytes.risk_release_ownership();
		m_size = 0;
	}
	void risk_set_data(const void* vdata, size_t num_tuples) {
		byte_t* data = (byte_t*)vdata;
		m_bytes.risk_set_data(data, compute_mem_size(num_tuples));
		m_size = num_tuples;
	}

	void swap(bitfield_array& y) {
		m_bytes.swap(y.m_bytes);
		std::swap(m_size, y.m_size);
	}

	void clear() {
		m_bytes.clear();
		m_size = 0;
	}

	template<class... ArgList>
	void emplace_back(const ArgList... args) {
		static_assert(sizeof...(ArgList) == sizeof...(BitFieldsBits)
				, "ArgList Length Must Match");
		size_t idx = m_size;
		resize(m_size + 1);
		pset<0, ArgList...>(idx, args...);
	}

	template<class UintType>
	typename std::enable_if
		< sizeof...(BitFieldsBits)==1 && std::is_integral<UintType>::value
		, void
		>::type
	push_back(UintType val) {
		size_t idx = m_size;
		resize(m_size + 1);
		set0(idx, val);
	}

	template<class... Uints>
	void aset(size_t idx, Uints... fields) {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		aligned_pset<0>(m_bytes.data(), idx, fields...);
#else
		unaligned_pset<0>(m_bytes.data(), idx, fields...);
#endif
	}
	template<int FirstField, class... Uints>
	void pset(size_t idx, Uints... fields) {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		aligned_pset<FirstField>(m_bytes.data(), idx, fields...);
#else
		unaligned_pset<FirstField>(m_bytes.data(), idx, fields...);
#endif
	}

	template<int NthField>
	typename BestUint<NthField>::type get(size_t idx) const {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		return aligned_get<NthField>(m_bytes.data(), idx);
#else
		return unaligned_get<NthField>(m_bytes.data(), idx);
#endif
	}

	template<int NthField>
	void set(size_t idx, typename BestUint<NthField>::type val) {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		aligned_set<NthField>(m_bytes.data(), idx, val);
#else
		unaligned_set<NthField>(m_bytes.data(), idx, val);
#endif
	}

	typename BestUint<0>::type get0(size_t idx) const {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		return aligned_get<0>(m_bytes.data(), idx);
#else
		return unaligned_get<0>(m_bytes.data(), idx);
#endif
	}

	template<class IndexType>
	typename std::enable_if
		< sizeof...(BitFieldsBits)==1 && std::is_integral<IndexType>::value
		, typename BestUint<0>::type
		>::type
	operator[](IndexType idx) const {
		assert(size_t(idx) < m_size);
		return get0(idx);
	}

	void set0(size_t idx, typename BestUint<0>::type val) {
		assert(idx < m_size);
#if defined(BITFIELD_ARRAY_FAST_MISALIGN)
		aligned_set<0>(m_bytes.data(), idx, val);
#else
		unaligned_set<0>(m_bytes.data(), idx, val);
#endif
	}

#define aligned_load unaligned_load
#define aligned_save unaligned_save
#define get_func     unaligned_get
#define set_func     unaligned_set
#define pget_func    unaligned_pget
#define pset_func    unaligned_pset
#define aset_func    unaligned_aset
#include "bitfield_array_access.hpp"
#undef aligned_load
#undef aligned_save
#undef get_func
#undef set_func
#undef pget_func
#undef pset_func
#undef aset_func

#define get_func     aligned_get
#define set_func     aligned_set
#define pget_func    aligned_pget
#define pset_func    aligned_pset
#define aset_func    aligned_aset
#include "bitfield_array_access.hpp"
#undef get_func
#undef set_func
#undef pget_func
#undef pset_func
#undef aset_func

};

} // namespace detail
using detail::bitfield_array;

} // namespace terark

using terark::detail::bitfield_array;

#endif // __terark_bitfield_array_hpp__