//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef CLOTHO_BATCH_CROSSOVER_TASKWE_HPP_
#define CLOTHO_BATCH_CROSSOVER_TASKWE_HPP_

#include "clotho/data_spaces/task/task.hpp"
#include "clotho/data_spaces/task/thread_pool.hpp"

#include <boost/random/bernoulli_distribution.hpp>
#include "clotho/data_spaces/crossover/position_classifier.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"
#include "clotho/data_spaces/crossover/crossover_task_list.hpp"

namespace clotho {
namespace genetics {

template <  class MatePairType, class SequenceSpaceType, class AlleleSpaceType >
class batch_crossover_taskwe;

/* : public task {
public:
    typedef batch_crossover_taskwe< MatePairType, SequenceSpaceType, AlleleSpaceType >   self_type;

    typedef SequenceSpaceType                               sequence_space_type;
    typedef AlleleSpaceType                                 allele_type;
    typedef RNG                                             random_engine_type;

    typedef typename sequence_space_type::sequence_vector      sequence_vector;
    typedef typename sequence_space_type::raw_block_pointer    sequence_iterator;
//    typedef typename sequence_space_type::individual_id_type   individual_id_type;

//    typedef std::vector< std::pair< individual_id_type, individual_id_type > >  mate_pair_type;
    typedef MatePairType                                        mate_pair_type;
    typedef typename mate_pair_type::iterator                   iterator;
    typedef typename mate_pair_type::const_iterator             const_iterator;

    typedef PositionClassifier< typename allele_type::position_vector >          classifier_type;
    typedef typename classifier_type::event_type                                event_type;

    typedef typename position_distribution_helper< typename allele_type::position_type >::type position_distribution_type;
    typedef typename crossover_event_distribution_helper< double >::type event_distribution_type;

    batch_crossover_taskwe ( random_engine_type * rng, sequence_space_type * parent, sequence_space_type * offspring, allele_type * alleles, unsigned int off_idx, const_iterator first, const_iterator last, double recomb_rate, double seq_bias) :
        m_rng(rng)
        , m_parental( parent )
        , m_offspring( offspring )
        , m_alleles( alleles )
        , m_parents(first, last )
        , m_offspring_index(off_idx)
        , m_recomb_rate(recomb_rate)
        , m_seq_bias( seq_bias )
    { }

    batch_crossover_taskwe( const self_type & other ) :
        m_rng( other.m_rng )
        , m_parental( other.m_parental )
        , m_offspring( other.m_offspring )
        , m_alleles( other.m_alleles )
        , m_parents( other.m_parents )
        , m_offspring_index( other.m_offspring_index )
        , m_recomb_rate( other.m_recomb_rate )
        , m_seq_bias( other.m_seq_bias )
    { }

    void operator()() {
        event_distribution_type     event_dist( m_recomb_rate);
        boost::random::bernoulli_distribution< double > bias_dist( m_seq_bias);

        iterator mate_it = m_parents.begin(), mate_end = m_parents.end();

#ifdef DEBUGGING
        BOOST_LOG_TRIVIAL(debug) << "Starting batch crossover task";
#endif // DEBUGGING
        size_t i = 2 * m_offspring_index;
        while( mate_it != mate_end ) {

            unsigned int idx = 2 * mate_it->first;

            assert( idx < m_parental->row_count() );

            sequence_vector s0 = m_parental->getSequence( idx++ );
            sequence_vector s1 = m_parental->getSequence( idx );

            sequence_vector c = m_offspring->getSequence( i++ );

            event_type evts, evts1;

            fill_events( evts, event_dist( *m_rng ) );
            classifier_type cfier0( &m_alleles->getPositions(), evts );
            bool _swap = bias_dist( *m_rng );

#ifdef DEBUGGING
            BOOST_LOG_TRIVIAL(debug) << mate_it->first << "; Crossover s0: " << s0.first << " - " << s0.second << "; s1: " << s1.first << " - " << s1.second << "; child: " << c.first << "; event size: " << evts.size();
#endif  // DEBUGGING
            run_crossover_task( cfier0, s0.first, s0.second, s1.first, s1.second, c.first, _swap );

            idx = 2 * mate_it->second;
            assert( idx < m_parental->row_count() );

            s0 = m_parental->getSequence( idx++ );
            s1 = m_parental->getSequence( idx );

            c = m_offspring->getSequence( i++ );
#ifdef DEBUGGING
            BOOST_LOG_TRIVIAL(debug) << mate_it->second << "; Crossover s0: " << s0.first << " - " << s0.second << "; s1: " << s1.first << " - " << s1.second << "; child: " << c.first << "; event size: " << evts.size();
#endif  // DEBUGGING

            fill_events( evts1, event_dist( *m_rng ) );

            classifier_type cfier1( &m_alleles->getPositions(), evts1 );
            _swap = bias_dist( *m_rng );
            run_crossover_task( cfier1, s0.first, s0.second, s1.first, s1.second, c.first, _swap);

            ++mate_it;
        }

#ifdef DEBUGGING
        BOOST_LOG_TRIVIAL(debug) << "End Batch Crossover Task";
#endif // DEBUGGING
    }

    virtual ~batch_crossover_taskwe() {}

protected:

    inline void fill_events( event_type & evt, unsigned int N ) {
        while( N-- ) {
            evt.push_back( m_pos_dist( *m_rng ) );
        }
    }

    event_type make_events( unsigned int N ) {
        event_type res;

        while( N-- ) {
            res.push_back( m_pos_dist( *m_rng ) );
        }

        return res;
    }

    template < class Classifier, class BlockType >
    void run_crossover_task( const Classifier & cls, BlockType * s0, unsigned int s0_len, BlockType * s1, unsigned int s1_len, BlockType * res ) {
        typedef copy_crossover_task< BlockType > copy_type;
        typedef segment_crossover_task< Classifier, BlockType > segment_type;
        typedef tail_crossover_task< Classifier, BlockType, top_strand_tail > top_tail_type;
        typedef tail_crossover_task< Classifier, BlockType, bottom_strand_tail > bottom_tail_type;

        if( s0_len == 0 && s1_len == 0 ) return;

        if( cls.event_count() == 0 ) {
            // there are no crossover cls
            // therefore, offspring strand will be a copy of the top strand
            copy_type t( s0, res, s0_len );
            t();
        } else if( s0_len < s1_len ) {
            // top strand is shorter than bottom strand
            segment_type s( cls, s0, s1, res, 0, s0_len );
            s();

            bottom_tail_type t( cls, s1, res, s0_len, s1_len );
            t();
        } else if( s1_len < s0_len ) {
            // bottom strand is shorter than top strand
            segment_type s( cls, s0, s1, res, 0, s1_len );
            s();

            top_tail_type t( cls, s0, res, s1_len, s0_len );
            t();
        } else {
            // both strands are equal in length
            segment_type s( cls, s0, s1, res, 0, s0_len );
            s();
        }
    }

    template < class Classifier, class BlockType >
    void run_crossover_task( const Classifier & cls, BlockType * s0, unsigned int s0_len, BlockType * s1, unsigned int s1_len, BlockType * res, bool should_swap_strands ) {

        if( should_swap_strands ) {
            run_crossover_task( cls, s1, s1_len, s0, s0_len, res );
        } else {
            run_crossover_task( cls, s0, s0_len, s1, s1_len, res );
        }
    }

    random_engine_type  * m_rng;
    sequence_space_type * m_parental, * m_offspring;
    allele_type         * m_alleles;

    mate_pair_type m_parents;

    unsigned int m_offspring_index;

    double m_recomb_rate, m_seq_bias;

    position_distribution_type  m_pos_dist;
};
*/

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_BATCH_CROSSOVER_TASKWE_HPP_


///* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
//
///*
// Copyright (C) 2014 Jose Aparicio
//
// 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.
//*/
//
//#include <boost/make_shared.hpp>
//
//#include <qlo/objects/credit/defaultbasket.hpp>
//
//#include <ql/currencies/europe.hpp>
//
//#include <ql/experimental/credit/basket.hpp>
//
//
//namespace QuantLibAddin {
//
//    /* \todo: Add amortizing version
//    */
//    Basket::Basket(
//        const boost::shared_ptr<reposit::ValueObject>& properties,
//
//        const std::vector<std::string>& names,
//        const std::vector<boost::shared_ptr<QuantLib::Issuer> >& issuers,
//        const std::vector<QuantLib::Real>& notionals,
//        const QuantLib::Date& refDate,
//        QuantLib::Real attachRatio,
//        QuantLib::Real detachRatio,
//        bool amortizes,
//
//        bool permanent
//        )
//        : reposit::LibraryObject<QuantLib::Basket>(properties, permanent)
//    {
//        QL_REQUIRE(names.size() == issuers.size(),
//            "Different number of names and issuers.");
//
//        std::vector<QuantLib::DefaultProbKey> contractTriggers(
//            names.size(),
//            QuantLib::NorthAmericaCorpDefaultKey(QuantLib::EURCurrency(),
//                QuantLib::SeniorSec,
//                QuantLib::Period(),
//                1. // amount threshold
//            ));
//
//        boost::shared_ptr<QuantLib::Pool> pool(new QuantLib::Pool());
//        for(QuantLib::Size i=0; i<names.size(); i++)
//                pool->add(names[i], *issuers[i], contractTriggers[i]);
//        libraryObject_ =
//            boost::make_shared<QuantLib::Basket>(
//                QuantLib::Basket(refDate, names, notionals, pool,
//                    attachRatio, detachRatio));
//    }
//
//}


#pragma once
#include <armadillo>
#include "project/vecPush.hpp"

void vec_push(arma::vec &v, float value) {
    arma::vec av(1);
    av.at(0) = value;
    v.insert_rows(v.n_rows, av.row(0));
}


// Copyright (C) 2015, Pawel Tomulik <ptomulik@meil.pw.edu.pl>
//
// 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)
#define BOOST_TEST_MODULE test_txpl_vm_eval_binary_bitor

#include <txpl/test_config.hpp>
#include <boost/test/unit_test.hpp>

#ifndef TXPL_TEST_SKIP_VM_EVAL_BINARY_BITOR

#include <txpl/vm/eval_binary_op.hpp>
#include <txpl/vm/basic_types.hpp>
#include <txpl/vm/value.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>

using namespace txpl::vm;
typedef basic_types<>::char_type   char_type;
typedef basic_types<>::int_type    int_type;
typedef basic_types<>::bool_type   bool_type;
typedef basic_types<>::real_type   real_type;
typedef basic_types<>::string_type string_type;
typedef basic_types<>::regex_type  regex_type;
typedef basic_types<>::blank_type  blank_type;
typedef array<value<> >            array_type;
typedef object<value<> >           object_type;

BOOST_AUTO_TEST_CASE(char__bitor__char)
{
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\0'} | char_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\0'} | int_type{0}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\0'} | bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'\0'};
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(int__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{0} | char_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0x1200};
    const value<> v2 = int_type{0x0034};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{0x1200} | int_type{0x0034}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{0} | bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(bool__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{false} | bool_type{'\0'}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{false} | int_type{0}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{false} | bool_type{false}));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(real__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(string__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(regex__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(array__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(object__bitor__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__bitor__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::bitor_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

#else
BOOST_AUTO_TEST_CASE(dummy)
{
  BOOST_CHECK(true);
}
#endif


/*
*   open .obj file and read get the graph
*   in .obj files, edges are noted "l" (for line)
*                  vertices are noted "v"
*
*   this file is scavenged from the readOBJ file of libIGL
*   the modifications are:
*    - remove the faces and add edge loading
*    - remove dependencies to exterior files (libIGL)
*    - change the #define tag
*
*   by R. Falque
*   10/05/2019
*/

#ifndef WRITE_GRAPH_OBJ_HPP
#define WRITE_GRAPH_OBJ_HPP

#include <Eigen/Core>
#include <vector>

#include <string>
#include <cstdio>

#include <iostream>
#include <fstream>
#include <sstream>
#include <iterator>

namespace libgraphcpp
{
    inline bool writeGraphOBJ(const Eigen::MatrixXd & V_in, const Eigen::MatrixXi & E_in, const std::string obj_file_name){
        bool verbose = false;

        using namespace std;
        using namespace Eigen;
        assert(V_in.cols() == 3 && "V should have 3 columns");
        ofstream s(obj_file_name);
        if(!s.is_open())
        {
            fprintf(stderr,"IOError: writeOBJ() could not open %s\n",obj_file_name.c_str());
            return false;
        }
        s<<
        V_in.format(IOFormat(FullPrecision,DontAlignCols," ","\n","v ","","","\n"))<<
        (E_in.array()+1).format(IOFormat(FullPrecision,DontAlignCols," ","\n","l ","","","\n"));
        return true;
    };
}

#endif


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    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)
=============================================================================*/
#if !defined(BOOST_SPIRIT_UTREE_OPERATORS)
#define BOOST_SPIRIT_UTREE_OPERATORS

#include <utree/utree.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/type_traits/is_integral.hpp>

#if defined(BOOST_MSVC)
# pragma warning(push)
# pragma warning(disable: 4804)
# pragma warning(disable: 4805)
#endif

#include <exception>
#include <utree/utree.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/type_traits/is_integral.hpp>

namespace scheme
{
    struct utree_exception : std::exception {};

    struct illegal_arithmetic_operation : utree_exception
    {
        virtual const char* what() const throw()
        {
            return "utree: Illegal arithmetic operation.";
        }
    };

    struct illegal_integral_operation : utree_exception
    {
        virtual const char* what() const throw()
        {
            return "utree: Illegal integral operation.";
        }
    };

    // Relational operators
    bool operator==(utree const& a, utree const& b);
    bool operator<(utree const& a, utree const& b);
    bool operator!=(utree const& a, utree const& b);
    bool operator>(utree const& a, utree const& b);
    bool operator<=(utree const& a, utree const& b);
    bool operator>=(utree const& a, utree const& b);

    // Input and output
    std::ostream& operator<<(std::ostream& out, utree const& x);
    std::istream& operator>>(std::istream& in, utree& x);

    std::ostream& operator<<(std::ostream& out, nil const& x);

    // Logical operators
    utree operator&&(utree const& a, utree const& b);
    utree operator||(utree const& a, utree const& b);
    utree operator!(utree const& a);

    // Arithmetic operators
    utree operator+(utree const& a, utree const& b);
    utree operator-(utree const& a, utree const& b);
    utree operator*(utree const& a, utree const& b);
    utree operator/(utree const& a, utree const& b);
    utree operator%(utree const& a, utree const& b);
    utree operator-(utree const& a);

    // Bitwise operators
    utree operator&(utree const& a, utree const& b);
    utree operator|(utree const& a, utree const& b);
    utree operator^(utree const& a, utree const& b);
    utree operator<<(utree const& a, utree const& b);
    utree operator>>(utree const& a, utree const& b);
    utree operator~(utree const& a);

    // Implementation
    struct utree_is_equal
    {
        typedef bool result_type;

        template <typename A, typename B>
        bool dispatch(const A&, const B&, boost::mpl::false_) const
        {
            return false; // cannot compare different types by default
        }

        template <typename A, typename B>
        bool dispatch(const A& a, const B& b, boost::mpl::true_) const
        {
            return a == b; // for arithmetic types
        }

        template <typename A, typename B>
        bool operator()(const A& a, const B& b) const
        {
            return dispatch(a, b,
                boost::mpl::and_<
                    boost::is_arithmetic<A>,
                    boost::is_arithmetic<B> >());
        }

        template <typename T>
        bool operator()(const T& a, const T& b) const
        {
            // This code works for lists
            return a == b;
        }

        template <typename Base, utree_type::info type_>
        bool operator()(
            basic_string<Base, type_> const& a,
            basic_string<Base, type_> const& b) const
        {
            return static_cast<Base const&>(a) == static_cast<Base const&>(b);
        }

        bool operator()(nil, nil) const
        {
            return true;
        }

        bool operator()(function_base const& a, function_base const& b) const
        {
            return false; // just don't allow comparison of functions
        }
    };

    struct utree_is_less_than
    {
        typedef bool result_type;

        template <typename A, typename B>
        bool dispatch(const A&, const B&, boost::mpl::false_) const
        {
            return false; // cannot compare different types by default
        }

        template <typename A, typename B>
        bool dispatch(const A& a, const B& b, boost::mpl::true_) const
        {
            return a < b; // for arithmetic types
        }

        template <typename A, typename B>
        bool operator()(const A& a, const B& b) const
        {
            return dispatch(a, b,
                boost::mpl::and_<
                    boost::is_arithmetic<A>,
                    boost::is_arithmetic<B> >());
        }

        template <typename T>
        bool operator()(const T& a, const T& b) const
        {
            // This code works for lists
            return a < b;
        }

        template <typename Base, utree_type::info type_>
        bool operator()(
            basic_string<Base, type_> const& a,
            basic_string<Base, type_> const& b) const
        {
            return static_cast<Base const&>(a) < static_cast<Base const&>(b);
        }

        bool operator()(nil, nil) const
        {
            BOOST_ASSERT(false);
            return false; // no less than comparison for nil
        }

        bool operator()(any_ptr const& a, any_ptr const& b) const
        {
            BOOST_ASSERT(false);
            return false; // no less than comparison for any_ptr
        }

        bool operator()(function_base const& a, function_base const& b) const
        {
            BOOST_ASSERT(false);
            return false; // no less than comparison of functions
        }
    };

#if !defined(SCHEME_USE_SPIRIT_IO)

    struct utree_print
    {
        typedef void result_type;

        std::ostream& out;
        utree_print(std::ostream& out) : out(out) {}

        void operator()(scheme::nil) const
        {
            out << "<nil> ";
        }

        template <typename T>
        void operator()(T val) const
        {
            out << val << ' ';
        }

        void operator()(bool b) const
        {
            out << (b ? "true" : "false") << ' ';
        }

        void operator()(binary_range const& b) const
        {
            out << "#";
            out.width(2);
            out.fill('0');

            typedef binary_range::const_iterator iterator;
            for (iterator i = b.begin(); i != b.end(); ++i)
                out << std::hex << int((unsigned char)*i);
            out << std::dec << "# ";
        }

        void operator()(utf8_string_range const& str) const
        {
            typedef utf8_string_range::const_iterator iterator;
            iterator i = str.begin();
            out << '"';
            for (; i != str.end(); ++i)
                out << *i;
            out << "\" ";
        }

        void operator()(utf8_symbol_range const& str) const
        {
            typedef utf8_symbol_range::const_iterator iterator;
            iterator i = str.begin();
            for (; i != str.end(); ++i)
                out << *i;
        }

        template <typename Iterator>
        void operator()(boost::iterator_range<Iterator> const& range) const
        {
            typedef typename boost::iterator_range<Iterator>::const_iterator iterator;
            (*this)('(');
            for (iterator i = range.begin(); i != range.end(); ++i)
            {
                scheme::utree::visit(*i, *this);
            }
            (*this)(')');
        }

        void operator()(any_ptr const& p) const
        {
            return (*this)("<pointer>");
        }

        void operator()(function_base const& pf) const
        {
            return (*this)("<function>");
        }
    };
#endif

    template <typename Base>
    struct logical_function
    {
        typedef utree result_type;

        // In scheme, anything except false is true

        // binary
        utree operator()(bool a, bool b) const
        {
            return Base::eval(a, b); // for boolean types
        }

        // binary
        template <typename A>
        utree operator()(A const& a, bool b) const
        {
            return Base::eval(true, b);
        }

        // binary
        template <typename B>
        utree operator()(bool a, B const& b) const
        {
            return Base::eval(a, true);
        }

        // binary
        template <typename A, typename B>
        utree operator()(A const& a, B const& b) const
        {
            return Base::eval(true, true);
        }

        // unary
        utree operator()(bool a) const
        {
            return Base::eval(a);
        }

        // unary
        template <typename A>
        utree operator()(A const& a) const
        {
            return Base::eval(true);
        }
    };

    template <typename Base>
    struct arithmetic_function
    {
        typedef utree result_type;

        template <typename A, typename B>
        utree dispatch(A const&, B const&, boost::mpl::false_) const
        {
            throw illegal_arithmetic_operation();
            return utree(); // cannot apply to non-arithmetic types
        }

        template <typename A, typename B>
        utree dispatch(A const& a, B const& b, boost::mpl::true_) const
        {
            return Base::eval(a, b); // for arithmetic types
        }

        // binary
        template <typename A, typename B>
        utree operator()(A const& a, B const& b) const
        {
            return dispatch(a, b,
                boost::mpl::and_<
                    boost::is_arithmetic<A>,
                    boost::is_arithmetic<B> >());
        }

        template <typename A>
        utree dispatch(A const&, boost::mpl::false_) const
        {
            throw illegal_arithmetic_operation();
            return utree(); // cannot apply to non-arithmetic types
        }

        template <typename A>
        utree dispatch(A const& a, boost::mpl::true_) const
        {
            return Base::eval(a); // for arithmetic types
        }

        // unary
        template <typename A>
        utree operator()(A const& a) const
        {
            return dispatch(a, boost::is_arithmetic<A>());
        }
    };

    template <typename Base>
    struct integral_function
    {
        typedef utree result_type;

        template <typename A, typename B>
        utree dispatch(A const&, B const&, boost::mpl::false_) const
        {
            throw illegal_integral_operation();
            return utree(); // cannot apply to non-integral types
        }

        template <typename A, typename B>
        utree dispatch(A const& a, B const& b, boost::mpl::true_) const
        {
            return Base::eval(a, b); // for integral types
        }

        // binary
        template <typename A, typename B>
        utree operator()(A const& a, B const& b) const
        {
            return dispatch(a, b,
                boost::mpl::and_<
                    boost::is_integral<A>,
                    boost::is_integral<B> >());
        }

        template <typename A>
        utree dispatch(A const&, boost::mpl::false_) const
        {
            throw illegal_integral_operation();
            return utree(); // cannot apply to non-integral types
        }

        template <typename A>
        utree dispatch(A const& a, boost::mpl::true_) const
        {
            return Base::eval(a); // for integral types
        }

        // unary
        template <typename A>
        utree operator()(A const& a) const
        {
            return dispatch(a, boost::is_integral<A>());
        }
    };

#define SCHEME_CREATE_FUNCTION(name, expr, base)                                \
    struct BOOST_PP_CAT(function_impl_, name)                                   \
    {                                                                           \
        template <typename A, typename B>                                       \
        static utree eval(A const& a, B const& b)                               \
        {                                                                       \
            return utree(expr);                                                 \
        }                                                                       \
        template <typename A>                                                   \
        static utree eval(A const& a)                                           \
        {                                                                       \
            static int b;                                                       \
            (void) b;                                                           \
            return utree(expr);                                                 \
        }                                                                       \
    };                                                                          \
    base<BOOST_PP_CAT(function_impl_, name)> const                              \
        BOOST_PP_CAT(base, BOOST_PP_CAT(_, name)) = {};                         \
    /***/

#define SCHEME_CREATE_ARITHMETIC_FUNCTION(name, expr)                           \
    SCHEME_CREATE_FUNCTION(name, expr, arithmetic_function)                     \
    /***/

#define SCHEME_CREATE_INTEGRAL_FUNCTION(name, expr)                             \
    SCHEME_CREATE_FUNCTION(name, expr, integral_function)                       \
    /***/

#define SCHEME_CREATE_LOGICAL_FUNCTION(name, expr)                              \
    SCHEME_CREATE_FUNCTION(name, expr, logical_function)                        \
    /***/

    inline bool operator==(utree const& a, utree const& b)
    {
        return utree::visit(a, b, utree_is_equal());
    }

    inline bool operator<(utree const& a, utree const& b)
    {
        return utree::visit(a, b, utree_is_less_than());
    }

    inline bool operator!=(utree const& a, utree const& b)
    {
        return !(a == b);
    }

    inline bool operator>(utree const& a, utree const& b)
    {
        return b < a;
    }

    inline bool operator<=(utree const& a, utree const& b)
    {
        return !(b < a);
    }

    inline bool operator>=(utree const& a, utree const& b)
    {
        return !(a < b);
    }

#if !defined(SCHEME_USE_SPIRIT_IO)
    inline std::ostream& operator<<(std::ostream& out, utree const& x)
    {
        utree::visit(x, utree_print(out));
        return out;
    }
#endif

    inline std::ostream& operator<<(std::ostream& out, nil const& x)
    {
        return out;
    }

    SCHEME_CREATE_LOGICAL_FUNCTION(and_, a&&b);
    SCHEME_CREATE_LOGICAL_FUNCTION(or_, a||b);
    SCHEME_CREATE_LOGICAL_FUNCTION(not_, !a);

    SCHEME_CREATE_ARITHMETIC_FUNCTION(plus, a+b);
    SCHEME_CREATE_ARITHMETIC_FUNCTION(minus, a-b);
    SCHEME_CREATE_ARITHMETIC_FUNCTION(times, a*b);
    SCHEME_CREATE_ARITHMETIC_FUNCTION(divides, a/b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(modulus, a%b);
    SCHEME_CREATE_ARITHMETIC_FUNCTION(negate, -a);

    SCHEME_CREATE_INTEGRAL_FUNCTION(bitand_, a&b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(bitor_, a|b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(bitxor_, a^b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(shift_left, a<<b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(shift_right, a>>b);
    SCHEME_CREATE_INTEGRAL_FUNCTION(invert, ~a);

    inline utree operator&&(utree const& a, utree const& b)
    {
        return utree::visit(a, b, logical_function_and_);
    }

    inline utree operator||(utree const& a, utree const& b)
    {
        return utree::visit(a, b, logical_function_or_);
    }

    inline utree operator!(utree const& a)
    {
        return utree::visit(a, logical_function_not_);
    }

    inline utree operator+(utree const& a, utree const& b)
    {
        return utree::visit(a, b, arithmetic_function_plus);
    }

    inline utree operator-(utree const& a, utree const& b)
    {
        return utree::visit(a, b, arithmetic_function_minus);
    }

    inline utree operator*(utree const& a, utree const& b)
    {
        return utree::visit(a, b, arithmetic_function_times);
    }

    inline utree operator/(utree const& a, utree const& b)
    {
        return utree::visit(a, b, arithmetic_function_divides);
    }

    inline utree operator%(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_modulus);
    }

    inline utree operator-(utree const& a)
    {
        return utree::visit(a, arithmetic_function_negate);
    }

    inline utree operator&(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_bitand_);
    }

    inline utree operator|(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_bitor_);
    }

    inline utree operator^(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_bitxor_);
    }

    inline utree operator<<(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_shift_left);
    }

    inline utree operator>>(utree const& a, utree const& b)
    {
        return utree::visit(a, b, integral_function_shift_right);
    }

    inline utree operator~(utree const& a)
    {
        return utree::visit(a, integral_function_invert);
    }
}

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

#endif


#include <boost/gil/cmyk.hpp>


/**
BSD 3-Clause License

This file is part of the code accompanying the paper
From Planes to Corners: Multi-Purpose Primitive Detection in Unorganized 3D Point Clouds
by C. Sommer, Y. Sun, L. Guibas, D. Cremers and T. Birdal,
accepted for Publication in IEEE Robotics and Automation Letters (RA-L) 2020.

Copyright (c) 2019, Christiane Sommer.
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 copyright holder 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.
*/

// includes
#include <iostream>
#include <fstream>
#include <string>
#include "definitions.h"
// libraries
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <CLI/CLI.hpp>
// classes
#include "Timer.h"
#include "Plane.h"
#include "graph/PlaneGraph.h"
#include "graph/ParallelPlaneGraph.h"
#include "PPF/PairDetector.h"
// function includes
#include "io/load_ply_cloud.h"
#include "visualize/pcshow.h"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <experimental/filesystem>

namespace fs = std::experimental::filesystem;

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

  int nr_volume=0;
  int nr_pointclouds=0;

  std::string string_pointcloud;
  std::ifstream MyReadFile2("../data/main_programs/config_files/input_file_plane_dataset.txt");
  std::getline (MyReadFile2, string_pointcloud);

  std::ofstream myfile("/home/alex-pop/Desktop/Doctorat/Side_projects/Volume_box_batch_processing/orthogonal-planes/data/output_volumes.txt");

  myfile<<"Hello"<<'\n';

	//fs::path path("/home/alex-pop/Desktop/Doctorat/Side_projects/Volume_box_batch_processing/orthogonal-planes/data/Pointcloud_recordings");
  fs::path path(string_pointcloud);
	for (auto& p : fs::directory_iterator(path))
	{

	std::stringstream ss;

    ss << p ;

   std::string str = ss.str();

   str.erase(std::remove(str.begin(), str.end(), '"'), str.end());

   std::cout<<str<<'\n';

   pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);

    if (pcl::io::loadPCDFile<pcl::PointNormal> (str, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  std::cout << "Loaded "
            << cloud->width * cloud->height
            << " data points from "<<str<<".pcd with the following fields: "
            << std::endl;

  std::vector<Eigen::Vector3f> points;
  std::vector<Eigen::Vector3f> normals;

  nr_pointclouds++;

  for (int i = 0; i < cloud->size(); i++)
    {

        Eigen::Vector3f position_aux= Eigen::Vector3f( (float) cloud->points[i].x,(float) cloud->points[i].y,(float) cloud->points[i].z);
        points.push_back(position_aux);

        Eigen::Vector3f normal_aux= Eigen::Vector3f( (float) cloud->points[i].normal_x,(float) cloud->points[i].normal_y,(float) cloud->points[i].normal_z);
        normals.push_back(normal_aux);
    }

     std::cout << points.size() << " points loaded." << std::endl;
     std::cout << normals.size() << " normals loaded." << std::endl;
    std::cout<<"\n";

    Timer T;

    // parse setttings from command line
    std::string ply_file;
    int min_votes = 5;
    double d_min = .1, d_max = 1.;
    int sampling = 10;

    std::ifstream MyReadFile("../data/main_programs/config_files/input_file_plane.txt");

    std::ifstream MyReadFile2("../data/main_programs/config_files/input_file_plane_dataset.txt");

    std::string min_votes_text;
    std::string d_min_text;
    std::string d_max_text;
    std::string sampling_text;
    std::string threshold_text;

    std::getline (MyReadFile, min_votes_text);
    std::getline (MyReadFile, d_min_text);
    std::getline (MyReadFile, d_max_text);
    std::getline (MyReadFile, sampling_text);
    std::getline (MyReadFile, threshold_text);

    min_votes=std::stoi(min_votes_text);
    std::cout<<"min_votes="<<min_votes<< std::endl;
    d_min=std::stod(d_min_text);
     std::cout<<"d_min="<<d_min<< std::endl;
     d_max=std::stod(d_max_text);
     std::cout<<"d_max="<<d_max<< std::endl;
     sampling=std::stoi(sampling_text);
     std::cout<<"sampling_text="<<sampling<< std::endl;
     double threshold=std::stod(threshold_text);
     std::cout<<"threshold lines="<<threshold<< std::endl;

     for(int nr_exec=0;nr_exec<3;nr_exec++)
     {

    // pairing
    ppf::PairDetector pairDet(d_max, d_min, min_votes);
    PlaneGraph planeMap(pairDet.para_thresh(), pairDet.distance_bin());
    T.tic();
    pairDet.detect_ortho_pairs(points, normals, planeMap);
    T.toc("time pairing PPF ");
    //planeMap.print_info();

    std::cout << std::endl << "Thresholds:\tAngle:\t" << pairDet.para_thresh() << "\tDistance:\t" << pairDet.distance_bin() << std::endl << std::endl;

    // clustering & filtering
    T.tic();
    planeMap.cluster_graph_vertices();
    T.toc("clustering planes");
    //planeMap.print_info();
    //planeMap.print_parameters();
    //planeMap.print_edges();

    // find triangles
    std::vector<Graph<Plane>::Triangle> triangles;
    T.tic();
    planeMap.find_triangles(triangles);
    T.toc("Finding triangles");
    //planeMap.print_triangles(triangles);

    // reduce graph to ParallelPlaneGraph
    T.tic();
    ParallelPlaneGraph redMap = planeMap.reduce_graph();
    T.toc("graph reduction");
    //redMap.print_info();
    //redMap.print_edges();
    T.tic();
    redMap.triangle_reduce();
    T.toc("triangle reduction");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    // CERES problem setup and solving
    double lambda = 0.01 * points.size();
    T.tic();
    redMap.refine_coarse_fine(points, normals, lambda, pairDet.distance_bin(), sampling);
    T.toc("Multi-plane parameter optimization");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    T.tic();
    redMap.filter_outliers(points, normals, pairDet.distance_bin(), .5*sampling);
    T.toc("outlier filtering");
    //redMap.print_info();
    //redMap.print_parameters();
    //redMap.print_edges();

    // From here on visualization / debugging

//    // get points located on planes and not-planes
    PlaneGraph finalMap = PlaneGraph::from_ppg(redMap, 2*pairDet.para_thresh()*pairDet.para_thresh()-1, pairDet.distance_bin()); // double angle
    std::vector<Eigen::Matrix<float, 6, 1>> labeled;
    T.tic();
    finalMap.go_thr_pcl(points, normals, labeled);
    T.toc("labeling planes");

    // visualize
   // pcshow(labeled, "Result");

   std::vector<Vec6> lines;
   // line extraction and visualization
   T.tic();
   lines = finalMap.extract_lines(points, normals);
   T.toc("get lines");
   //std::cout << lines.size() << " lines found." << std::endl;

   std::vector<Vec6> new_lines;

   std::vector<Vec6> final_lines;
   std::vector<double> distances;

   std::vector<double> Volume_edge;

   std::vector<int> positions_wrong;
   std::vector<double> Volumes;

    std::cout<<'\n';

  if (lines.size()>=3){
    for(int i = 0; i != lines.size()-2; i++) {
    //std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<lines[i][3]<<" "<<lines[i][4]<<" "<<lines[i][5]<<" "<<'\n';
      for(int j = i+1; j != lines.size()-1; j++) {
          for(int k = j+1; k != lines.size(); k++) {

                bool ok_ij=1;
                bool ok_jk=1;
                bool ok_ki=1;

                double average_0,average_1,average_2;

                if( (lines[i][0]==lines[j][0]) && (lines[i][1]==lines[j][1]) && (lines[i][2]==lines[j][2])) {ok_ij=0;}
                if( (lines[j][0]==lines[k][0]) && (lines[j][1]==lines[k][1]) && (lines[j][2]==lines[k][2])) {ok_jk=0;}
                if( (lines[k][0]==lines[i][0]) && (lines[k][1]==lines[i][1]) && (lines[k][2]==lines[i][2])) {ok_ki=0;}

                if (ok_ij && ok_jk && ok_ki){

              average_0=(lines[i][0]+lines[j][0]+lines[k][0]) / 3;
              average_1=(lines[i][1]+lines[j][1]+lines[k][1]) /3 ;
              average_2=(lines[i][2]+lines[j][2]+lines[k][2]) /3;

              double dist_i_average= sqrt (  (lines[i][0]-average_0)*(lines[i][0]-average_0)  +  (lines[i][1]-average_1)*(lines[i][1]-average_1) +(lines[i][2]-average_2)*(lines[i][2]-average_2)   );
              double dist_j_average= sqrt (  (lines[j][0]-average_0)*(lines[j][0]-average_0)  +  (lines[j][1]-average_1)*(lines[j][1]-average_1) +(lines[j][2]-average_2)*(lines[j][2]-average_2)   );
              double dist_k_average= sqrt (  (lines[k][0]-average_0)*(lines[k][0]-average_0)  +  (lines[k][1]-average_1)*(lines[k][1]-average_1) +(lines[k][2]-average_2)*(lines[k][2]-average_2)   );

              if(  (dist_i_average<threshold) && (dist_j_average<threshold) &&(dist_k_average<threshold) )
              {
                  /*
                  std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<'\n';
                  std::cout<< lines[j][0]<<" "<<lines[j][1]<<" "<<lines[j][2]<<" "<<'\n';
                  std::cout<< lines[k][0]<<" "<<lines[k][1]<<" "<<lines[k][2]<<" "<<'\n';
                  */

                  distances.push_back(dist_i_average);
                  distances.push_back(dist_j_average);
                  distances.push_back(dist_k_average);

                  /*
                  std::cout<<"Average:"<<average_0<<" "<<average_1<<" "<<average_2<<"\n";
                  std::cout<<"\n";
                  */
                  /*
                std::cout<< lines[i][0]<<" "<<lines[i][1]<<" "<<lines[i][2]<<" "<<lines[i][3]<<" "<<lines[i][4]<<" "<<lines[i][5]<<" "<<'\n';
                std::cout<< lines[j][0]<<" "<<lines[j][1]<<" "<<lines[j][2]<<" "<<lines[j][3]<<" "<<lines[j][4]<<" "<<lines[j][5]<<" "<<'\n';
                std::cout<< lines[k][0]<<" "<<lines[k][1]<<" "<<lines[k][2]<<" "<<lines[k][3]<<" "<<lines[k][4]<<" "<<lines[k][5]<<" "<<'\n';
                */
                double latura_i = sqrt (  (lines[i][0]-lines[i][3])*(lines[i][0]-lines[i][3])  +  (lines[i][1]-lines[i][4])*(lines[i][1]-lines[i][4]) +(lines[i][2]-lines[i][5])*(lines[i][2]-lines[i][5])   );
                double latura_j = sqrt (  (lines[j][0]-lines[j][3])*(lines[j][0]-lines[j][3])  +  (lines[j][1]-lines[j][4])*(lines[j][1]-lines[j][4]) +(lines[j][2]-lines[j][5])*(lines[j][2]-lines[j][5])   );
                double latura_k = sqrt (  (lines[k][0]-lines[k][3])*(lines[k][0]-lines[k][3])  +  (lines[k][1]-lines[k][4])*(lines[k][1]-lines[k][4]) +(lines[k][2]-lines[k][5])*(lines[k][2]-lines[k][5])   );

                double Volum = latura_i*latura_j*latura_k;
                 /*
                std::cout<<"Latura 1: "<<latura_1<<'\n';
                std::cout<<"Latura 2: "<<latura_3<<'\n';
                std::cout<<"Latura 3: "<<latura_3<<'\n';
                */

                //std::cout<<"Volum="<<Volum<<'\n';

                new_lines.push_back(lines[i]);
                new_lines.push_back(lines[j]);
                new_lines.push_back(lines[k]);

                Volume_edge.push_back(latura_i);
                Volume_edge.push_back(latura_j);
                Volume_edge.push_back(latura_k);

                Volumes.push_back(Volum);
                Volumes.push_back(Volum);
                Volumes.push_back(Volum);
              }

              }
            }
          }

       }

   if (new_lines.size()>1){
    for(int i = 0; i != new_lines.size(); i++) {
    //std::cout<< new_lines[i][0]<<" "<<new_lines[i][1]<<" "<<new_lines[i][2]<<" "<<new_lines[i][3]<<" "<<new_lines[i][4]<<" "<<new_lines[i][5]<<" "<<'\n';
    }

    for(int i = 0; i != new_lines.size()-1; i++) {

      for(int j = i+1; j != new_lines.size(); j++) {
          if( ( (new_lines[i][0]==new_lines[j][0])  && (new_lines[i][1]==new_lines[j][1]) && (new_lines[i][2]==new_lines[j][2]) && (new_lines[i][3]==new_lines[j][3]) && (new_lines[i][4]==new_lines[j][4]) && (new_lines[i][5]==new_lines[j][5]) ) ||
              ( (new_lines[i][0]==new_lines[j][3])  && (new_lines[i][1]==new_lines[j][4])&&  (new_lines[i][2]==new_lines[j][5]) && (new_lines[i][3]==new_lines[j][0]) && (new_lines[i][4]==new_lines[j][1]) && (new_lines[i][5]==new_lines[j][2]) ) )
               {
            //std::cout<<"Dublura gasita la"<<i<<" "<<j<<'\n';

            double total_distance_i;
            double total_distance_j;

            int offset_i= (i % 3);
            int offset_j= (j % 3);

            total_distance_i=distances[i - offset_i]+distances[i+1 - offset_i]+distances[i+2 - offset_i];
            total_distance_j=distances[j - offset_j]+distances[j+1 - offset_j]+distances[j+2 - offset_j];

            //std::cout<<"Distance i: "<<total_distance_i<<"\n";
           // std::cout<<"Distance j: "<<total_distance_j<<"\n";

            if(total_distance_i>total_distance_j){
              positions_wrong.push_back(i - offset_i);
            }
            else
            {
               positions_wrong.push_back(j - offset_j);
            }

          }

      }
    }
   }

  /*
  std::cout<<"Positions wrong:"<<'\n';

   for(int i = 0; i != positions_wrong.size(); i++) {
    std::cout<<positions_wrong[i]<<'\n';
    }

    */
    if (new_lines.size()>=3){
      nr_volume++;

       //myfile<<str<<" "<<'\n';

       //myfile<<nr_exec<<" iteration"<<'\n';

    for(int i = 0; i < new_lines.size(); i=i+3) {
      bool ok_position=1;
      for(int j = 0; j != positions_wrong.size(); j++) {
        if(i==positions_wrong[j]){
          ok_position=0;
        }
      }
      if(ok_position){
        final_lines.push_back(new_lines[i]);
        final_lines.push_back(new_lines[i+1]);
        final_lines.push_back(new_lines[i+2]);
        std::cout<<"Volum "<<(i/3)<<": "<<Volumes[i]<<" cm^3"<<'\n';

        myfile<<Volumes[i]<<" "<<nr_pointclouds<<'\n';

        /*
        std::cout<< "("<<new_lines[i][0]<<" , "<<new_lines[i][1]<<" , "<<new_lines[i][2]<<" , "<<new_lines[i][3]<<" , "<<new_lines[i][4]<<" , "<<new_lines[i][5]<<")"<<" =>distance1= "<<Volume_edge[i] <<'\n';
        std::cout<< "("<<new_lines[i+1][0]<<" , "<<new_lines[i+1][1]<<" , "<<new_lines[i+1][2]<<" , "<<new_lines[i+1][3]<<" , "<<new_lines[i+1][4]<<" , "<<new_lines[i+1][5]<<")"<<" =>distance2= "<<Volume_edge[i+1] <<'\n';
        std::cout<< "("<<new_lines[i+2][0]<<" , "<<new_lines[i+2][1]<<" , "<<new_lines[i+2][2]<<" , "<<new_lines[i+2][3]<<" , "<<new_lines[i+2][4]<<" , "<<new_lines[i+2][5]<<")"<<" =>distance3= "<<Volume_edge[i+2] <<'\n';
        */

        std::cout<< "distance1= "<<Volume_edge[i] <<" cm"<<'\n';
        std::cout<< "distance2= "<<Volume_edge[i+1]<<" cm" <<'\n';
        std::cout<< "distance3= "<<Volume_edge[i+2]<<" cm" <<'\n';

        std::cout<<'\n';

      }
    }
   }

   //pcshow_lines(points, lines, "Orth Plane Intersections");

   //pcshow_lines(points, new_lines, "Volume Computation Edges");

   //pcshow_lines(points, final_lines, "Volume Final Edges");

  }
     }
  }
   std::cout<<nr_pointclouds<<" pointclouds found"<<'\n';
    std::cout<<nr_volume<<" pointclouds with volumes"<<'\n';

}


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "piecewise_polynomial_discontinuous_scalar_space.hpp"

#include "space_helper.hpp"

#include "../assembly/discrete_sparse_boundary_operator.hpp"
#include "../common/acc.hpp"
#include "../common/boost_make_shared_fwd.hpp"
#include "../common/bounding_box_helpers.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../grid/entity.hpp"
#include "../grid/entity_iterator.hpp"
#include "../grid/geometry.hpp"
#include "../grid/grid.hpp"
#include "../grid/grid_view.hpp"
#include "../grid/grid_segment.hpp"
#include "../grid/mapper.hpp"
#include "../grid/vtk_writer.hpp"

#include <stdexcept>
#include <iostream>

#include <boost/array.hpp>

namespace Bempp {

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialDiscontinuousScalarSpace(
        const shared_ptr<const Grid> &grid, int polynomialOrder)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_flatLocalDofCount(0) {
  initialize(GridSegment::wholeGrid(*grid));
}

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    PiecewisePolynomialDiscontinuousScalarSpace(
        const shared_ptr<const Grid> &grid, int polynomialOrder,
        const GridSegment &segment, int dofMode)
    : ScalarSpace<BasisFunctionType>(grid), m_polynomialOrder(polynomialOrder),
      m_flatLocalDofCount(0) {
  initialize(segment, dofMode);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::initialize(
    const GridSegment &segment, int dofMode) {
  const int gridDim = this->grid()->dim();
  if (gridDim != 2)
    throw std::invalid_argument("PiecewisePolynomialDiscontinuousScalarSpace::"
                                "initialize(): "
                                "only 2-dimensional grids are supported");
  if (!(dofMode & (REFERENCE_POINT_ON_SEGMENT | ELEMENT_ON_SEGMENT)))
    throw std::invalid_argument("PiecewisePolynomialDiscontinuousScalarSpace::"
                                "initialize(): invalid dofMode");
  m_view = this->grid()->leafView();
  if (m_polynomialOrder == 0)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 0>());
  else if (m_polynomialOrder == 1)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 1>());
  else if (m_polynomialOrder == 2)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 2>());
  else if (m_polynomialOrder == 3)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 3>());
  else if (m_polynomialOrder == 4)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 4>());
  else if (m_polynomialOrder == 5)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 5>());
  else if (m_polynomialOrder == 6)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 6>());
  else if (m_polynomialOrder == 7)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 7>());
  else if (m_polynomialOrder == 8)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 8>());
  else if (m_polynomialOrder == 9)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 9>());
  else if (m_polynomialOrder == 10)
    m_triangleShapeset.reset(
        new Fiber::LagrangeScalarBasis<3, BasisFunctionType, 10>());
  else
    throw std::invalid_argument(
        "PiecewisePolynomialDiscontinuousScalarSpace::"
        "PiecewisePolynomialDiscontinuousScalarSpace(): "
        "polynomialOrder must be >= 0 and <= 10");
  assignDofsImpl(segment, dofMode);
}

