// Boost.Geometry
// This file is manually converted from PROJ4

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// This file was converted to Geometry Library by Adam Wulkiewicz

// Original copyright notice:
// Author:   Frank Warmerdam, warmerdam@pobox.com

// Copyright (c) 2000, Frank Warmerdam

// 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.

#ifndef BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_SHARED_HPP
#define BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_SHARED_HPP

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/srs/projections/impl/pj_apply_gridshift.hpp>
#include <boost/geometry/srs/projections/impl/pj_gridlist_shared.hpp>

namespace boost { namespace geometry { namespace projections
{

namespace detail
{

template <bool Inverse, typename CalcT, typename StreamPolicy, typename Range>
inline bool pj_apply_gridshift_3(StreamPolicy const& stream_policy,
                                 Range & range,
                                 srs::shared_grids & grids,
                                 std::vector<std::size_t> const& gridindexes)
{
    typedef typename boost::range_size<Range>::type size_type;

    // If the grids are empty the indexes are as well
    if (gridindexes.empty())
    {
        //pj_ctx_set_errno(ctx, PJD_ERR_FAILED_TO_LOAD_GRID);
        //return PJD_ERR_FAILED_TO_LOAD_GRID;
        return false;
    }

    size_type point_count = boost::size(range);

    // local storage
    pj_gi_load local_gi;

    for (size_type i = 0 ; i < point_count ; )
    {
        bool load_needed = false;

        CalcT in_lon = 0;
        CalcT in_lat = 0;

        {
            boost::shared_lock<boost::shared_mutex> lock(grids.mutex);

            for ( ; i < point_count ; ++i )
            {
                typename boost::range_reference<Range>::type
                    point = range::at(range, i);

                in_lon = geometry::get_as_radian<0>(point);
                in_lat = geometry::get_as_radian<1>(point);

                pj_gi * gip = find_grid(in_lon, in_lat, grids.gridinfo, gridindexes);

                if (gip == NULL)
                {
                    // do nothing
                }
                else if (! gip->ct.cvs.empty())
                {
                    // TODO: use set_invalid_point() or similar mechanism
                    CalcT out_lon = HUGE_VAL;
                    CalcT out_lat = HUGE_VAL;

                    nad_cvt<Inverse>(in_lon, in_lat, out_lon, out_lat, *gip);

                    // TODO: check differently
                    if (out_lon != HUGE_VAL)
                    {
                        geometry::set_from_radian<0>(point, out_lon);
                        geometry::set_from_radian<1>(point, out_lat);
                    }
                }
                else
                {
                    // loading is needed
                    local_gi = *gip;
                    load_needed = true;
                    break;
                }
            }
        }

        if (load_needed)
        {
            if (load_grid(stream_policy, local_gi))
            {
                boost::unique_lock<boost::shared_mutex> lock(grids.mutex);

                // check again in case other thread already loaded the grid.
                pj_gi * gip = find_grid(in_lon, in_lat, grids.gridinfo, gridindexes);

                if (gip != NULL && gip->ct.cvs.empty())
                {
                    // swap loaded local storage with empty grid
                    local_gi.swap(*gip);
                }
            }
            else
            {
                ++i;
            }
        }
    }

    return true;
}

} // namespace detail

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_SRS_PROJECTIONS_IMPL_PJ_APPLY_GRIDSHIFT_SHARED_HPP


#include <SDL.h>
#include <boost/test/unit_test.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <util/get_sources_path.h>
#include "test_fixture.h"
#include "font.h"
#include "material.h"
#include "vertex_array.h"
#include "index_array.h"
#include "uber_draw_call.h"
#include "d3d9_renderer.h"

BOOST_AUTO_TEST_SUITE(font)

BOOST_FIXTURE_TEST_CASE(make_texture, graphic::TestFixture)
{
	graphic::RendererPtr renderer(new graphic::D3D9Renderer());
	renderer->postconstruct();

	graphic::FontPtr font(new graphic::Font(renderer, "arial.ttf", 72));

	graphic::TexturePtr texture = font->make_texture("bonjour", math::vec<4>(1, 1, 1, 0), math::vec<4>(0, 0.2f, 0.2f, 0));

	graphic::Texture::save_to_file(texture, "rendered_text_texture.bmp");
}

BOOST_FIXTURE_TEST_CASE(make_draw_call, graphic::TestFixture)
{
	graphic::RendererPtr renderer(new graphic::D3D9Renderer());
	renderer->postconstruct();

	graphic::FontPtr font(new graphic::Font(renderer, "arial.ttf", 72));

	boost::shared_ptr<graphic::Camera> camera(new graphic::Camera());
	camera->set_orthographic_projection(renderer->get_screen_width(), renderer->get_screen_height(), -1, 1000);

	math::matrix<4,4> view_matrix;
	view_matrix.translation(-renderer->get_screen_width() / 2, -renderer->get_screen_height() / 2, 0);
	view_matrix.scale(1, -1, 1);
	camera->set_view_matrix(view_matrix);

	graphic::MaterialPtr hacked_material = renderer->create_material();
	hacked_material->set_diffuse_texture(graphic::Texture::load_from_file(renderer,
		util::get_graphic_sources_path() + "/test_directions.png"));

	graphic::DrawCallPtr draw_call = font->make_draw_call(math::vec<3>(0, 0, 0), math::vec<3>(100, 0, 0),
		math::vec<3>(100, 100, 0), math::vec<3>(0, 100, 0), "bonjour", math::vec<4>(1, 1, 1, 0), math::vec<4>(0, 0.2f, 0.2f, 0));
	draw_call->set_camera(camera);
	draw_call->set_always_visible(true);
	draw_call->commit();

	graphic::DrawCallPtr hacked_draw_call = font->make_draw_call(math::vec<3>(0, 100, 0), math::vec<3>(100, 100, 0),
		math::vec<3>(100, 200, 0), math::vec<3>(0, 200, 0), "bonjour", math::vec<4>(1, 1, 1, 0), math::vec<4>(0, 0.2f, 0.2f, 0));
	hacked_draw_call->set_material(hacked_material);
	hacked_draw_call->set_camera(camera);
	hacked_draw_call->commit();

	boost::posix_time::ptime start_time(boost::posix_time::microsec_clock::local_time());
	math::scalar delta;
	math::scalar const DURATION = 5e6;

	while ((delta = math::scalar((boost::posix_time::microsec_clock::local_time() - start_time).total_microseconds())) < DURATION)
	{
		graphic::TestFixture::process_events();

		renderer->begin_frame();

		draw_call->execute();
		hacked_draw_call->execute();

		renderer->finish_frame();
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>

#include "variant/variant_reader.h"
#include "variant/variant.h"
#include "variant/genotype.h"

#include <boost/dynamic_bitset.hpp>
#include <algorithm>
#include <stdexcept>

using namespace std;
using namespace gamgee;
using namespace boost;

void select_if_test(const vector<dynamic_bitset<>>& truth, const std::string& filename, const std::function<bool (const gamgee::Genotype& )> pred){
  auto record_idx = 0u;
  for (const auto& record : SingleVariantReader{filename}) {
    const auto genotype = record.genotypes();
    const auto result = Variant::select_if(genotype.begin(), genotype.end(), pred );

    BOOST_CHECK(result == truth[record_idx]);
    ++record_idx;
  }
}

const auto diploid= "testdata/test_variants.vcf";
const auto multi_ploidy = "testdata/test_variants_alternate_ploidy.vcf";

bool hom_ref(const gamgee::Genotype& g) { return g.hom_ref(); };
bool hom_var(const gamgee::Genotype& g) { return g.hom_var(); };

BOOST_AUTO_TEST_CASE( hom_ref_test ){
  const auto truth = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010"))};

  select_if_test(truth, diploid, hom_ref  );
}

BOOST_AUTO_TEST_CASE( hom_ref_alternate_ploidy_test ){
  const auto truth = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("010"))};

  select_if_test(truth, multi_ploidy , hom_ref );
}

BOOST_AUTO_TEST_CASE( hom_var_test ){
  const auto truth = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100"))};

  select_if_test(truth, diploid, hom_var);
}

BOOST_AUTO_TEST_CASE( hom_var_alternate_ploidy_test ){
  const auto truth = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("101")),
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("100"))};

  select_if_test(truth, multi_ploidy, hom_var);
}

BOOST_AUTO_TEST_CASE( het_alternate_ploidy_test ){
  const auto truth = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("000")),
    dynamic_bitset<>(string("001")),
    dynamic_bitset<>(string("000"))};

  select_if_test(truth, multi_ploidy, [](const auto& g) {return g.het(); });
}

BOOST_AUTO_TEST_CASE( het_snp_is_true ) {
  const auto rec = *(SingleVariantReader{"testdata/test_variants.bcf"}.begin());
  const auto mask = rec.allele_mask();
  BOOST_CHECK(rec.genotypes()[0].snp(mask));
  BOOST_CHECK(!rec.genotypes()[0].insertion(mask));
  BOOST_CHECK(!rec.genotypes()[0].deletion(mask));
}

BOOST_AUTO_TEST_CASE( het_insertion_is_true ) {
  auto it = SingleVariantReader{"testdata/test_variants.bcf"}.begin();
  ++it; ++it; ++it;
  const auto rec = *it;
  const auto mask = rec.allele_mask();
  BOOST_CHECK(rec.genotypes()[0].insertion(mask));
  BOOST_CHECK(!rec.genotypes()[0].snp(mask));
  BOOST_CHECK(!rec.genotypes()[0].deletion(mask));
}