template <typename BasisFunctionType>
PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::~PiecewisePolynomialDiscontinuousScalarSpace() {}

template <typename BasisFunctionType>
int PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::domainDimension() const {
  return this->grid()->dim();
}

template <typename BasisFunctionType>
int PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::codomainDimension() const {
  return 1;
}

template <typename BasisFunctionType>
const Fiber::Shapeset<BasisFunctionType> &
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::shapeset(
    const Entity<0> &element) const {
  if (elementVariant(element) == 3)
    return *m_triangleShapeset;
  throw std::logic_error(
      "PiecewisePolynomialDiscontinuousScalarSpace::shapeset(): "
      "invalid element variant, this shouldn't happen!");
}

template <typename BasisFunctionType>
bool PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::spaceIsCompatible(const Space<BasisFunctionType> &other)
    const {

  typedef PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>
  thisSpaceType;

  if (other.grid().get() != this->grid().get())
    return false;

  if (other.spaceIdentifier() == this->spaceIdentifier()) {
    // Try to typecast the other space down.
    const thisSpaceType &temp = dynamic_cast<const thisSpaceType &>(other);
    if (this->m_polynomialOrder == temp.m_polynomialOrder)
      return true;
    else
      return false;
  } else
    return false;
}

template <typename BasisFunctionType>
ElementVariant
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::elementVariant(
    const Entity<0> &element) const {
  GeometryType type = element.type();
  if (type.isLine())
    return 2;
  else if (type.isTriangle())
    return 3;
  else if (type.isQuadrilateral())
    return 4;
  else
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "elementVariant(): invalid geometry type, "
                             "this shouldn't happen!");
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::setElementVariant(const Entity<0> &element,
                                          ElementVariant variant) {
  if (variant != elementVariant(element))
    // for this space, the element variants are unmodifiable,
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "setElementVariant(): invalid variant");
}

template <typename BasisFunctionType>
shared_ptr<const Space<BasisFunctionType>>
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    discontinuousSpace(const shared_ptr<const Space<BasisFunctionType>> &self)
    const {
  if (self.get() != this)
    throw std::invalid_argument(
        "PiecewisePolynomialDiscontinuousScalarSpace::discontinuousSpace(): "
        "argument should be a shared pointer to *this");
  return self;
}

template <typename BasisFunctionType>
bool PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::isDiscontinuous() const {
  return true;
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::assignDofsImpl(
    const GridSegment &segment, int dofMode) {
  const int gridDim = this->domainDimension();
  if (gridDim != 2)
    throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                             "assignDofsImpl(): only 2-dimensional grids "
                             "are supported at present");
  const int vertexCodim = 2, edgeCodim = 1, elementCodim = 0;
  const Mapper &elementMapper = m_view->elementMapper();
  const IndexSet &indexSet = m_view->indexSet();

  int elementCount = m_view->entityCount(0);

  const int localDofCountPerTriangle =
      (m_polynomialOrder + 1) * (m_polynomialOrder + 2) / 2;
  const int localDofCountPerQuad =
      (m_polynomialOrder + 1) * (m_polynomialOrder + 1);

  BoundingBox<CoordinateType> model;
  model.lbound.x = std::numeric_limits<CoordinateType>::max();
  model.lbound.y = std::numeric_limits<CoordinateType>::max();
  model.lbound.z = std::numeric_limits<CoordinateType>::max();
  model.ubound.x = -std::numeric_limits<CoordinateType>::max();
  model.ubound.y = -std::numeric_limits<CoordinateType>::max();
  model.ubound.z = -std::numeric_limits<CoordinateType>::max();
  m_globalDofBoundingBoxes.reserve(localDofCountPerQuad * elementCount);

  // (Re)initialise DOF maps
  m_local2globalDofs.clear();
  m_local2globalDofs.resize(elementCount);
  m_global2localDofs.clear();
  // estimated number of global DOFs
  m_global2localDofs.reserve(localDofCountPerQuad * elementCount);

  // Fill in global<->local dof maps
  std::unique_ptr<EntityIterator<0>> it = m_view->entityIterator<0>();
  arma::Mat<CoordinateType> vertices;
  arma::Col<CoordinateType> dofPosition;
  GlobalDofIndex globalDofCount = 0;
  while (!it->finished()) {
    const Entity<0> &element = it->entity();
    EntityIndex elementIndex = elementMapper.entityIndex(element);
    bool elementContained = !(dofMode & ELEMENT_ON_SEGMENT) ||
                            segment.contains(elementCodim, elementIndex);
    typedef arma::Col<CoordinateType> Col;
    const Geometry &geo = element.geometry();
    geo.getCorners(vertices);
    int vertexCount = vertices.n_cols;
    int localDofCount =
        vertexCount == 3 ? localDofCountPerTriangle : localDofCountPerQuad;

    // List of global DOF indices corresponding to the local DOFs of the
    // current element
    std::vector<GlobalDofIndex> &globalDofs = m_local2globalDofs[elementIndex];
    globalDofs.resize(localDofCount);
    // GlobalDofIndex gdofStart = globalDofCount;
    // for (int i = 0; i < localDofCount; ++i) {
    //     globalDofs.push_back(globalDofCount);
    //     std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, i));
    //     m_global2localDofs.push_back(localDofs);
    //     ++globalDofCount;
    // }
    // GlobalDofIndex gdofEnd = globalDofCount;

    // Bounding boxes
    BoundingBox<CoordinateType> bbox = model;
    extendBoundingBox(bbox, vertices);
    // m_globalDofBoundingBoxes.insert(m_globalDofBoundingBoxes.end(),
    //                                 localDofCount, bbox);
    if (vertexCount == 3) {
      int subEntityIndex;
      int ldof;

      if (m_polynomialOrder == 0) {
        ldof = 0;
        if (segment.contains(0, elementIndex)) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount),
              (vertices.col(0) + vertices(1) + vertices(2)) / 3);
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;
      } else {
        // vertex dofs
        ldof = 0;
        subEntityIndex = indexSet.subEntityIndex(element, 0, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(0));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        ldof = m_polynomialOrder;
        subEntityIndex = indexSet.subEntityIndex(element, 1, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(1));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        ldof = localDofCount - 1;
        subEntityIndex = indexSet.subEntityIndex(element, 2, vertexCodim);
        if (elementContained &&
            (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
             segment.contains(vertexCodim, subEntityIndex))) {
          acc(globalDofs, ldof) = globalDofCount;
          std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
          m_global2localDofs.push_back(localDofs);
          m_globalDofBoundingBoxes.push_back(bbox);
          setBoundingBoxReference<CoordinateType>(
              acc(m_globalDofBoundingBoxes, globalDofCount), vertices.col(2));
          ++globalDofCount;
        } else
          acc(globalDofs, ldof) = -1;

        // edge dofs
        if (m_polynomialOrder >= 2) {

          subEntityIndex = indexSet.subEntityIndex(element, 0, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(0) + vertices.col(1));
            for (int ldof = 1; ldof < m_polynomialOrder; ++ldof) {
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldof = 1; ldof < m_polynomialOrder; ++ldof) {
              acc(globalDofs, ldof) = -1;
            }

          subEntityIndex = indexSet.subEntityIndex(element, 1, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(0) + vertices.col(2));
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof =
                  ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof =
                  ldofy * (m_polynomialOrder + 1) - ldofy * (ldofy - 1) / 2;
              acc(globalDofs, ldof) = -1;
            }

          subEntityIndex = indexSet.subEntityIndex(element, 2, edgeCodim);
          if (elementContained &&
              (!(dofMode & REFERENCE_POINT_ON_SEGMENT) ||
               segment.contains(edgeCodim, subEntityIndex))) {
            dofPosition = 0.5 * (vertices.col(1) + vertices.col(2));
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof = ldofy * (m_polynomialOrder + 1) -
                         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
              acc(globalDofs, ldof) = globalDofCount;
              std::vector<LocalDof> localDofs(1, LocalDof(elementIndex, ldof));
              m_global2localDofs.push_back(localDofs);
              m_globalDofBoundingBoxes.push_back(bbox);
              setBoundingBoxReference<CoordinateType>(
                  acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
              ++globalDofCount;
            }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
              int ldof = ldofy * (m_polynomialOrder + 1) -
                         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
              acc(globalDofs, ldof) = -1;
            }

          // dofPosition = 0.5 * (vertices.col(0) + vertices.col(1));
          // for (int ldof = 1; ldof < m_polynomialOrder; ++ldof){
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
          // dofPosition = 0.5 * (vertices.col(0) + vertices.col(2));
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
          //     int ldof = ldofy * (m_polynomialOrder + 1) -
          //         ldofy * (ldofy - 1) / 2;
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
          // dofPosition = 0.5 * (vertices.col(1) + vertices.col(2));
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy) {
          //     int ldof = ldofy * (m_polynomialOrder + 1) -
          //         ldofy * (ldofy - 1) / 2 + (m_polynomialOrder - ldofy);
          //     setBoundingBoxReference<CoordinateType>(
          //         acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          // dofPosition);
          // }
        }
        // bubble dofs
        if (m_polynomialOrder >= 3) {
          if (segment.contains(elementCodim, elementIndex)) {
            dofPosition =
                (vertices.col(0) + vertices.col(1) + vertices.col(2)) / 3.;
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
              for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx) {
                int ldof = ldofy * (m_polynomialOrder + 1) -
                           ldofy * (ldofy - 1) / 2 + ldofx;
                acc(globalDofs, ldof) = globalDofCount;
                std::vector<LocalDof> localDofs(1,
                                                LocalDof(elementIndex, ldof));
                m_global2localDofs.push_back(localDofs);
                m_globalDofBoundingBoxes.push_back(bbox);
                setBoundingBoxReference<CoordinateType>(
                    acc(m_globalDofBoundingBoxes, globalDofCount), dofPosition);
                ++globalDofCount;
              }
          } else
            for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
              for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx) {
                int ldof = ldofy * (m_polynomialOrder + 1) -
                           ldofy * (ldofy - 1) / 2 + ldofx;
                acc(globalDofs, ldof) = -1;
              }

          // dofPosition = (vertices.col(0) + vertices.col(1) +
          //                vertices.col(2)) / 3.;
          // for (int ldofy = 1; ldofy < m_polynomialOrder; ++ldofy)
          //     for (int ldofx = 1; ldofx + ldofy < m_polynomialOrder; ++ldofx)
          // {
          //         int ldof = ldofy * (m_polynomialOrder + 1) -
          //             ldofy * (ldofy - 1) / 2 + ldofx;
          //         setBoundingBoxReference<CoordinateType>(
          //             acc(m_globalDofBoundingBoxes, gdofStart + ldof),
          //             dofPosition);
          //     }
        }
      }
    }

    it->next();
  }

  // Initialize the container mapping the flat local dof indices to
  // local dof indices
  SpaceHelper<BasisFunctionType>::initializeLocal2FlatLocalDofMap(
      m_flatLocalDofCount, m_local2globalDofs, m_flatLocal2localDofs);