BOOST_AUTO_TEST_CASE( het_deletion_is_true ) {
  auto it = SingleVariantReader{"testdata/test_variants.bcf"}.begin();
  ++it; ++it;
  const auto rec = *it;
  const auto mask = rec.allele_mask();
  BOOST_CHECK(rec.genotypes()[0].deletion(mask));
  BOOST_CHECK(!rec.genotypes()[0].insertion(mask));
  BOOST_CHECK(!rec.genotypes()[0].snp(mask));
}

BOOST_AUTO_TEST_CASE( het_indel_is_true ) {
  const auto test_rec = [](const Variant& r, const bool t){
    auto mask = r.allele_mask();
    BOOST_CHECK_EQUAL(r.genotypes()[0].indel(mask), t);
  };
  const auto truth = vector<bool>{false, false, true, true, true, true, true};
  auto i = 0;
  for (const auto& rec : SingleVariantReader{"testdata/test_variants.bcf"}) {
    test_rec(rec, truth[i]);
    ++i;
  }
}

BOOST_AUTO_TEST_CASE( complex_test ) {
  auto test_rec = [](const Variant& r, const bool t){
    auto mask = r.allele_mask();
    BOOST_CHECK_EQUAL(r.genotypes()[0].complex(), t);
  };
  const auto truth = vector<bool> {false, false, false, false, true, false, false};
  auto i = 0;
  for (const auto& rec : SingleVariantReader{"testdata/test_variants.bcf"}) {
    test_rec(rec, truth[i]);
    ++i;
  }
}

BOOST_AUTO_TEST_CASE( mixed_test ) {
  auto test_rec = [](const Variant& r, const bool t){
    auto mask = r.allele_mask();
    BOOST_CHECK_EQUAL(r.genotypes()[0].mixed(), t);
  };
  const auto truth = vector<bool> {false, false, false, false, true, false, false};
  auto i = 0;
  for (const auto& rec : SingleVariantReader{"testdata/test_variants.bcf"}) {
    test_rec(rec, truth[i]);
    ++i;
  }
}

BOOST_AUTO_TEST_CASE( is_variant ) {
  const auto truth = vector<bool>{true, false, true};
  const auto rec = *(SingleVariantReader{"testdata/test_variants.bcf"}.begin());
  const auto genotypes = rec.genotypes();
  for (auto i=0u; i != genotypes.size(); ++i)
    BOOST_CHECK_EQUAL(genotypes[i].variant(), truth[i]);
}

BOOST_AUTO_TEST_CASE( mixed_ploidy_random_access ) {
  const auto rec = (*(SingleVariantReader("testdata/test_variants_mixed_ploidy.vcf").begin()));
  const auto expected = vector<vector<int32_t>>{ {0, 1}, {0, bcf_int32_vector_end}, {1, bcf_int32_vector_end} };

  auto sample_idx = 0u;
  for ( const auto& genotype : rec.genotypes() ) {
    for ( auto allele_idx = 0u; allele_idx < genotype.size(); ++allele_idx ) {
      BOOST_CHECK_EQUAL(genotype[allele_idx], expected[sample_idx][allele_idx]);
    }
    ++sample_idx;
  }
}

BOOST_AUTO_TEST_CASE( encode_genotype ) {
  auto alleles = vector<int32_t>{};
  auto expected = vector<int32_t>{};

  alleles = {-1};
  expected = {0};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {0};
  expected = {2};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {1};
  expected = {4};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {2};
  expected = {6};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  // vector end values should not get transformed by encoding
  alleles = {bcf_int32_vector_end};
  expected = {bcf_int32_vector_end};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {0, 1};
  expected = {2, 4};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {-1, 0};
  expected = {0, 2};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {1, 2, 0};
  expected = {4, 6, 2};
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  // If there are multiple ploidies and/or missing genotypes we will have vector end values mixed in with the alleles
  alleles = { 1, bcf_int32_vector_end, -1, bcf_int32_vector_end, 0, 1 };
  expected = { 4, bcf_int32_vector_end, 0, bcf_int32_vector_end, 2, 4 };
  Genotype::encode_genotype(alleles);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  // First allele should not be phased when phasing is requested
  alleles = {-1};
  expected = {0};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {0};
  expected = {2};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {1};
  expected = {4};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  // Second and subsequent alleles should be phased with respect to the previous
  alleles = {0, 1};
  expected = {2, 5};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {-1, 0};
  expected = {0, 3};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  alleles = {1, 2, 0};
  expected = {4, 7, 3};
  Genotype::encode_genotype(alleles, true);
  BOOST_CHECK(equal(alleles.begin(), alleles.end(), expected.begin()));

  // Should detect invalid values: only allele indices >= 0, -1 for missing values, and vector end should be allowed
  alleles = {-2};
  BOOST_CHECK_THROW(Genotype::encode_genotype(alleles), std::invalid_argument);

  alleles = {bcf_int32_missing};
  BOOST_CHECK_THROW(Genotype::encode_genotype(alleles), std::invalid_argument);

  alleles = {0, bcf_int32_vector_end + 1};
  BOOST_CHECK_THROW(Genotype::encode_genotype(alleles), std::invalid_argument);
}


#include <chrono>
#include <iostream>
#include <memory>
#include <cmath>

#include <boost/asio.hpp>
#include <thread>

#include <spdlog/spdlog.h>

#include "pb/packet.pb.h"
#include "error.h"
#include "util.h"
#include "session.h"
#include "connection.h"

using namespace std;

namespace NKN {
    namespace NCP {
        Connection::Connection(const shared_ptr<Session_t>& sess, string localCliID, string remoteCliID)
                : session(sess), localClientID(std::move(localCliID)), remoteClientID(std::move(remoteCliID)),
                  windowSize(sess->config->InitialConnectionWindowSize), sendWindowUpdate(1), timeSentSeq(),
                  resentSeq(), sendAckQueue(),
                  retransmissionTimeout(chrono::milliseconds(sess->config->InitialRetransmissionTimeout)) {}

        void Connection::ReceiveAck(uint32_t seq, bool isSentByMe) {
            // TODO Lock
            if (timeSentSeq->count(seq) == 0) {
                return;
            }

            // increase windowSize if success without resent
            if (resentSeq->count(seq) == 0) {
                windowSize = min(++windowSize, session->config->MaxConnectionWindowSize);
            }

            if (isSentByMe) {
                auto mil_sec = chrono::milliseconds(1).count();
                auto rtt = chrono::duration_cast<chrono::milliseconds>(
                        chrono::steady_clock::now() - (*timeSentSeq)[seq]);
                auto delta = tanh(double(3 * rtt.count() - retransmissionTimeout.count()) / mil_sec / 1000);

                retransmissionTimeout += chrono::milliseconds(long(100 * delta));
                if (retransmissionTimeout.count() > session->config->MaxRetransmissionTimeout)
                    retransmissionTimeout = chrono::milliseconds(session->config->MaxRetransmissionTimeout);
            }

            timeSentSeq->erase(seq);
            resentSeq->erase(seq);

            sendWindowUpdate.push(make_unique<bool>(true), true);
        }

        boost::system::error_code Connection::tx() {
            boost::system::error_code ec;
            uint32_t seq = 0;

            while (true) {
                if (seq == 0) {
                    seq = session->getResendSeq();
                    // spdlog::info("Connection[{}-{}]::tx():{} getResendSeq == {}", localClientID, remoteClientID, __LINE__, seq);
                }

                if (seq == 0) {
                    ec = waitForSendWindow(chrono::milliseconds(1000));
                    // spdlog::info("Connection[{}-{}]::tx():{} waitForSendWindow got err: {}:{}",
                            // localClientID, remoteClientID, __LINE__, ec.message(), ec.value());
                    if (ec == ErrCode::ErrMaxWait)
                        continue;
                    if (ec) {
                        return ec;
                    }
                    seq = session->getSendSeq();
                    spdlog::info("Connection[{}-{}]::tx():{} getSendSeq == {}", localClientID, remoteClientID, __LINE__, seq);
                }

                auto data = session->GetDataToSend(seq);
                if (data->size() == 0) {
                    // TODO Lock
                    timeSentSeq->erase(seq);
                    resentSeq->erase(seq);
                    spdlog::info("Connection[{}-{}]::tx():{} GetDataToSend got 0 data with seq:{}", localClientID, remoteClientID, __LINE__, seq);
                    continue;
                }

                ec = session->sendWith(localClientID, remoteClientID, data, retransmissionTimeout);
                if (ec) {
                    spdlog::info("Connection[{}-{}]::tx():{} sendWith got err {}:{}",
                            localClientID, remoteClientID, __LINE__, ec.message(), ec.value());
                    if (session->IsClosed())
                        return ErrCode::ErrSessionClosed;
                    if (ec == ErrCode::ErrConnClosed)
                        return ec;

                    // TODO log ec
                    session->resendChan->push(make_unique<uint32_t>(seq));
                    this_thread::sleep_for(chrono::seconds(1));
                    continue;
                }

                // sendWith success
                // TODO Lock
                if (timeSentSeq->count(seq) == 0)
                    (*timeSentSeq)[seq] = chrono::steady_clock::now();
                resentSeq->erase(seq);
                // TODO UnLock
                seq = 0;

                // spdlog::info("Connection[{}-{}]::tx():{} reached loop ending", localClientID, remoteClientID, __LINE__);
            }
        }