#ifndef NDEBUG
  for (size_t i = 0; i < m_globalDofBoundingBoxes.size(); ++i) {
    const BoundingBox<CoordinateType> &bbox = acc(m_globalDofBoundingBoxes, i);

    assert(bbox.reference.x >= bbox.lbound.x);
    assert(bbox.reference.y >= bbox.lbound.y);
    assert(bbox.reference.z >= bbox.lbound.z);
    assert(bbox.reference.x <= bbox.ubound.x);
    assert(bbox.reference.y <= bbox.ubound.y);
    assert(bbox.reference.z <= bbox.ubound.z);
  }
#endif // NDEBUG
}

template <typename BasisFunctionType>
size_t
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::globalDofCount()
    const {
  return m_global2localDofs.size();
}

template <typename BasisFunctionType>
size_t PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::flatLocalDofCount() const {
  return m_flatLocalDofCount;
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::getGlobalDofs(
    const Entity<0> &element, std::vector<GlobalDofIndex> &dofs) const {
  const Mapper &mapper = m_view->elementMapper();
  EntityIndex index = mapper.entityIndex(element);
  dofs = m_local2globalDofs[index];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    global2localDofs(const std::vector<GlobalDofIndex> &globalDofs,
                     std::vector<std::vector<LocalDof>> &localDofs) const {
  localDofs.resize(globalDofs.size());
  for (size_t i = 0; i < globalDofs.size(); ++i)
    localDofs[i] = m_global2localDofs[globalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    flatLocal2localDofs(const std::vector<FlatLocalDofIndex> &flatLocalDofs,
                        std::vector<LocalDof> &localDofs) const {
  localDofs.resize(flatLocalDofs.size());
  for (size_t i = 0; i < flatLocalDofs.size(); ++i)
    localDofs[i] = m_flatLocal2localDofs[flatLocalDofs[i]];
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  positions.resize(m_globalDofBoundingBoxes.size());
  for (size_t i = 0; i < m_globalDofBoundingBoxes.size(); ++i)
    acc(positions, i) = acc(m_globalDofBoundingBoxes, i).reference;
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofPositions(std::vector<Point3D<CoordinateType>> &positions)
    const {
  getGlobalDofPositions(positions);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofBoundingBoxes(std::vector<BoundingBox<CoordinateType>> &bboxes)
    const {
  bboxes = m_globalDofBoundingBoxes;
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofBoundingBoxes(
        std::vector<BoundingBox<CoordinateType>> &bboxes) const {
  getGlobalDofBoundingBoxes(bboxes);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getGlobalDofNormals(std::vector<Point3D<CoordinateType>> &normals) const {
  SpaceHelper<BasisFunctionType>::getGlobalDofNormals_defaultImplementation(
      *m_view, m_global2localDofs, normals);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::
    getFlatLocalDofNormals(std::vector<Point3D<CoordinateType>> &normals)
    const {
  getGlobalDofNormals(normals);
}

template <typename BasisFunctionType>
void
PiecewisePolynomialDiscontinuousScalarSpace<BasisFunctionType>::dumpClusterIds(
    const char *fileName,
    const std::vector<unsigned int> &clusterIdsOfDofs) const {
  dumpClusterIdsEx(fileName, clusterIdsOfDofs, GLOBAL_DOFS);
}

template <typename BasisFunctionType>
void PiecewisePolynomialDiscontinuousScalarSpace<
    BasisFunctionType>::dumpClusterIdsEx(const char *fileName,
                                         const std::vector<unsigned int> &
                                             clusterIdsOfDofs,
                                         DofType dofType) const {
  throw std::runtime_error("PiecewisePolynomialDiscontinuousScalarSpace::"
                           "dumpClusterIdsEx(): not implemented yet");
}

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS(
    PiecewisePolynomialDiscontinuousScalarSpace);

} // namespace Bempp


/*******************************************************************************
*
*  Filename    : Implementation of filling and plotting functions
*  Description : One Line descrption of file contents
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/SignalMCStudy/interface/Common.hpp"
#include "TstarAnalysis/SignalMCStudy/interface/MCResMgr.hpp"

#include "TFile.h"
#include "TTree.h"
#include <boost/format.hpp>
#include <iostream>

using namespace std;
using namespace mgr;

MCResMgr::MCResMgr( const string& name ) :
  Named( name ),
  HistMgr( name )
{
  AddHist( "HadWMass",      "Hadronic W mass (truth reconstruction)",   "GeV/c^{2}",  80,    0,  240 );
  AddHist( "HadTopMass",    "Hadronic top mass (truth reconstuction)",  "GeV/c^{2}",  70,    0,  350 );
  AddHist( "HadTstarMass",  "Hadronic t* mass (truth reconstuction)",   "GeV/c^{2}", 120,    0, 3000 );
  AddHist( "LepTopMass",    "Leptonic top mass (truth reconstuction)",  "GeV/c^{2}",  70,    0,  350 );
  AddHist( "LepTstarMass",  "Leptonic t* mass (truth reconstuction)",   "GeV/c^{2}", 120,    0, 3000 );
  AddHist( "TstarMassDiff", "t* mass difference (truth reconstuction)", "GeV/c^{2}",  60, -600,  600 );
  AddHist( "TopMassDiff",   "Top mass (truth reconstuction)",           "GeV/c^{2}",  60,  -30,   30 );

  FillFromFile();
}

void
MCResMgr::FillFromFile()
{
  const string filename = ResultsDir() / ( "mcres_M"+Name()+".root" );
  TFile* file           = TFile::Open( filename.c_str() );
  TTree* tree           = (TTree*)( file->Get( "mcres/MCResolution" ) );

  double lepwmass;
  double leptopmass;
  double leptstarmass;
  double hadwmass;
  double hadtopmass;
  double hadtstarmass;

  tree->SetBranchAddress( "LepWMass",     &lepwmass );
  tree->SetBranchAddress( "LepTopMass",   &leptopmass );
  tree->SetBranchAddress( "LepTstarMass", &leptstarmass );
  tree->SetBranchAddress( "HadWMass",     &hadwmass );
  tree->SetBranchAddress( "HadTopMass",   &hadtopmass );
  tree->SetBranchAddress( "HadTstarMass", &hadtstarmass );

  boost::format entryfmt( "\r%s Entry[%d/%d]" );

  for( int i = 0; i < tree->GetEntries(); ++i ){
    tree->GetEntry( i );

    cout << entryfmt
      % Name()
      % i % tree->GetEntries()
         << flush;

    Hist( "LepTopMass" )->Fill( leptopmass );
    Hist( "LepTstarMass" )->Fill( leptstarmass );
    Hist( "HadWMass" )->Fill( hadwmass );
    Hist( "HadTopMass" )->Fill( hadtopmass );
    Hist( "HadTstarMass" )->Fill( hadtstarmass );
    Hist( "TstarMassDiff" )->Fill( hadtstarmass - leptstarmass );
    Hist( "TopMassDiff" )->Fill( hadtopmass - leptopmass );
  }

  cout << "Done!" << endl;
  cout << " lept:" << Hist( "LepTopMass" )->GetRMS() << "\t"
       << " lepts:" << Hist( "LepTstarMass" )->GetRMS() <<  "\t"
       << " hadw:" << Hist( "HadWMass" )->GetRMS() << "\t"
       << " hadt:" << Hist( "HadTopMass" )->GetRMS() << "\t"
       << " hadts:" << Hist( "HadTstarMass" )->GetRMS() << "\t"
       << " dts:" << Hist( "TstarMassDiff" )->GetRMS() << endl;
}