        boost::system::error_code Connection::sendAck() {
            boost::system::error_code ec;
            boost::asio::io_context io;

            boost::asio::deadline_timer t(io);
            auto interval = boost::posix_time::milliseconds(session->config->SendAckInterval);
            while (!session->IsClosed()) {
                t.expires_from_now(interval, ec);
                t.wait(ec);

                // spdlog::info("sendAck({}-{}):{} SendAckQueueLen:{}",
                        // localClientID, remoteClientID, __LINE__, SendAckQueueLen());
                if (SendAckQueueLen() == 0) {
                    interval = boost::posix_time::milliseconds(1000);
                    continue;
                }
                interval = boost::posix_time::milliseconds(session->config->SendAckInterval);

                vector<uint32_t> ackStartSeqList;
                vector<uint32_t> ackSeqCountList;

                // TODO Lock
                while (sendAckQueue->size() > 0 && (ackStartSeqList.size() < session->config->MaxAckSeqListSize)) {
                    uint32_t ackSeqCount, ackStartSeq;
                    ackStartSeq = sendAckQueue->top();
                    sendAckQueue->pop();
                    ackSeqCount = 1;
                    while (sendAckQueue->size() > 0 && sendAckQueue->top() == NextSeq(ackStartSeq, ackSeqCount)) {
                        sendAckQueue->pop();
                        ackSeqCount++;
                    }

                    ackStartSeqList.push_back(ackStartSeq);
                    ackSeqCountList.push_back(ackSeqCount);

                    spdlog::info("sendAck({}-{}):{} ackStartSeq:{} ackSeqCount:{}",
                            localClientID, remoteClientID, __LINE__, ackStartSeq, ackSeqCount);
                }
                // TODO UnLock

                if (all_of(ackSeqCountList.cbegin(), ackSeqCountList.cend(), [](uint32_t n) { return n == 1; })) {
                    ackSeqCountList.clear();
                }

                auto pktPtr = make_shared<pb::Packet>();
                for (auto it = ackStartSeqList.cbegin(); it < ackStartSeqList.cend(); it++) {
                    pktPtr->add_ack_start_seq(*it);
                }
                for (auto it = ackSeqCountList.cbegin(); it < ackSeqCountList.cend(); it++) {
                    pktPtr->add_ack_seq_count(*it);
                }
                pktPtr->set_bytes_read(session->GetBytesRead());
                auto raw = make_shared<string>(pktPtr->ByteSizeLong(), 0);
                pktPtr->SerializeToArray((void *) raw->data(), raw->length());

                auto err = session->sendWith(localClientID, remoteClientID, raw,
                                             retransmissionTimeout);
                spdlog::info("sendAck({}-{}):{} sendWith result {}:{}",
                        localClientID, remoteClientID, __LINE__, err.message(), err.value());
                if (err) {
                    if (err == ErrCode::ErrConnClosed) {
                        return err;
                    }
                    this_thread::sleep_for(chrono::seconds(1));
                    continue;
                }
                session->updateBytesReadSentTime();
            }
            spdlog::warn("sendAck({}-{}):{} thread terminal", localClientID, remoteClientID, __LINE__);
            return ErrCode::ErrConnClosed;
        }

        boost::system::error_code Connection::checkTimeout() {
            boost::system::error_code ec;
            boost::asio::io_context io;

            boost::asio::deadline_timer t(io);
            auto interval = boost::posix_time::milliseconds(session->config->CheckTimeoutInterval);
            while (true) {
                t.expires_from_now(interval, ec);
                t.wait(ec);

                if (session->IsClosed()) {
                    return ErrCode::ErrSessionClosed;
                }

                time_point threshold = chrono::steady_clock::now() - retransmissionTimeout;
                // TODO Lock
                for (auto it = timeSentSeq->begin(); it != timeSentSeq->end(); it++) {
                    if (resentSeq->count(it->first) > 0) {   // resent already
                        continue;
                    }
                    if (it->second < threshold) {   // expired already
                        session->resendChan->push(make_unique<uint32_t>(it->first));
                        windowSize = std::max(windowSize / 2, session->config->MinConnectionWindowSize);
                    }
                }
            }
            return ErrCode::Success;
        }

        boost::system::error_code Connection::waitForSendWindow(const chrono::milliseconds& timeo) {
            if (SendWindowUsed() >= this->windowSize) {
                auto ret = sendWindowUpdate.pop(false, timeo);
                if (ret == nullptr) {   // timeout
                    return ErrCode::ErrMaxWait;
                }

                if (this->session->IsClosed()) {
                    return ErrCode::ErrSessionClosed;
                }
                // this_thread::sleep_for(milliseconds(100));
            }
            return ErrCode::Success;
        }
    };  // namespace NCP
};  // namespace NKN


#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

#include <libbenchmark/testsuite.hpp>

#include <libsimd/algorithms/base64/naive.hpp>

#ifdef ENABLE_SIMD_AVX2

#include <libsimd/algorithms/base64/avx2/shuffle.hpp>

#endif

const std::size_t repeats = 100;

benchmark::testrun perform_test_to_base64(std::uniform_int_distribution<> & distribution, std::mt19937 & generator, std::size_t values)
{
    std::vector<std::uint8_t> data;
	data.reserve(values);

 	for (std::size_t n = 0; n < values; ++n)
	{
        data.push_back(distribution(generator));
    }

    const std::uint32_t length = data.size();
    const std::uint32_t result_length = static_cast<std::uint32_t>(std::ceil(length / 3.0 * 4.0));
    const std::uint32_t result_padding = 4 - result_length % 4;

    std::vector<std::uint8_t> result;
    result.resize(result_length + result_padding);

	benchmark::testrun tr(values);

	// naive without SIMD
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::to_base64_naive(&data[0], length, result_padding, &result[0]);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

        tr.add_result("naive", sum / static_cast<double>(repeats));
    }

#ifdef ENABLE_SIMD_AVX2
	// AVX2 with byte mapping
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::avx2::to_base64_shuffle(&data[0], length, result_padding, &result[0]);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

		tr.add_result("AVX2 (shuffle)", sum / static_cast<double>(repeats));
	}
#endif

    return tr;
}

benchmark::testrun perform_test_from_base64(std::uniform_int_distribution<> & distribution, std::mt19937 & generator, std::size_t values)
{
    std::vector<std::uint8_t> data;
	data.reserve(values);

	for (std::size_t n = 0; n < values; ++n)
	{
        data.push_back(distribution(generator));
    }

    const std::uint32_t length = data.size();
    const std::uint32_t result_length = static_cast<std::uint32_t>(std::ceil(length / 4.0 * 3.0));

    std::vector<std::uint8_t> result;
    result.resize(result_length);

	benchmark::testrun tr(values);

	// naive without SIMD
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::from_base64_naive(&data[0], length, &result[0]);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

        tr.add_result("naive", sum / static_cast<double>(repeats));
    }

#ifdef ENABLE_SIMD_AVX2
	// AVX2 with byte mapping
	{
		std::vector<std::chrono::microseconds> durations;
		durations.reserve(repeats);

		for (std::size_t i = 0; i < repeats; ++i)
		{
			auto start = std::chrono::system_clock::now();

			simd::algorithms::avx2::from_base64_shuffle(&data[0], length, &result[0]);

			auto stop = std::chrono::system_clock::now();

			durations.push_back(std::chrono::duration_cast<std::chrono::microseconds>(stop - start));
		}

		auto sum = std::accumulate(durations.begin(),
								   durations.end(),
								   static_cast<std::size_t>(0),
								   [](std::size_t sum, auto duration)
								   {
									   return sum + duration.count();
								   });

		tr.add_result("AVX2 (shuffle)", sum / static_cast<double>(repeats));
	}
#endif

	return tr;
}

int main(int argc, char * argv[])
{
	namespace po = boost::program_options;

	po::options_description general_options("general options");
	general_options.add_options()
		("help,h", "show this help text")
		("csv,c", "generate separate CSV files of performance results")
		("csv-all", "generate a single CSV file of all performance results")
	;

	po::options_description cmdline_options("usage: bench_base64 [options]");
	cmdline_options.add(general_options);

	po::variables_map variables;
	po::store(po::parse_command_line(argc, argv, cmdline_options), variables);
	po::notify(variables);

	if (variables.count("help"))
	{
		std::cout << cmdline_options;
		return 1;
	}

	// create random numbers (inspired by https://en.cppreference.com/w/cpp/numeric/random/uniform_int_distribution)

    std::random_device rd;  // will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); // standard mersenne_twister_engine seeded with rd()

    std::uniform_int_distribution<> dist(0, 127);

	benchmark::testsuite ts_to_base64("to_base64");

	for (std::size_t i = 4; i <= 10; ++i)
	{
		ts_to_base64.add(perform_test_to_base64(dist, gen, 1024 * std::pow(2, i)));
	}

	std::cout << ts_to_base64.to_string() << std::endl;

	benchmark::testsuite ts_from_base64("from_base64");

	for (std::size_t i = 4; i <= 10; ++i)
	{
		ts_from_base64.add(perform_test_from_base64(dist, gen, 1024 * std::pow(2, i)));
	}

	std::cout << ts_from_base64.to_string() << std::endl;

	if (variables.count("csv"))
	{
		for (auto const & r : ts_to_base64.to_csv())
		{
			std::string filename(ts_to_base64.title());
			filename.append("_");
			filename.append(std::to_string(r.quantity));
			filename.append(".csv");
			std::replace(filename.begin(), filename.end(), ' ', '_');

			std::ofstream file;
			file.open(filename);
			file << r.data;
			file.close();
		}

		for (auto const & r : ts_from_base64.to_csv())
		{
			std::string filename(ts_from_base64.title());
			filename.append("_");
			filename.append(std::to_string(r.quantity));
			filename.append(".csv");
			std::replace(filename.begin(), filename.end(), ' ', '_');

			std::ofstream file;
			file.open(filename);
			file << r.data;
			file.close();
		}
	}

	if (variables.count("csv-all"))
	{
		// write base64 encoder test results
		{
			auto r = ts_to_base64.to_csv(false);

			if (!r.empty())
			{
				std::string filename(ts_to_base64.title());
				filename.append("_all.csv");
				std::replace(filename.begin(), filename.end(), ' ', '_');

				std::ofstream file;
				file.open(filename);
				file << r.front().data;
				file.close();
			}
		}

		// write base64 decoder test results
		{
			auto r = ts_from_base64.to_csv(false);

			if (!r.empty())
			{
				std::string filename(ts_from_base64.title());
				filename.append("_all.csv");
				std::replace(filename.begin(), filename.end(), ' ', '_');

				std::ofstream file;
				file.open(filename);
				file << r.front().data;
				file.close();
			}
		}
	}

    return 0;
}


/**
 * @file   global.hpp
 * @Author Leobardo Campos (leobardo.e.campos.macias@intel.com)
 * @date   June, 2018
 * @brief  Definitions and common headers
 * @section LICENSE
 *
 *  BSD-3-Clause License
 *
 * @copyright Copyright (C) 2020 Intel Corporation
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. 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.
 *
**/

#ifndef RLGLOBAL_HPP_
#define RLGLOBAL_HPP_

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
#include <vector>
#include <set>
#include <map>
#include <inttypes.h>

namespace rlmap
{

#ifndef finline
  #ifdef WIN32
    #define finline __forceinline
  #else
    #define finline inline __attribute__((always_inline))
  #endif
#endif

class Voxel;
typedef unsigned __int128 									uint128_t;
typedef uint64_t											type_key;
typedef uint16_t											data_key_size;
typedef int16_t												s_data_key_size;
typedef std::set<type_key>									keySet;
typedef std::vector<type_key>								keyVector;
typedef std::map<type_key, Voxel> 							VoxelMap;

typedef Eigen::Vector3d 									Point;
typedef Eigen::MatrixXd 									Matrix;
typedef	std::vector<Point>									PointList;
typedef std::vector<Voxel> 									stdVoxelVector;
typedef VoxelMap::iterator									VoxelMap_it;
typedef Eigen::Vector2i 									Pixel;
typedef Eigen::Vector2d 									Point2D;
typedef	std::vector<Pixel>									PixelList;
typedef cv::Mat 											FrameObj;
typedef Eigen::Matrix3d 									Rotation3D;
typedef Eigen::Quaterniond									Quaternion;

template<typename T>
T clamp(T value, T min, T max)
{
	const T tmp = value < min ? min : value;
	return tmp > max ? max : tmp;
}

Rotation3D finline GetYawRotation(double yaw)
{
	Rotation3D rot; rot<< cos(yaw), -sin(yaw), 0.0, sin(yaw), cos(yaw), 0.0, 0.0, 0.0, 1.0;
	return rot;
}
Rotation3D finline GetRollRotation(double roll)
{
	Rotation3D rot; rot<< 1.0, 0.0, 0.0, 0.0, cos(roll), -sin(roll), 0.0, sin(roll), cos(roll);
	return rot;
}
Rotation3D finline GetPitchRotation(double pitch)
{
	Rotation3D rot; rot<< cos(pitch), 0.0, sin(pitch), 0.0, 1.0, 0.0, -sin(pitch), 0.0, cos(pitch);
	return rot;
}
Rotation3D finline GetZYXRotation(double roll, double pitch, double yaw)
{
	Rotation3D rot; rot=GetYawRotation(yaw)*GetPitchRotation(pitch)*GetRollRotation(roll);
	return rot;
}

struct Transform
{
		Point 				position_;
		Rotation3D			rotation_;
		double				yaw_;

		Transform(Point position, Quaternion rotation) : position_(position)
		{
			rotation_ = rotation.normalized().toRotationMatrix();
		}

		Transform(Point position, Point rotation) : position_(position)
		{
			rotation_ = GetZYXRotation(rotation(0), rotation(1), rotation(2));
			yaw_ = rotation(2);
		}
		Transform(Point position, double yaw) : position_(position)
		{
			rotation_ = GetYawRotation(yaw);
			yaw_ = yaw;
		}
		Transform(Point position, Rotation3D rotation, double yaw) : position_(position), rotation_(rotation), yaw_(yaw)
		{
		}
};

}/* namespace rlmap */

#endif /* GLOBAL_HPP_ */


///////////////////////////////////////////////////////////////////////////////
// Copyright Christopher Kormanyos 2014.
// Copyright John Maddock 2014.
// Copyright Paul Bristow 2014.
// Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// Implement quadruple-precision (and extended) support for <complex>.

#ifndef BOOST_MATH_CSTDFLOAT_COMPLEX_2014_02_15_HPP_
  #define BOOST_MATH_CSTDFLOAT_COMPLEX_2014_02_15_HPP_

  #include <boost/math/cstdfloat/cstdfloat_types.hpp>
  #include <boost/math/cstdfloat/cstdfloat_limits.hpp>
  #include <boost/math/cstdfloat/cstdfloat_cmath.hpp>
  #include <boost/math/cstdfloat/cstdfloat_iostream.hpp>

  #if defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_LIMITS)
  #error You can not use <boost/math/cstdfloat/cstdfloat_complex.hpp> with BOOST_CSTDFLOAT_NO_LIBQUADMATH_LIMITS defined.
  #endif
  #if defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_CMATH)
  #error You can not use <boost/math/cstdfloat/cstdfloat_complex.hpp> with BOOST_CSTDFLOAT_NO_LIBQUADMATH_CMATH defined.
  #endif
  #if defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_IOSTREAM)
  #error You can not use <boost/math/cstdfloat/cstdfloat_complex.hpp> with BOOST_CSTDFLOAT_NO_LIBQUADMATH_IOSTREAM defined.
  #endif

  #if defined(BOOST_CSTDFLOAT_HAS_INTERNAL_FLOAT128_T) && defined(BOOST_MATH_USE_FLOAT128) && !defined(BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT)

  #define BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE boost::math::cstdfloat::detail::float_internal128_t
  #include <boost/math/cstdfloat/cstdfloat_complex_std.hpp>
  #undef BOOST_CSTDFLOAT_EXTENDED_COMPLEX_FLOAT_TYPE

  #endif // Not BOOST_CSTDFLOAT_NO_LIBQUADMATH_SUPPORT (i.e., the user would like to have libquadmath support)

#endif // BOOST_MATH_CSTDFLOAT_COMPLEX_2014_02_15_HPP_


//The MIT License(MIT)
//
//Copyright(c) 2016 Matthias M�ller
//
//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.

#pragma once

#include <vector>
#include <type_traits>
#include <utility>
#include <fstream>

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_fusion.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>

namespace tyti {
    namespace stl {

        ////////////////////////////////////////////////////////////////////////////////
        // vector
        template<typename T>
        struct basic_vec3
        {
            using scalar_type = T;
            static_assert(std::is_floating_point<scalar_type>::value, "T must be a floating point");
            T data[3];
            basic_vec3():data{T(0),T(0),T(0)}{}
            basic_vec3(T x, T y, T z):data{x,y,z}{}

            T operator[](size_t i)const {return data[i];}
            const T &operator[](size_t i) { return data[i]; }
        };

        using dvec3 = basic_vec3<double>;
        using vec3 = basic_vec3<float>;

        //

        ////////////////////////////////////////////////////////////////////////////////
        // solid type
        template<typename T>
        struct basic_solid
        {
            using vec3 = basic_vec3<T>;
            using scalar_type = T;
            std::string header; //solid name or header
            std::vector<vec3> normals;
            std::vector<vec3> vertices;
            std::vector<std::uint16_t> attributes; //only available in binary files
        };

        using dsolid = basic_solid<double>;
        using solid = basic_solid<float>;

        ////////////////////////////////////////////////////////////////////////////////
        // Parsers
        template <typename T, typename Iterator>
        struct stl_parser_ascii : boost::spirit::qi::grammar<Iterator, basic_solid<T>(), boost::spirit::ascii::space_type>
        {

            stl_parser_ascii() : stl_parser_ascii::base_type(start)
            {
                namespace qi = boost::spirit::qi;
                namespace ql = qi::labels;
                namespace ascii = boost::spirit::ascii;
                namespace fusion = boost::fusion;
                namespace px = boost::phoenix;

                startSolid %= qi::lexeme[qi::lit("solid ") >> (+(ascii::char_ - qi::eol) | qi::attr(""))]; //either return header or empty string if no header was found

                endSolid = qi::lexeme[qi::lit("endsolid ") >> ascii::string(ql::_r1)];

                vector %= qi::float_ >> qi::float_ >> qi::float_;

                start =
                    startSolid[px::at_c<0>(ql::_val) = ql::_1]

                    >> *(qi::lit("facet") >> qi::lit("normal") >> vector[px::push_back(px::at_c<1>(ql::_val), ql::_1)]//normal
                        //vertices
                        >> qi::lit("outer loop")
                        >> qi::repeat(3)[qi::lit("vertex") >> vector[px::push_back(px::at_c<2>(ql::_val), ql::_1)]]
                        >> qi::lit("endloop")
                        >> qi::lit("endfacet")
                        )
                    //end
                    >> endSolid(px::at_c<0>(ql::_val))
                    ;
            }

            boost::spirit::qi::rule<Iterator, basic_solid<T>(), boost::spirit::ascii::space_type> start;
            boost::spirit::qi::rule<Iterator, std::string(), boost::spirit::ascii::space_type> startSolid;
            boost::spirit::qi::rule<Iterator, basic_vec3<T>(), boost::spirit::ascii::space_type> vector;
            boost::spirit::qi::rule<Iterator, void(std::string), boost::spirit::ascii::space_type> endSolid;
        };

        template <typename T, typename Iterator>
        struct stl_parser_binary : boost::spirit::qi::grammar<Iterator, basic_solid<T>()>
        {

            stl_parser_binary() : stl_parser_binary::base_type(start)
            {
                namespace spirit = boost::spirit;
                namespace qi = boost::spirit::qi;
                namespace ql = qi::labels;
                namespace ascii = boost::spirit::ascii;
                namespace px = boost::phoenix;

                vector %= spirit::little_bin_float >> spirit::little_bin_float >> spirit::little_bin_float;

                start =
                    qi::repeat(80)[spirit::byte_[px::at_c<0>(ql::_val) += ql::_1]]              //80x8-bit header
                    >> spirit::little_dword[px::reserve(px::at_c<1>(ql::_val), ql::_1),
                    px::reserve(px::at_c<2>(ql::_val), 3 * ql::_1),
                    px::reserve(px::at_c<3>(ql::_val), ql::_1)]    //# of triangles
                    >> *(vector[px::push_back(px::at_c<1>(ql::_val), ql::_1)]  //normal
                        >> spirit::repeat(3)[vector[px::push_back(px::at_c<2>(ql::_val), ql::_1)]] //vertices
                        >> spirit::little_word[px::push_back(px::at_c<3>(ql::_val), ql::_1)]  //attribute
                        )
                    //end
                    ;
            }

            boost::spirit::qi::rule<Iterator, basic_solid<T>()> start;
            boost::spirit::qi::rule<Iterator, basic_vec3<T>()> vector;
            boost::spirit::qi::rule<Iterator, void(std::string)> endSolid;
        };

        template <typename T, typename Iterator>
        struct stl_parser : boost::spirit::qi::grammar<Iterator, basic_solid<T>(), boost::spirit::ascii::space_type>
        {
            stl_parser() : stl_parser::base_type(start)
            {
                start %= stl_ascii_ | boost::spirit::qi::lexeme[stl_binary_];
            }
            stl_parser_ascii<T, Iterator> stl_ascii_;
            stl_parser_binary<T, Iterator> stl_binary_;
            boost::spirit::qi::rule<Iterator, basic_solid<T>(), boost::spirit::ascii::space_type> start;
        };

        ////////////////////////////////////////////////////////////////////////////////
        // Interface functions
        ////////////////////////////////////////////////////////////////////////////////

        ////////////////////////////////////////////////////////////////////////////////
        // Readers
        template<typename T, typename IterT>
        inline bool read(basic_solid<T>& out, IterT begin, IterT end)
        {
            stl_parser< T, IterT > p;
            return boost::spirit::qi::phrase_parse(begin, end, p, boost::spirit::ascii::space, out);
        }

        template<typename T, typename IterT>
        inline std::pair< basic_solid<T>, bool > read(IterT begin, IterT end)
        {
            basic_solid<T> o;
            bool r = read(o, begin, end);
            return std::make_pair<basic_solid<T>, bool>(std::move(o), std::move(r));
        }

        template<typename IterT>
        inline std::pair< solid, bool > read(IterT begin, IterT end)
        {
            return read<float>(begin, end);
        }

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

        template<typename T>
        inline bool read(basic_solid<T>& out, const std::string& fileName)
        {
            std::ifstream f(fileName, std::ios::binary);
            std::string str;
            f.seekg(0, std::ios::end);
            str.resize((unsigned)f.tellg());
            f.seekg(0, std::ios::beg);
            f.read(&str[0], str.size());
            f.close();

            return read<T>(out, str.cbegin(), str.cend());
        }

        template<typename T>
        inline std::pair<basic_solid<T>, bool> read(const std::string& fileName)
        {
            basic_solid<T> out;
            bool r = read(out, fileName);
            return std::make_pair<basic_solid<T>, bool>(std::move(out), std::move(r));
        }

        inline std::pair<solid, bool> read(const std::string& fileName)
        {
            return read<float>(fileName);
        }

        ////////////////////////////////////////////////////////////////////////////////
        // Writers

        template<typename T, typename streamT>
        void write_ascii(streamT& out, const basic_solid<T>&s)
        {
            out << "solid " << s.header << "\n";

            for (size_t i = 0; i < s.normals.size(); ++i)
            {
                out << "\tfacet normal "
                    << s.normals[i][0] << " "
                    << s.normals[i][1] << " "
                    << s.normals[i][2] << " "
                    << "\n";

                out << "\t\touter loop\n";
                for (size_t j = 0; j < 3; ++j)
                {
                    out << "\t\t\tvertex "
                        << s.vertices[3 * i + j][0] << " "
                        << s.vertices[3 * i + j][1] << " "
                        << s.vertices[3 * i + j][2] << " "
                        << "\n";
                }
                out << "\t\tendloop\n"
                    << "\tendfacet\n";
            }
            out << "endsolid " << s.header << "\n";
        }

        namespace detail
        {
            template<typename streamT>
            inline void write_vectorF32(streamT& out, const double* p)
            {
                for (size_t i = 0; i < 3; ++i)
                {
                    const float f = static_cast<float>(p[i]);
                    out.write(reinterpret_cast<const char *>(p), sizeof(float));
                }
            }

            template<typename streamT>
            inline void write_vectorF32(streamT& out, const float* p)
            {
                out.write(reinterpret_cast<const char*>(p), sizeof(float) * 3);
            }
        }

        template<typename T, typename streamT>
        void write_binary(streamT& out, basic_solid<T>&s)
        {
            s.header.resize(80);
            out.write(&s.header[0], 80);

            const size_t num_triangles{ s.normals.size() };
            s.attributes.resize(num_triangles);

            out.write(reinterpret_cast<const char*>(&num_triangles), 4);

            out << num_triangles;
            for (size_t i = 0; i < num_triangles; ++i)
            {
                //we cannot do direct writ
                detail::write_vectorF32(out, s.normals[i].data);
                for (size_t j = 0; j < 3; ++j)
                    detail::write_vectorF32(out, s.vertices[3 * i + j].data);
                out.write(reinterpret_cast<const char*>(&s.attributes[i]), sizeof(uint16_t));
            }
        }

        template<typename T, typename streamT>
        void write(streamT& out, basic_solid<T>& s, bool binary)
        {
            s.vertices.resize(3 * s.normals.size());

            if (binary)
                write_binary(out, s);
            else
                write_ascii(out, s);
        }

    }
}//end namespaces

BOOST_FUSION_ADAPT_TPL_STRUCT(
(T),
(tyti::stl::basic_solid)(T),
(std::string, header),
(std::vector< tyti::stl::basic_vec3<T> >, normals),
(std::vector< tyti::stl::basic_vec3<T> >, vertices),
(std::vector<std::uint16_t>, attributes)
);

BOOST_FUSION_ADAPT_TPL_STRUCT(
(T),
(tyti::stl::basic_vec3)(T),
(T, data[0])
(T, data[1])
(T, data[2])
);


/*
    Copyright (c) 2014, Philipp Krähenbühl
    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 Stanford University 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 Philipp Krähenbühl ''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 Philipp Krähenbühl 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.
*/
#include "util/win_util.h"
#include "util/threading.h"
#include "splitcriterion.h"
#include <random>
#include <stdexcept>
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

int SplitCriterion::repLabel() const {
	return -1;
}
class LabeledSplit: public SplitCriterion {
protected:
	VectorXi lbl_;
	VectorXf weight_;
	virtual float score( const ArrayXf & p ) const = 0;
public:
	LabeledSplit(){
	}
	LabeledSplit( const RMatrixXf & lbl, const VectorXf & weight ): weight_(weight) {
		if( lbl.cols()!= 1 )
			throw std::invalid_argument( "Only 1d labels supported!" );
		lbl_ = lbl.cast<int>();
	}
	virtual float gain( const VectorXb& is_left ) const {
		const int N = lbl_.maxCoeff()+1;
		ArrayXf wl = 1e-20*ArrayXf::Ones(N), wr = 1e-20*ArrayXf::Ones(N);
		for( int i=0; i<(int)lbl_.size(); i++ )
			if( is_left[i] )
				wl[lbl_[i]] += weight_[i];
			else
				wr[lbl_[i]] += weight_[i];
		const float l = wl.sum(), r = wr.sum();
		const float sl = score(wl/l), sr = score(wr/r);
		printf("%d  %d\n", (int)l, (int)r );
		return score( (wl+wr)/(l+r) ) - (sl*l/(l+r) + sr*r/(l+r));
	}
	virtual float bestThreshold( const VectorXf & f, float * gain ) const {
		const int N = lbl_.maxCoeff()+1;
		const float EPS=1e-6;
		// Create the feature/label pairs
		std::vector< std::pair<float,int> > elements( f.size() );
		for( int i=0; i<f.size(); i++ )
			elements[i] = std::make_pair( f[i], i );
		std::sort(elements.begin(), elements.end() );

		// And compute the probabilities and cirterion
		ArrayXf wl = 1e-20*ArrayXf::Ones(N), wr = 1e-20*ArrayXf::Ones(N);
		for( int i=0; i<lbl_.size(); i++ )
			wr[ lbl_[i] ] += weight_[i];

		// Initialize the thresholds
		float best_gain = 0, tbest = (elements.front().first+elements.back().first)/2, last_t = elements.front().first;
		const float tot_s = score( wr/wr.sum() );
		// Find the best threshold
		for( auto i: elements ) {
			const float t = i.first;
			const int j = i.second;
			// If there is a threshold
			if( t - last_t > EPS ) {
				// Compute the score
				const float l = wl.sum(), r = wr.sum();
				const float sl = score(wl/l), sr = score(wr/r);
				const float g = tot_s - ( sl*l/(l+r) + sr*r/(l+r) );
				if( g > best_gain ) {
					best_gain = g;
					tbest = (last_t+t)/2.;
				}
			}
			// Update the probabilities
			wl[ lbl_[j] ] += weight_[j];
			wr[ lbl_[j] ] -= weight_[j];
			last_t = t;
		}
		if( gain )
			*gain = best_gain;
		return tbest;
	}
	virtual bool is_pure( ) const {
		for( int i=1; i<lbl_.size(); i++ )
			if( lbl_[0] != lbl_[i] )
				return false;
		return true;
	}
	virtual int repLabel() const {
		// Count the label occurence
		const int n_lbl = lbl_.maxCoeff()+1;
		VectorXi lbl_cnt = VectorXi::Zero( n_lbl );
		for( int i=0; i<lbl_.size(); i++ )
			lbl_cnt[lbl_[i]]++;
		// And return the maximum
		int rep_lbl = 0;
		for( int i=0; i<lbl_.size(); i++ )
			if( lbl_cnt[lbl_[rep_lbl]] < lbl_cnt[lbl_[i]] )
				rep_lbl = i;
		return rep_lbl;
	}
};
class GiniSplit: public LabeledSplit {
public:
	template<typename ...ARGS> GiniSplit(ARGS... args) :LabeledSplit(args...) {}
	virtual std::shared_ptr<SplitCriterion> create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared<GiniSplit>( lbl, weight );
	}
	virtual float score( const ArrayXf & p ) const {
		return (p*(1-p)).sum();
	}
};
class EntropySplit: public LabeledSplit {
public:
	template<typename ...ARGS> EntropySplit(ARGS... args) :LabeledSplit(args...) {}
	virtual std::shared_ptr<SplitCriterion> create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared<EntropySplit>( lbl, weight );
	}
	virtual float score( const ArrayXf & p ) const {
		return -(p*(p+1e-20).log()).sum();
	}
};
RMatrixXf pairwiseDistance( const RMatrixXf & X, int N ) {
	static std::mt19937 gen;
	// Do a random pairwise projection
	const int M = X.cols();
	RMatrixXf r( X.rows(), N );
	std::vector<int> o1(N), o2(N);
	for( int i=0; i<N; i++ ) {
		o1[i] = 1+(gen()%(M-1));
		o2[i] = gen()%o1[i];
	}
	for( int i=0; i<X.rows(); i++ )
		for( int j=0; j<N; j++ )
			r(i,j) = ((X(i,o1[j]) == X(i,o2[j])));
	return r;
}
// RMatrixXf pairwiseDistance( const RMatrixXf & X, int N ) {
// 	static std::mt19937 gen;
// 	// Do a random pairwise projection
// 	const int M = X.cols();
//
// 	// Figure out if a column has more than one value
// 	VectorXb singleton = VectorXb::Ones( M );
// 	VectorXf v = X.row(0);
// 	for( int i=1; i<X.rows(); i++ )
// 		for( int j=0; j<M; j++ )
// 			if( singleton[j] )
// 				singleton[j] = (v[j] == X(i,j));
// 	int N_ns = (singleton.array() == 0).cast<int>().sum();
// 	if( N_ns == 0 )
// 		return RMatrixXf::Zero( X.rows(), 1 );
//
// 	// Adjust N to the fewer samples in theory by a factor of 1-(1-N_ns/M)^2,
// 	// in practice 2*N_ns/M seems close enough [without the qudartic term]
// 	if( 2*N_ns < M )
// 		N = 2*N*N_ns / M;
//
// 	VectorXi good_o( N_ns );
// 	for( int i=0,j=0; i<M; i++ )
// 		if( !singleton[i] )
// 			good_o[j++] = i;
//
// 	RMatrixXf r( X.rows(), N );
// 	std::vector<int> o1(N), o2(N);
// 	for( int i=0; i<N; i++ ) {
// 		o1[i] = good_o[gen()%N_ns];
// 		o2[i] = gen()%M;
// 	}
// 	for( int i=0; i<X.rows(); i++ )
// 		for( int j=0; j<N; j++ ) {
// 			r(i,j) = (X(i,o1[j]) != X(i,o2[j]));
// 		}
// 	return r;
// }
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
	const bool fast = true, very_fast = true;
	// Remove the DC
	RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
	// ... and use (pc > 0)
	VectorXf lbl = VectorXf::Zero( Y.rows() );
	// Find the largest PC
	if( very_fast ) {
		// Find the PC using poweriterations
		lbl.setRandom();
		float old_v = 0;
		for( int it=0; it<20; it++ ){
			// Normalize
			lbl.array() /= lbl.norm()+std::numeric_limits<float>::min();
			lbl = dY*(dY.transpose()*lbl);
			float v = lbl.norm();
			if( fabs(v-old_v) < 1e-4*fabs(v) )
				break;
			old_v = v;
		}
		VectorXf tmp = dY.transpose()*lbl;
		tmp.array() /= tmp.norm()+std::numeric_limits<float>::min();
		lbl = dY*tmp;
	}
	else if(fast) {
		// Compute the eigen values of the covariance (and project onto the largest eigenvector)
		MatrixXf cov = dY.transpose()*dY;
		SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
		MatrixXf ev = eigensolver.eigenvectors();
		lbl = dY * ev.col( ev.cols()-1 );
	}
	else {
		// Use the SVD
		JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
		// Project onto the largest PC
		lbl = svd.matrixU().col(0) * svd.singularValues()[0];
	}
	// Find the representative label
	if( rep_label )
		dY.array().square().rowwise().sum().minCoeff( rep_label );

	return (lbl.array() > 0).cast<float>();
}

template<typename Split> class Structured: public Split {
protected:
	int rep_label_;
	int n_struc_samples_;
public:
	Structured( int n_struc_samples = 256 ):n_struc_samples_(n_struc_samples){}
	Structured( const RMatrixXf & lbl, const VectorXf & weight, int n_struc_samples ): Split( project1D(pairwiseDistance(lbl,n_struc_samples),&rep_label_), weight ) {
	}
	virtual std::shared_ptr< SplitCriterion > create( const RMatrixXf & lbl, const VectorXf & weight ) const {
		return std::make_shared< Structured<Split> >( lbl, weight, n_struc_samples_ );
	}
	virtual int repLabel() const {
		return rep_label_;
	}
};
std::shared_ptr<SplitCriterion> entropySplit() {
	return std::make_shared<EntropySplit>();
}
std::shared_ptr<SplitCriterion> giniSplit() {
	return std::make_shared<GiniSplit>();
}
std::shared_ptr<SplitCriterion> structEntropySplit( int n_struc_samples ) {
	return std::make_shared< Structured<EntropySplit> >( n_struc_samples );
}
std::shared_ptr<SplitCriterion> structGiniSplit( int n_struc_samples ) {
	return std::make_shared< Structured<GiniSplit> >( n_struc_samples );
}


// #include </home/ssonawane/Desktop/catkin_ws/src/ethz_piksi_ros/message_converter/include/message_converter/converter_node.hpp>
#include "ros/ros.h"
#include <boost/array.hpp>
#include <geometry_msgs/PointStamped.h>
#include <gnss_msgs/BaselinePosition.h>
#include <gnss_msgs/BaselineVelocity.h>
#include <geometry_msgs/PointStamped.h>
#include <piksi_rtk_msgs/BaselineNed.h>
#include <piksi_rtk_msgs/VelNedCov.h>
#include <libsbp_ros_msgs/MsgBaselineNed.h>
#include <libsbp_ros_msgs/MsgVelNedCov.h>

class SwiftNavRover{
public:
    SwiftNavRover(ros::NodeHandle* node_handle);
    ros::NodeHandle nh;

    // ros::NodeHandle rover;
    ros::Subscriber vel_sub;
    ros::Subscriber pos_sub;
    ros::Publisher ned_point_fix;
    ros::Publisher ned_baseline_position_fix;
    ros::Publisher ned_vel_cov_fix;
    void init();
    void publishBaselinePosition(ros::Time t, float n, float e, float d, int tow, boost::array<double, 9> covariance, int n_sats, int fixed_mode);
    void baselinePositionCallback(const libsbp_ros_msgs::MsgBaselineNed::ConstPtr & msg);
    void publishBaselineVelocity(ros::Time t, int tow,float n,float e,float d,boost::array<double, 9> covariance,int n_sats,int vel_mode,int ins_mode);
    void baselineVelocityCallback(const libsbp_ros_msgs::MsgVelNedCov::ConstPtr & msg);
};

SwiftNavRover::SwiftNavRover(ros::NodeHandle* node_handle):nh(*node_handle){
    SwiftNavRover::init();
}

void SwiftNavRover::init()
{
    vel_sub = nh.subscribe("/rover/piksi/position_receiver_0/sbp/vel_ned_cov", 1000, &SwiftNavRover::baselineVelocityCallback, this);
    pos_sub = nh.subscribe("/rover/piksi/position_receiver_0/sbp/baseline_ned", 1000, &SwiftNavRover::baselinePositionCallback, this);
}

void SwiftNavRover::baselinePositionCallback(const libsbp_ros_msgs::MsgBaselineNed::ConstPtr & msg)
{
    float n = 1e-3*(msg->n);
    float e = 1e-3*(msg->e);
    float d = 1e-3*(msg->d);
    int tow = msg->tow;
    int h_accuracy = msg->h_accuracy;
    int v_accuracy = msg->v_accuracy;
    boost::array<double, 9> covariance = {{h_accuracy, 0, 0, 0, h_accuracy, 0, 0, 0, v_accuracy}};
    int n_sats = msg->n_sats;
    int fixed_mode = (msg->flags) & (0b00000111);
    ros::Time t = ros::Time::now();
    SwiftNavRover::publishBaselinePosition(t,n,e,d,tow,covariance,n_sats,fixed_mode);

}

void SwiftNavRover::baselineVelocityCallback(const libsbp_ros_msgs::MsgVelNedCov::ConstPtr & msg)
{
    float n = 1e-3*(msg->n);
    float e = 1e-3*(msg->e);
    float d = 1e-3*(msg->d);
    int tow = msg->tow;
    float cov_n_n = msg->cov_n_n;
    float cov_n_e = msg->cov_n_e;
    float cov_n_d = msg->cov_n_d;
    float cov_e_e = msg->cov_e_e;
    float cov_e_d = msg->cov_e_d;
    float cov_d_d = msg->cov_d_d;
    int n_sats = msg->n_sats;
    int ins_mode = ((msg->flags) & (0b00011000)) >> 3;
    int vel_mode = (msg->flags) & (0b00000111);
    boost::array<double, 9> covariance = {{cov_n_n,cov_n_e,cov_n_d,cov_n_e,cov_e_e,cov_e_d,cov_n_d,cov_e_d,cov_d_d}};
    ros::Time t = ros::Time::now();
    SwiftNavRover::publishBaselineVelocity(t, tow, n, e, d, covariance, n_sats, vel_mode, ins_mode);
}

void SwiftNavRover::publishBaselinePosition(ros::Time t,float n,float e,float d,int tow,boost::array<double, 9> covariance,int n_sats,int fixed_mode)
{
    geometry_msgs::PointStamped msgPointStamped;
    msgPointStamped.header.frame_id = "ned";
    msgPointStamped.header.stamp = t;
    msgPointStamped.point.x = n;
    msgPointStamped.point.y = e;
    msgPointStamped.point.z = d;
    ned_point_fix = nh.advertise<geometry_msgs::PointStamped>("/rover/ned_point_fix", 1000);
    ned_point_fix.publish(msgPointStamped);
    gnss_msgs::BaselinePosition msg;
    msg.header.frame_id = "ned";
    msg.header.stamp = t;
    msg.gps_tow = tow;
    msg.n = n;
    msg.e = e;
    msg.d = d;
    msg.n_sats = n_sats;
    msg.covariance = covariance;
    msg.mode = fixed_mode;
    ned_baseline_position_fix = nh.advertise<gnss_msgs::BaselinePosition>("/rover/ned_baseline_position_fix", 1000);
    ned_baseline_position_fix.publish(msg);
}

void SwiftNavRover::publishBaselineVelocity(ros::Time t, int tow,float n,float e,float d,boost::array<double, 9> covariance,int n_sats,int vel_mode,int ins_mode)
{
    gnss_msgs::BaselineVelocity msg;
    msg.header.frame_id = "ned";
    msg.header.stamp = t;
    msg.gps_tow = tow;
    msg.n = n;
    msg.e = e;
    msg.d = d;
    msg.n_sats = n_sats;
    msg.covariance = covariance;
    msg.vel_mode = vel_mode;
    msg.ins_mode = ins_mode;
    ned_vel_cov_fix = nh.advertise<gnss_msgs::BaselineVelocity>("/rover/ned_vel_cov_fix", 1000);
    ned_vel_cov_fix.publish(msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rover");
    ros::NodeHandle n;
    SwiftNavRover obj(&n);
    ros::spin();
    return 0;
}


#include <boost/units/systems/si/mass_density.hpp>


#include <fstream>
#include <iostream>
#include <boost/numeric/conversion/cast.hpp>
#include "WavefrontObjLoader.hpp"
namespace trianglemesh3d {

WavefrontObjLoader::WavefrontObjLoader(std::istream& obj_stream) {
	parse_lines(obj_stream);
	expand_vertices_and_normals();

	// clear unused members
	m_parsed_normals.clear();
	m_parsed_tri_faces.clear();
	m_parsed_vertices.clear();
}

void WavefrontObjLoader::parse_lines(std::istream& obj_stream) {
	std::string current_line;
	while (std::getline(obj_stream, current_line)) {
		process_text_line(current_line);
	}
}

void WavefrontObjLoader::process_text_line(const std::string& line) {
	std::stringstream ss;
	ss << line;
	std::string command;
	ss >> command;
	dispatch_command(command, ss);
}

void WavefrontObjLoader::dispatch_command(const std::string& command, std::stringstream& rest) {
	if (command == "#") {
		// ignore comment lines
	} else if (command == "v") {
		process_vertex_data(rest);
	} else if (command == "vn") {
		process_vertex_normal_data(rest);
	} else if (command == "f") {
		process_face_data(rest);
	} else {
		// ignore unknown commands
	}
}

void WavefrontObjLoader::process_vertex_data(std::istream& rest) {
    Vector3d vertex;
    rest >> vertex;
    if (rest.fail()) {
        throw std::runtime_error("failed to parse vertex");
    }
    m_parsed_vertices.push_back(vertex);
}

void WavefrontObjLoader::process_vertex_normal_data(std::istream& rest) {
    Vector3d normal;
    rest >> normal;
    if (rest.fail()) {
        throw std::runtime_error("failed to parse vertex normal");
    }
    m_parsed_normals.push_back(normal);
}

void WavefrontObjLoader::process_face_data(std::istream& rest) {
    TriFace face;
    rest >> face;
    // TODO: Check that not more stuff to parse left?
    m_parsed_tri_faces.push_back(face);
}

std::istream& operator>>(std::istream& is, Vector3d& v) {
    return (is >> v.x >> v.y >> v.z);
}

std::istream& operator>>(std::istream& is, TriFace& f) {
    std::string part;

    for (size_t vertex_no = 0; vertex_no < 3; vertex_no++) {
        is >> part;
        if (is.fail()) {
            throw std::runtime_error("failed reading triangle vertex");
        }
        std::stringstream part_ss(part);
        std::vector<std::string> part_tokens;
        std::string token;
        while (std::getline(part_ss, token, '/')) {
            part_tokens.push_back(token);
        }

        if (part_tokens.size() != 3) {
            throw std::runtime_error("failed parsing triangle vertex");
        }

        try {
            const auto vertex_ind = boost::numeric_cast<size_t>(std::stoi(part_tokens[0]) - 1);
            const auto normal_ind = boost::numeric_cast<size_t>(std::stoi(part_tokens[2]) - 1);
            f.vertex_inds[vertex_no] = vertex_ind;
            f.normal_inds[vertex_no] = normal_ind;
        } catch (boost::numeric::bad_numeric_cast& /*e*/) {
            throw std::runtime_error("bad numerical cast detected");
        } catch (std::runtime_error& /*e*/) {
            throw std::runtime_error("error interpreting triangle face");
        }
    }
    return is;
}

size_t WavefrontObjLoader::num_vertices() const {
    return m_vertex_data.size() / 3;
}

const double* WavefrontObjLoader::vertex_data() const {
    return m_vertex_data.data();
}

const double* WavefrontObjLoader::normal_data() const {
    return m_normal_data.data();
}

void WavefrontObjLoader::expand_vertices_and_normals() {
    const auto num_triangles = m_parsed_tri_faces.size();
    const auto vector_size = num_triangles*3*3;
    m_vertex_data.reserve(vector_size);
    m_normal_data.reserve(vector_size);

    for (const auto& tri : m_parsed_tri_faces) {
        for (size_t i = 0; i < 3; i++) {
            const auto v_ind = tri.vertex_inds[i];
            if (v_ind >= m_parsed_vertices.size()) {
                throw std::runtime_error("invalid vertex index");
            }
            m_vertex_data.push_back(m_parsed_vertices[v_ind].x);
            m_vertex_data.push_back(m_parsed_vertices[v_ind].y);
            m_vertex_data.push_back(m_parsed_vertices[v_ind].z);

            const auto n_ind = tri.normal_inds[i];
            if (n_ind >= m_parsed_normals.size()) {
                throw std::runtime_error("invalid normal index");
            }
            m_normal_data.push_back(m_parsed_normals[n_ind].x);
            m_normal_data.push_back(m_parsed_normals[n_ind].y);
            m_normal_data.push_back(m_parsed_normals[n_ind].z);
        }
    }
}

}   // end namespace


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_VMX_ALTIVEC_NONE_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_VMX_ALTIVEC_NONE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/reduction/functions/none.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( none_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_ < arithmetic_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                      ))
                                    )
  {
    typedef typename meta::as_logical<typename A0::value_type>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::none( is_nez(a0) );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( none_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_ < logical_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                      ))
                                    )
  {
    typedef typename meta::as_logical<typename A0::value_type>::type result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return result_type( vec_all_eq( a0(), False<A0>()() ) );
    }
  };
} } }

#endif
#endif


#ifndef options_hpp
#define options_hpp

#include <iostream>
#include <sstream>
#include <fstream>
#include <stdio.h>
#include <cstring>
#include <string>
#include <limits.h>
#include <boost/format.hpp>
#include "gadgets.hpp"
#include <Eigen/Eigen>

using namespace std;
using namespace boost;

const unsigned Megabase = 1e6;

class Options {
public:

    unsigned shuffleMarkers      = 1;
#ifdef USE_MPI
    //bool     mpiBayesGroups    = false;
    string   groupIndexFile    = "";
    string   groupMixtureFile  = "";
    bool     restart             = false;
    bool     useXfilesInRestart  = true;
    bool     sparseSync          = false;
    bool     bedSync             = false;
    unsigned syncRate            = 1;
    bool     bedToSparse         = false;
    bool     readFromBedFile     = false;
    bool     readFromSparseFiles = false;
    bool     mixedRepresentation = false;
    unsigned blocksPerRank       = 1;     //EO: for bed -> sparse conversion, to split blocks if too large
    bool     checkRam            = false;
    unsigned checkRamTasks       = 0;
    unsigned checkRamTpn         = 0;
#endif
    unsigned numberMarkers       = 0;
    unsigned numberIndividuals   = 0;
    unsigned chainLength;
    unsigned burnin;
    unsigned seed;
    unsigned thin;                       // save every this th sampled value in MCMC
    unsigned save;                       // sampling rate of the epsilon vector
    vector<double> S;                    // variance components

    //marion : include annotation variables
    unsigned int numGroups;
    Eigen::MatrixXd mS;
    string groupFile;
    string mSfile;
    string failureFile;
    string bayesW_version;
    string quad_points;
    string priorsFile;
    string dPriorsFile;

    string title;
    string analysisType;
    string bayesType;

    string phenotypeFile;
    bool   multi_phen = false;
    vector<string> phenotypeFiles;
    bool   interleave = false;

    string markerBlocksFile;
    string bedFile;
    string mcmcOutDir  = "";
    string mcmcOutNam  = "default_output_name";
    string mcmcOut     = "default_output_name";
    string sparseDir   = "";
    string sparseBsn   = "";
    string optionFile;
    string covariatesFile;         // for extra covariates.
    bool   covariates  = false;    // for extra covatiates.
    bool   compress    = false;
    bool   deltaUpdate = true;     // Use the delta epsilon to pass the message in mpi

    // Use BED representation over SPARSE if fraction of non-zero elements (fnz)
    // is greater than this threshold:
    double threshold_fnz = 0.06;

    // FH related
    double betaA = 0.0;
    double betaB = 0.0;
    double tau0  = 1.0;  // global hyperparameter
    double s02c  = 1.0;  // scale regularising slab
    //EO: should be of type int. const as well?
    double v0c   = 3;  // degrees of freedom regularising slab
    double v0L   = 3;  // degrees of freedom local parameters
    double v0t   = 3;  // degrees of freedom global paramaeters

    string options_s;

    Options(){
        chainLength             = 10000;
        burnin                  = 5000;
        seed                    = static_cast<unsigned int>(std::time(0));
        thin                    = 5;
        save                    = 10;
        S.resize(3);
        S[0]                    = 0.01;
        S[1]                    = 0.001;
        S[2]                    = 0.0001;
        title                   = "brr";
        analysisType            = "Bayes";
        bayesType               = "C";
        phenotypeFile           = "";
        markerBlocksFile        = "";
        bedFile                 = "";
        sparseDir               = "";
        sparseBsn               = "";
        optionFile		= "";
        numGroups		= 1;
        groupFile               = "";
        priorsFile              = "";
        dPriorsFile             = "";
        mSfile                  = "";
	betaA                   = 1.0;
	betaB                   = 1.0;
    }

    void inputOptions(const int argc, const char* argv[]);

    void printBanner(void);
    void printProcessingOptions(void);

private:
    void readFile(const string &file);
    void makeTitle(void);
    void seedEngine(void);
};

#endif /* options_hpp */


/*

Copyright (c) 2005-2019, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef TEST3DBIDOMAINFOREFFICIENCYWITHFASTERODES_HPP_
#define TEST3DBIDOMAINFOREFFICIENCYWITHFASTERODES_HPP_

#include <cxxtest/TestSuite.h>
#include "MonodomainProblem.hpp"
#include "BidomainProblem.hpp"
#include <petscvec.h>
#include <vector>
#include <boost/shared_ptr.hpp>
#include "PetscSetupAndFinalize.hpp"
#include "AbstractCardiacCellFactory.hpp"
#include "LuoRudy1991BackwardEuler.hpp"
#include "RegularStimulus.hpp"
#include "RandomNumberGenerator.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include "SimpleStimulus.hpp"

class BidomainFaceStimulusCellFactory : public AbstractCardiacCellFactory<3>
{
private:
    boost::shared_ptr<SimpleStimulus> mpStimulus;
    boost::shared_ptr<RegularStimulus> mpRegStimulus;

public:
    //Pdetime step is (by default) 0.01
    //Odetime step set below to 0.01 as backward Euler should be stable
    BidomainFaceStimulusCellFactory()
        : AbstractCardiacCellFactory<3>(),
          mpStimulus(new SimpleStimulus(-900.0*1000, 0.5)),
          mpRegStimulus(new RegularStimulus(-900.0*1000, 0.5, 100.0, 0.0))
    {
    }

    AbstractCardiacCell* CreateCardiacCellForTissueNode(Node<3>* pNode)
    {
        boost::shared_ptr<AbstractIvpOdeSolver> p_solver;
        if (pNode->GetPoint()[0] == 0.0)
        {
            //std::cout << node+1 << "\n";
            return new CellLuoRudy1991FromCellMLBackwardEuler(p_solver, mpRegStimulus);
        }
        else
        {
            return new CellLuoRudy1991FromCellMLBackwardEuler(p_solver, mpZeroStimulus);
        }
    }
};

class Test3dBidomainProblemForEfficiencyWithFasterOdes :  public CxxTest::TestSuite
{
public:

    void TestBidomain3d()
    {
        HeartConfig::Instance()->SetIntracellularConductivities(Create_c_vector(1.75, 1.75, 1.75));
        HeartConfig::Instance()->SetExtracellularConductivities(Create_c_vector(7.0, 7.0, 7.0));
        HeartConfig::Instance()->SetSimulationDuration(150.0);
        HeartConfig::Instance()->SetMeshFileName("mesh/test/data/3D_0_to_.5mm_1889_elements_irregular");

        BidomainFaceStimulusCellFactory bidomain_cell_factory;

        BidomainProblem<3> bidomain_problem( &bidomain_cell_factory );

        bidomain_problem.PrintOutput(false);

        PetscTools::SetOption("-log_summary", "");

        bidomain_problem.Initialise();

        bidomain_problem.Solve();

        Vec voltage=bidomain_problem.GetSolution();
        ReplicatableVector voltage_replicated;
        voltage_replicated.ReplicatePetscVector(voltage);

        /*
         * Test the top right node against the right one in the 1D case,
         * comparing voltage, and then test all the nodes on the right hand
         * face of the cube against the top right one, comparing voltage.
         */
        bool need_initialisation = true;
        double probe_voltage=-9999.;

        need_initialisation = true;

        // Test the RHF of the mesh
        for (unsigned i = 0; i < bidomain_problem.rGetMesh().GetNumNodes(); i++)
        {
            if (bidomain_problem.rGetMesh().GetNode(i)->GetPoint()[0] == 0.05)
            {
                // x = 0 is where the stimulus has been applied
                // x = 0.05cm is the other end of the mesh and where we want to
                //       to test the value of the nodes

                if (need_initialisation)
                {
                    probe_voltage = voltage_replicated[2*i];
                    need_initialisation = false;
                }
                else
                {
                    // the voltage at the end face varies a little because
                    // of drift due to the orientation of the tets in the mesh,
                    // hence the tolerance of 0.02
                    TS_ASSERT_DELTA(voltage_replicated[2*i], probe_voltage, 0.02);
                }

                // Check against hard coded value
                // For 50 ms test TS_ASSERT_DELTA(voltage_replicated[2*i],  7.3, 0.2);
                // For 150 ms test
                TS_ASSERT_DELTA(voltage_replicated[2*i], -1.735, 0.1);
            }
        }
    }
};

#endif /*TEST3DBIDOMAINFOREFFICIENCYWITHFASTERODES_HPP_*/