#include "PointLocator.h"

#include "tensor/EigenMap.h"

#include <Eigen/LU>

#include <algorithm>
#include <stdexcept>
#include <utility>

namespace tndm {

template <std::size_t D>
PointLocator<D>::PointLocator(std::shared_ptr<Curvilinear<D>> cl, double atol, double rtol,
                              double bbox_factor, double max_its)
    : cl_(std::move(cl)), atol_(atol), rtol_(rtol), max_its_(max_its), bbox_(cl_->numElements()) {

    auto E = cl_->evaluateBasisAt(cl_->refElement().refNodes());
    auto num = cl_->refElement().refNodes().size();
    auto F = Managed(cl_->mapResultInfo(num));

    for (std::size_t elNo = 0; elNo < cl_->numElements(); ++elNo) {
        cl_->map(elNo, E, F);
        for (std::size_t d = 0; d < D; ++d) {
            auto& bb = bbox_[elNo][d];
            bb[0] = std::numeric_limits<double>::max();
            bb[1] = std::numeric_limits<double>::lowest();
            for (std::size_t i = 0; i < num; ++i) {
                bb[0] = std::min(bb[0], F(d, i));
                bb[1] = std::max(bb[1], F(d, i));
            }
            double r = (bb[1] - bb[0]) / 2.0;
            double mid = (bb[1] + bb[0]) / 2.0;
            bb[0] = mid - bbox_factor * r;
            bb[1] = mid + bbox_factor * r;
        }
    }
}

template <std::size_t D>
bool PointLocator<D>::inside_bbox(std::size_t elNo, point_t const& point) const {
    bool inside = true;
    auto& bb = bbox_[elNo];
    for (std::size_t d = 0; d < D; ++d) {
        inside = inside && (point[d] >= bb[d][0]) && (point[d] <= bb[d][1]);
    }
    return inside;
}

template <std::size_t D>
auto PointLocator<D>::xInv(std::size_t elNo, point_t const& point, XInvTmp& tmp) const -> point_t {
    point_t xi;
    xi.fill(1.0 / 3.0);
    std::size_t it = 0;

    double residual2_0 = 0.0;
    auto E = cl_->evaluateBasisAt({xi});
    cl_->map(elNo, E, tmp.F);
    auto X_0 = tmp.F.subtensor(slice{}, 0);
    auto F = EigenMap(X_0);
    for (std::size_t d = 0; d < D; ++d) {
        F(d) = point[d] - F(d);
        residual2_0 = F(d) * F(d);
    }

    while (it < max_its_) {
        // point - X(xi)
        auto E = cl_->evaluateBasisAt({xi});
        cl_->map(elNo, E, tmp.F);
        auto X_0 = tmp.F.subtensor(slice{}, 0);
        auto F = EigenMap(X_0);
        double residual2 = 0.0;
        for (std::size_t d = 0; d < D; ++d) {
            F(d) = point[d] - F(d);
            residual2 = F(d) * F(d);
        }
        if (residual2 < atol_ + rtol_ * residual2 / residual2_0) {
            break;
        }

        // JX(xi)
        auto gradE = cl_->evaluateGradientAt({xi});
        cl_->jacobian(elNo, gradE, tmp.J);
        auto J_0 = tmp.J.subtensor(slice{}, slice{}, 0);
        auto J = EigenMap(J_0);

        Eigen::Matrix<double, D, 1> h = J.fullPivLu().solve(F);
        for (std::size_t d = 0; d < D; ++d) {
            xi[d] += h(d);
        }

        ++it;
    }
    if (it == max_its_) {
        throw std::runtime_error("PointLocator: xInv did not converge.");
    }
    return xi;
}

template <std::size_t D>
BoundaryPointLocator<D>::BoundaryPointLocator(std::shared_ptr<PointLocator<D>> pl,
                                              LocalSimplexMesh<D> const& mesh,
                                              std::vector<std::size_t> const& fctNos)
    : pl_(std::move(pl)) {
    for (auto const& fctNo : fctNos) {
        auto elNos = mesh.template upward<D - 1u>(fctNo);
        for (auto elNo : elNos) {
            elNos_.emplace(elNo);
            auto dws = mesh.template downward<D - 1u, D>(elNo);
            int localFaceNo = std::distance(dws.begin(), std::find(dws.begin(), dws.end(), fctNo));
            assert(localFaceNo < D + 1u);
            bnds_.emplace(elNo, std::make_pair(fctNo, localFaceNo));
        }
    }

    Simplex<D> refPlex = Simplex<D>::referenceSimplex();
    auto refVertices = Simplex<D>::referenceSimplexVertices();
    auto f2v = refPlex.downward();
    assert(f2v.size() == closest_.size());

    for (std::size_t f = 0; f < f2v.size(); ++f) {
        std::array<std::array<double, D>, D> verts;
        for (std::size_t i = 0; i < D; ++i) {
            verts[i] = refVertices[f2v[f][i]];
        }
        closest_[f] = SimplexDistance(verts);
    }
}

template <std::size_t D>
auto BoundaryPointLocator<D>::locate(point_t const& point) const -> BoundaryPointLocatorResult<D> {
    auto r = BoundaryPointLocatorResult<D>{
        std::numeric_limits<std::size_t>::max(), std::numeric_limits<double>::max(), {}, {}};

    auto F = Managed(pl_->cl().mapResultInfo(1));

    auto [closest_elNo, el_dist, closest_xi, dummy] =
        pl_->locate(point, elNos_.begin(), elNos_.end());
    for (auto it = bnds_.find(closest_elNo); it != bnds_.end(); ++it) {
        auto fctNo = it->second.first;
        auto localFaceNo = it->second.second;
        auto [boundary_dist, xi, bary] = closest_[localFaceNo].closest(closest_xi);

        std::array<double, D - 1> chi;
        std::copy(bary.begin() + 1, bary.end(), chi.begin());
        auto E = pl_->cl().evaluateBasisAt({pl_->cl().facetParam(localFaceNo, chi)});
        pl_->cl().map(closest_elNo, E, F);
        point_t x{};
        for (std::size_t d = 0; d < D; ++d) {
            x[d] = F(d, 0);
        }

        double dist = norm(x - point);
        if (dist < r.dist) {
            r.dist = dist;
            r.no = fctNo;
            r.chi = chi;
            r.x = x;
        }
    }

    return r;
} // namespace tndm

template class PointLocator<1ul>;
template class PointLocator<2ul>;
template class PointLocator<3ul>;
template class BoundaryPointLocator<2ul>;
template class BoundaryPointLocator<3ul>;

} // namespace tndm


/**	\file TestArcIntersect.cpp
*	\brief
*/

/****************************************************************************/
/*	TestArcIntersect.cpp													*/
/****************************************************************************/
/*                                                                          */
/*  Copyright 2008 - 2010 Paul Kohut                                        */
/*  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.                          */
/*                                                                          */
/****************************************************************************/

//#include "stdafx.h"
#include <string>
#include <fstream>
#include "LatLongConversions.h"
#include "..\GeoFormulas\Conversions.h"
#include "..\GeoFormulas\GeoFormulas.h"

#include <boost/regex.hpp>

using namespace boost;
using namespace GeoCalcs;
using namespace std;

bool ParseTestArcIntersect(string sString)
{
	bool bPassed = true;
	TrimWhitespace(sString);
	string soTestId, soArc1CenterLat, soArc1CenterLon, soArc1Radius;
	string soArc2CenterLat, soArc2CenterLon, soArc2Radius;
	string soIntersection1Lat, soIntersection1Lon;
	string soIntersection2Lat, soIntersection2Lon;

	try
	{
		regex_constants::syntax_option_type flags =  regex_constants::icase | regex_constants::perl;

		string sRxPat = "([a-z]+|[A-Z]+\\d+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[.]?[0-9]+)[,]";
		sRxPat += "([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([-+]?[0-9]*[.]?[0-9]+)[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])[,]";
		sRxPat += "([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[NS])[,]([N/A]+|[0-9]*[:][0-9]*[:][0-9]*[.][0-9]*[WE])";

		regex pat(sRxPat, flags);

		int const sub_matches[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, };
		sregex_token_iterator it(sString.begin(), sString.end(), pat, sub_matches);
		if(it != sregex_token_iterator())
		{
			soTestId = *it++;
			soArc1CenterLat = *it++;
			soArc1CenterLon = *it++;
			soArc1Radius = *it++;
			soArc2CenterLat = *it++;
			soArc2CenterLon = *it++;
			soArc2Radius = *it++;
			soIntersection1Lat = *it++;
			soIntersection1Lon = *it++;
			soIntersection2Lat = *it++;
			soIntersection2Lon = *it++;

		}
	}
	catch(regex_error & e)
	{
		cout << "\n" << e.what();
		return false;
	}

	LLPoint arc1Center(Deg2Rad(ParseLatitude(soArc1CenterLat)), Deg2Rad(ParseLongitude(soArc1CenterLon)));
	LLPoint arc2Center(Deg2Rad(ParseLatitude(soArc2CenterLat)), Deg2Rad(ParseLongitude(soArc2CenterLon)));
	double arc1Radius = NmToMeters(atof(soArc1Radius.c_str()));
	double arc2Radius = NmToMeters(atof(soArc2Radius.c_str()));
	LLPoint intersection1;
	LLPoint intersection2;
	if(soIntersection1Lat != "N/A")
	{
		intersection1.Set(Deg2Rad(ParseLatitude(soIntersection1Lat)), Deg2Rad(ParseLongitude(soIntersection1Lon)));
		intersection2.Set(Deg2Rad(ParseLatitude(soIntersection2Lat)), Deg2Rad(ParseLongitude(soIntersection2Lon)));
	}

	LLPoint intPtC1, intPtC2;
	int nIndex = ArcIntercept(arc1Center, arc1Radius, arc2Center, arc2Radius, intPtC1, intPtC2, Tol());
	if(nIndex < 1)
	{
		if(soIntersection1Lat != "N/A")
			bPassed = false;

	} else {
		if(IsApprox(intPtC1.latitude, Deg2Rad(ParseLatitude(soIntersection2Lat)), Deg2Rad(1.0)) &&
			IsApprox(intPtC1.longitude, Deg2Rad(ParseLongitude(soIntersection2Lon)), Deg2Rad(1.0)) )
		{
			LLPoint pt = intPtC1;
			intPtC1 = intPtC2;
			intPtC2 = pt;
		}

		string sIntPt1Lat = ConvertLatitudeDdToDms(Rad2Deg(intPtC1.latitude));
		string sIntPt1Lon = ConvertLongitudeDdToDms(Rad2Deg(intPtC1.longitude));
		string sIntPt2Lat = ConvertLatitudeDdToDms(Rad2Deg(intPtC2.latitude));
		string sIntPt2Lon = ConvertLongitudeDdToDms(Rad2Deg(intPtC2.longitude));

		if(sIntPt1Lat.compare(soIntersection1Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(soIntersection1Lat));
//			if(IsApprox(dLat, intPtC1.latitude, 1e-10))
			if(IsNearZero(fabs(dLat) - fabs(intPtC1.latitude), 1e-10))
			{
				cout << "\n" << soTestId << " within rounding tolerance of 1e-10: Input Intersection 1 latitude: " << soIntersection1Lat << "  calced: " << sIntPt1Lat;
			} else {
				cout << "\n" << soTestId << " failed: Expected Intersection 1 latitude: " << soIntersection1Lat << "  calced: " << sIntPt1Lat;
				bPassed = false;
			}
		}

		if(sIntPt1Lon.compare(soIntersection1Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(soIntersection1Lon));
//			if(IsApprox(dLon, intPtC1.longitude, 1e-10))
			if(IsNearZero(fabs(dLon) - fabs(intPtC1.longitude), 1e-10))
			{
				cout << "\n" << soTestId << " within rounding tolerance of 1e-10: Input Intersection 1 longitude: " << soIntersection1Lon << "  calced: " << sIntPt1Lon;
			} else {
				cout << "\n" << soTestId << " failed: Expected Intersection 1 longitude: " << soIntersection1Lon << "  calced: " << sIntPt1Lon;
				bPassed = false;
			}
		}

		if(sIntPt2Lat.compare(soIntersection2Lat) != 0)
		{
			double dLat = Deg2Rad(ParseLatitude(soIntersection2Lat));
//			if(IsApprox(dLat, intPtC2.latitude, 1e-10))
			if(IsNearZero(fabs(dLat) - fabs(intPtC2.latitude), 1e-10))
			{
				cout << "\n" << soTestId << " within rounding tolerance of 1e-10: Input Intersection 2 latitude: " << soIntersection2Lat << "  calced: " << sIntPt2Lat;
			} else {
				cout << "\n" << soTestId << " failed: Expected Intersection 2 latitude: " << soIntersection2Lat << "  calced: " << sIntPt2Lat;
				bPassed = false;
			}
		}

		if(sIntPt2Lon.compare(soIntersection2Lon) != 0)
		{
			double dLon = Deg2Rad(ParseLongitude(soIntersection2Lon));
//			if(IsApprox(dLon, intPtC2.longitude, 1e-10))
			if(IsNearZero(fabs(dLon) - fabs(intPtC2.longitude), 1e-10))
			{
				cout << "\n" << soTestId << " within rounding tolerance of 1e-10: Input Intersection 2 longitude: " << soIntersection2Lon << "  calced: " << sIntPt2Lon;
			} else {
				cout << "\n" << soTestId << " failed: Expected Intersection 2 longitude: " << soIntersection2Lon << "  calced: " << sIntPt2Lon;
				bPassed = false;
			}
		}

	}

	return bPassed;
}

int TestArcIntersect(const string & sFilePath)
{
	ifstream infile;
	infile.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit);
	int nCount = 0;
	int nCommentCount = 0;
	bool bPassed = true;
	try
	{
		string sLine;
		infile.open(sFilePath.c_str(), ifstream::in);

		while(!infile.eof())
		{
			getline(infile, sLine);
			if(sLine.at(0) == '#')
			{
				nCommentCount++;
			}
			else
			{
				if(!ParseTestArcIntersect(sLine))
					bPassed = false;
				nCount++;
			}
		}
		infile.close();
		return bPassed;
	}

	catch(ifstream::failure e)
	{
		int nError = -99;
		// Per C++ standards for ifstream::failbit with global function getline
		// No characters were extracted because the end was prematurely found.Notice
		// that some eofbit cases will also set failbit.
		// In this case the end of the file is read and causes both flags to be raised,
		// so this presumably means all the data has been read correctly.
		if((infile.rdstate() & ifstream::failbit) && (infile.rdstate() & ifstream::eofbit) != 0)
			nError = bPassed;
		else if((infile.rdstate() & ifstream::failbit) != 0)
			nError = -1;
		else if((infile.rdstate() & ifstream::badbit) != 0)
			nError = -2;
		else if((infile.rdstate() & ifstream::eofbit) != 0)
			nError = -3;
		if(infile.is_open())
			infile.close();
		return nError;
	}
}


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

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

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

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

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/creditdefaultswap.hpp>
#include <ored/portfolio/creditdefaultswap.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/envelope.hpp>
#include <ored/portfolio/legdata.hpp>
#include <ored/portfolio/schedule.hpp>
#include <ored/utilities/indexparser.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/volatility/swaption/swaptionconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/time/daycounters/simpledaycounter.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace std;
using namespace ore::data;

namespace bdata = boost::unit_test::data;

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket(Real hazardRate, Real recoveryRate, Real liborRate) {
        asof_ = Date(3, Feb, 2016);
        // build discount
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")] =
            flatRateYts(liborRate);
        defaultCurves_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] = flatRateDcs(hazardRate);
        recoveryRates_[make_pair(Market::defaultConfiguration, "CreditCurve_A")] =
            Handle<Quote>(boost::make_shared<SimpleQuote>(recoveryRate));
        // build ibor index
        Handle<IborIndex> hEUR(ore::data::parseIborIndex(
            "EUR-EURIBOR-6M", yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "EUR")]));
        iborIndices_[make_pair(Market::defaultConfiguration, "EUR-EURIBOR-6M")] = hEUR;
    }

private:
    Handle<YieldTermStructure> flatRateYts(Real forward) {
        boost::shared_ptr<YieldTermStructure> yts(new FlatForward(0, NullCalendar(), forward, ActualActual()));
        yts->enableExtrapolation();
        return Handle<YieldTermStructure>(yts);
    }
    Handle<DefaultProbabilityTermStructure> flatRateDcs(Real forward) {
        boost::shared_ptr<DefaultProbabilityTermStructure> dcs(new FlatHazardRate(asof_, forward, SimpleDayCounter()));
        return Handle<DefaultProbabilityTermStructure>(dcs);
    }
};

struct CommonVars {
    // global data
    string ccy;
    string creditCurveId;
    string issuerId;
    bool isPayer;
    string start;
    string issue;
    string fixtenor;
    Calendar cal;
    string calStr;
    string conv;
    string rule;
    Size days;
    string fixDC;
    string settledays;
    bool isinarrears;
    Real notional;
    vector<Real> notionals;
    vector<Real> spread;

    // utilities
    boost::shared_ptr<ore::data::CreditDefaultSwap> makeCDS(string end, Real rate) {
        ScheduleData fixedSchedule(ScheduleRules(start, end, fixtenor, calStr, conv, conv, rule));

        // build CDS
        boost::shared_ptr<FixedLegData> fixedLegRateData = boost::make_shared<FixedLegData>(vector<double>(1, rate));
        LegData fixedLegData(fixedLegRateData, isPayer, ccy, fixedSchedule, fixDC, notionals);

        CreditDefaultSwapData cd(issuerId, creditCurveId, fixedLegData, false, true);
        Envelope env("CP1");

        boost::shared_ptr<ore::data::CreditDefaultSwap> cds(new ore::data::CreditDefaultSwap(env, cd));
        return cds;
    }

    CommonVars()
        : ccy("EUR"), creditCurveId("CreditCurve_A"), issuerId("CPTY_A"), isPayer(false), start("20160203"),
          issue("20160203"), fixtenor("1Y") {
        cal = TARGET();
        calStr = "TARGET";
        conv = "MF";
        rule = "Forward";
        fixDC = "ACT/365";
        settledays = "2";
        isinarrears = false;
        notional = 1;
        notionals.push_back(1);
        spread.push_back(0.0);
    }
};

struct MarketInputs {
    Real hazardRate;
    Real recoveryRate;
    Real liborRate;
};

// Needed for BOOST_DATA_TEST_CASE below as it writes out the MarketInputs
ostream& operator<<(ostream& os, const MarketInputs& m) {
    return os << "[" << m.hazardRate << "," << m.recoveryRate << "," << m.liborRate << "]";
}

struct TradeInputs {
    string endDate;
    Real fixedRate;
};

// Needed for BOOST_DATA_TEST_CASE below as it writes out the TradeInputs
ostream& operator<<(ostream& os, const TradeInputs& t) { return os << "[" << t.endDate << "," << t.fixedRate << "]"; }

Real cdsNpv(const MarketInputs& m, const TradeInputs& t) {

    // build market
    boost::shared_ptr<Market> market = boost::make_shared<TestMarket>(m.hazardRate, m.recoveryRate, m.liborRate);
    Settings::instance().evaluationDate() = market->asofDate();

    CommonVars vars;
    boost::shared_ptr<ore::data::CreditDefaultSwap> cds = vars.makeCDS(t.endDate, t.fixedRate);

    // Build and price
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("CreditDefaultSwap") = "DiscountedCashflows";
    engineData->engine("CreditDefaultSwap") = "MidPointCdsEngine";

    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);

    cds->build(engineFactory);

    return cds->instrument()->NPV();
}

// Cases below:
// 1) HazardRate = 0, couponRate = 0. ExpectedNpv = 0
// 2) RecoveryRate = 1, couponRate = 0. ExpectedNpv = 0
// 3) Example from Hull, Ch 21 (pp. 510 - 513). Take RR = 1 to show only couponNPV.
// 4) Example from Hull, Ch 21 (pp. 510 - 513). Take coupon rate = 0 to show only defaultNPV.

// Market inputs used in the test below
MarketInputs marketInputs[] = {{0, 1, 0}, {1, 1, 0}, {0.02, 1, 0.05}, {0.02, 0.4, 0.05}};

// Trade inputs used in the test below
TradeInputs tradeInputs[] = {{"20170203", 0}, {"20170203", 0}, {"20210203", 0.0124248849209095}, {"20210203", 0.0}};

// Expected NPVs given the market and trade inputs above
Real expNpvs[] = {0, 0, 0.050659, -0.05062};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CreditDefaultSwapTests)

BOOST_DATA_TEST_CASE(testCreditDefaultSwap, bdata::make(marketInputs) ^ bdata::make(tradeInputs) ^ bdata::make(expNpvs),
                     mkt, trd, exp) {

    BOOST_CHECK_CLOSE(cdsNpv(mkt, trd), exp, 0.01);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "test_PDNS_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/forcefield/PDNS/ProteinDNANonSpecificInteraction.hpp>
#include <mjolnir/core/VerletList.hpp>
#include <mjolnir/core/SpatialPartitionBase.hpp>
#include <mjolnir/core/NaivePairCalculation.hpp>

BOOST_AUTO_TEST_CASE(PDNS_Interaction)
{
    mjolnir::LoggerManager::set_default_logger("test_pdns_interaction.log");

    using traits_type       = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type         = traits_type::real_type;
    using coord_type        = traits_type::coordinate_type;
    using boundary_type     = traits_type::boundary_type;
    using system_type       = mjolnir::System<traits_type>;
    using topology_type     = mjolnir::Topology;

    using real_type              = double;
    using potential_type         = mjolnir::ProteinDNANonSpecificPotential<traits_type>;
    using contact_parameter_type = typename potential_type::contact_parameter_type;
    using dna_index_type         = typename potential_type::dna_index_type;
    using interaction_type       = mjolnir::ProteinDNANonSpecificInteraction<traits_type>;
    using ignore_molecule_type   = typename potential_type::ignore_molecule_type;
    using ignore_group_type      = typename potential_type::ignore_group_type;
    using partition_type         = mjolnir::NaivePairCalculation<traits_type, potential_type>;

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    //  0 o              |
    //     \             |
    //    1 o === o 3    |
    //     /       \     |
    //  2 o         o 4  |

    constexpr auto pi  = mjolnir::math::constants<real_type>::pi();

    const real_type r0     = 5.0;
    const real_type theta0 = pi * 0.5;
    const real_type phi0   = pi * 2.0 / 3.0;

    const real_type sigma  = 1.0;
    const real_type delta  = pi / 18.0;

    const contact_parameter_type p_pro{1, 0, 2, -1.2, r0, theta0, phi0, r0+5.0, (r0+5.0)*(r0+5.0)};
    const dna_index_type p_dna{3, 4};

    potential_type potential(
        sigma, delta, 5.0, {p_pro}, {p_dna}, {/*exclusion*/},
        ignore_molecule_type("Nothing"), ignore_group_type({}));

    interaction_type interaction(potential_type(potential),
        mjolnir::SpatialPartition<traits_type, potential_type>(
            mjolnir::make_unique<partition_type>()));

    system_type sys(5, boundary_type{});
    topology_type topol(5);

    topol.add_connection(0, 1, "bond");
    topol.add_connection(1, 2, "bond");
    topol.add_connection(3, 4, "bond");
    topol.construct_molecules();

    for(std::size_t i=0; i<sys.size(); ++i)
    {
        sys.mass(i)  = 1.0;
        sys.rmass(i) = 1.0;
        sys.position(i) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(i) = coord_type(0.0, 0.0, 0.0);
        sys.force(i)    = coord_type(0.0, 0.0, 0.0);
        sys.name(i)  = "DNA";
        sys.group(i) = "DNA";
    }
    potential  .initialize(sys, topol);
    interaction.initialize(sys, topol);

    for(const auto r     : {r0 - 0.2, r0 + 0.5})
    {
    for(const auto theta : {theta0 + delta * 0.2,
                            theta0 + delta * 0.7,
                            theta0 + delta * 1.2,
                            theta0 - delta * 1.2,
                            theta0 - delta * 0.7,
                            theta0 - delta * 0.2})
    {
    for(const auto phi   : {phi0 + delta * 0.2,
                            phi0 + delta * 0.7,
                            phi0 + delta * 1.2,
                            phi0 - delta * 1.2,
                            phi0 - delta * 0.7,
                            phi0 - delta * 0.2})
    {
        BOOST_TEST_MESSAGE("=======================================================");
        BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta << ", phi = " << phi);
        BOOST_TEST_MESSAGE("theta0 = " << theta0 << ", delta = " << delta);
        //  2 o              |
        //     \             |
        //  ^ 1 o === o 3    |
        //     /       \     |
        //  0 o         o 4  |

        sys.position(1) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
        sys.position(0) = mjolnir::math::make_coordinate<coord_type>(
                -std::cos(theta) - 1.0, -std::sin(theta), 0.0);
        sys.position(2) = mjolnir::math::make_coordinate<coord_type>(
                 std::cos(theta) - 1.0,  std::sin(theta), 0.0);

        sys.position(3) = mjolnir::math::make_coordinate<coord_type>(
                r, 0.0, 0.0);
        sys.position(4) = mjolnir::math::make_coordinate<coord_type>(
                r + std::cos(pi-phi), -std::sin(pi-phi), 0.0);

        // rotate in random direction
        {
            using matrix33_type = typename traits_type::matrix33_type;

            const auto rot_x = uni(mt) * pi;
            const auto rot_y = uni(mt) * pi;
            const auto rot_z = uni(mt) * pi;

            matrix33_type rotm_x(1.0,             0.0,              0.0,
                                 0.0, std::cos(rot_x), -std::sin(rot_x),
                                 0.0, std::sin(rot_x),  std::cos(rot_x));
            matrix33_type rotm_y( std::cos(rot_y), 0.0,  std::sin(rot_y),
                                              0.0, 1.0,              0.0,
                                 -std::sin(rot_y), 0.0,  std::cos(rot_y));
            matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                 std::sin(rot_z),  std::cos(rot_z), 0.0,
                                             0.0,              0.0, 1.0);

            const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
            for(std::size_t idx=0; idx<sys.size(); ++idx)
            {
                sys.position(idx) = rotm * sys.position(idx);
            }
        }
        // check configuration is okay
        {
            // XXX here it does not use boundary condition.
            const auto v13 = sys.position(3) - sys.position(1);
            BOOST_TEST_REQUIRE(mjolnir::math::length(v13) == r,
                               boost::test_tools::tolerance(1e-3));

            const auto v02 = sys.position(2) - sys.position(0);
            const auto dot_1302 = mjolnir::math::dot_product(v02, v13);
            const auto cos_1302 = mjolnir::math::clamp<real_type>(dot_1302 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v02), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_1302) == theta,
                               boost::test_tools::tolerance(1e-3));

            const auto v43 = sys.position(3) - sys.position(4);
            const auto dot_134 = mjolnir::math::dot_product(v13, v43);
            const auto cos_134 = mjolnir::math::clamp<real_type>(dot_134 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v43), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_134) == phi,
                               boost::test_tools::tolerance(1e-3));
        }

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            sys.position(idx) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.force(idx)     = coord_type(0.0, 0.0, 0.0);
        }
        const system_type init = sys;
        interaction.initialize(init, topol);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;
        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
                BOOST_TEST_MESSAGE("force.x = " << mjolnir::math::X(sys.force(idx)) <<
                                   ", -dE/dr = " << -dE/dr);
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
                BOOST_TEST_MESSAGE("force.y = " << mjolnir::math::Y(sys.force(idx)) <<
                                   ", -dE/dr = " << -dE/dr);
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE / dr == mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
                BOOST_TEST_MESSAGE("force.z = " << mjolnir::math::Z(sys.force(idx)) <<
                                   ", -dE/dr = " << -dE/dr);
            }
        }
    } // theta2
    } // theta1
    } // rbp
}

BOOST_AUTO_TEST_CASE(PDNS_Interaction_force_and_energy)
{
    mjolnir::LoggerManager::set_default_logger("test_pdns_interaction.log");

    using traits_type       = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type         = traits_type::real_type;
    using coord_type        = traits_type::coordinate_type;
    using boundary_type     = traits_type::boundary_type;
    using system_type       = mjolnir::System<traits_type>;
    using topology_type     = mjolnir::Topology;

    using real_type              = double;
    using potential_type         = mjolnir::ProteinDNANonSpecificPotential<traits_type>;
    using contact_parameter_type = typename potential_type::contact_parameter_type;
    using dna_index_type         = typename potential_type::dna_index_type;
    using interaction_type       = mjolnir::ProteinDNANonSpecificInteraction<traits_type>;
    using ignore_molecule_type   = typename potential_type::ignore_molecule_type;
    using ignore_group_type      = typename potential_type::ignore_group_type;
    using partition_type         = mjolnir::NaivePairCalculation<traits_type, potential_type>;

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    //  0 o              |
    //     \             |
    //    1 o === o 3    |
    //     /       \     |
    //  2 o         o 4  |

    constexpr auto pi  = mjolnir::math::constants<real_type>::pi();

    const real_type r0     = 5.0;
    const real_type theta0 = pi * 0.5;
    const real_type phi0   = pi * 2.0 / 3.0;

    const real_type sigma  = 1.0;
    const real_type delta  = pi / 18.0;

    const contact_parameter_type p_pro{1, 0, 2, -1.2, r0, theta0, phi0, r0+5.0, (r0+5.0)*(r0+5.0)};
    const dna_index_type p_dna{3, 4};

    potential_type potential(
        sigma, delta, 5.0, {p_pro}, {p_dna}, {/*exclusion*/},
        ignore_molecule_type("Nothing"), ignore_group_type({}));

    interaction_type interaction(potential_type(potential),
        mjolnir::SpatialPartition<traits_type, potential_type>(
            mjolnir::make_unique<partition_type>()));

    system_type sys(5, boundary_type{});
    topology_type topol(5);

    topol.add_connection(0, 1, "bond");
    topol.add_connection(1, 2, "bond");
    topol.add_connection(3, 4, "bond");
    topol.construct_molecules();

    for(std::size_t i=0; i<sys.size(); ++i)
    {
        sys.mass(i)  = 1.0;
        sys.rmass(i) = 1.0;
        sys.position(i) = coord_type(0.0, 0.0, 0.0);
        sys.velocity(i) = coord_type(0.0, 0.0, 0.0);
        sys.force(i)    = coord_type(0.0, 0.0, 0.0);
        sys.name(i)  = "DNA";
        sys.group(i) = "DNA";
    }
    potential  .initialize(sys, topol);
    interaction.initialize(sys, topol);

    for(const auto r     : {r0 - 0.2, r0 + 0.5})
    {
    for(const auto theta : {theta0 + delta * 0.2,
                            theta0 + delta * 0.7,
                            theta0 + delta * 1.2,
                            theta0 - delta * 1.2,
                            theta0 - delta * 0.7,
                            theta0 - delta * 0.2})
    {
    for(const auto phi   : {phi0 + delta * 0.2,
                            phi0 + delta * 0.7,
                            phi0 + delta * 1.2,
                            phi0 - delta * 1.2,
                            phi0 - delta * 0.7,
                            phi0 - delta * 0.2})
    {
        BOOST_TEST_MESSAGE("=======================================================");
        BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta << ", phi = " << phi);
        BOOST_TEST_MESSAGE("theta0 = " << theta0 << ", delta = " << delta);
        //  2 o              |
        //     \             |
        //  ^ 1 o === o 3    |
        //     /       \     |
        //  0 o         o 4  |

        sys.position(1) = mjolnir::math::make_coordinate<coord_type>(0.0, 0.0, 0.0);
        sys.position(0) = mjolnir::math::make_coordinate<coord_type>(
                -std::cos(theta) - 1.0, -std::sin(theta), 0.0);
        sys.position(2) = mjolnir::math::make_coordinate<coord_type>(
                 std::cos(theta) - 1.0,  std::sin(theta), 0.0);

        sys.position(3) = mjolnir::math::make_coordinate<coord_type>(
                r, 0.0, 0.0);
        sys.position(4) = mjolnir::math::make_coordinate<coord_type>(
                r + std::cos(pi-phi), -std::sin(pi-phi), 0.0);

        // rotate in random direction
        {
            using matrix33_type = typename traits_type::matrix33_type;

            const auto rot_x = uni(mt) * pi;
            const auto rot_y = uni(mt) * pi;
            const auto rot_z = uni(mt) * pi;

            matrix33_type rotm_x(1.0,             0.0,              0.0,
                                 0.0, std::cos(rot_x), -std::sin(rot_x),
                                 0.0, std::sin(rot_x),  std::cos(rot_x));
            matrix33_type rotm_y( std::cos(rot_y), 0.0,  std::sin(rot_y),
                                              0.0, 1.0,              0.0,
                                 -std::sin(rot_y), 0.0,  std::cos(rot_y));
            matrix33_type rotm_z(std::cos(rot_z), -std::sin(rot_z), 0.0,
                                 std::sin(rot_z),  std::cos(rot_z), 0.0,
                                             0.0,              0.0, 1.0);

            const matrix33_type rotm = rotm_x * rotm_y * rotm_z;
            for(std::size_t idx=0; idx<sys.size(); ++idx)
            {
                sys.position(idx) = rotm * sys.position(idx);
            }
        }
        // check configuration is okay
        {
            // XXX here it does not use boundary condition.
            const auto v13 = sys.position(3) - sys.position(1);
            BOOST_TEST_REQUIRE(mjolnir::math::length(v13) == r,
                               boost::test_tools::tolerance(1e-3));

            const auto v02 = sys.position(2) - sys.position(0);
            const auto dot_1302 = mjolnir::math::dot_product(v02, v13);
            const auto cos_1302 = mjolnir::math::clamp<real_type>(dot_1302 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v02), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_1302) == theta,
                               boost::test_tools::tolerance(1e-3));

            const auto v43 = sys.position(3) - sys.position(4);
            const auto dot_134 = mjolnir::math::dot_product(v13, v43);
            const auto cos_134 = mjolnir::math::clamp<real_type>(dot_134 *
                mjolnir::math::rlength(v13) * mjolnir::math::rlength(v43), -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos_134) == phi,
                               boost::test_tools::tolerance(1e-3));
        }

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            sys.position(idx) += coord_type(0.01 * uni(mt), 0.01 * uni(mt), 0.01 * uni(mt));
            sys.force(idx)     = coord_type(0.0, 0.0, 0.0);
        }
        system_type ref_sys = sys;
        interaction.initialize(sys, topol);

        constexpr real_type tol = 1e-4;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }

    } // theta2
    } // theta1
    } // rbp
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_LO_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_LO_HPP_INCLUDED

#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( lo_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::arithmetic_<A0> >
                          )
  {
    using result = bd::as_integer_t<A0,unsigned>;
    using s_t = bd::scalar_of_t<result>;

    BOOST_FORCEINLINE result operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      static const s_t pattern = (s_t(1) << sizeof(s_t)*(CHAR_BIT/2)) - 1;
      return bitwise_and( Ratio<result, pattern>() ,  a0 );
//      return bitwise_and( integral_constant<result, pattern>(), a0 );
    }
  };
} } }

#endif


#include <v4r/apps/compute_recognition_rate.h>
#include <v4r/common/pcl_opencv.h>
#include <v4r/io/filesystem.h>

#include <pcl/common/centroid.h>
#include <pcl/common/angles.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include <algorithm>    // std::next_permutation, std::sort
#include <glog/logging.h>

namespace po = boost::program_options;

namespace v4r
{
namespace apps
{

/**
 * @brief readHypothesesFromFile reads annotations from a text file
 * @param filename filename
 * @return stores hypotheses into Hypothesis class for each object model
 */
std::map<std::string, std::vector<Hypothesis> >
readHypothesesFromFile( const std::string &filename );
// ==========================================

bool
RecognitionEvaluator::computeError(const Eigen::Matrix4f &pose_a, const Eigen::Matrix4f &pose_b, const Eigen::Vector4f& centroid_model,
                  float &trans_error, float &rot_error, bool is_rotation_invariant, bool is_rotational_symmetric)
{
    const Eigen::Vector4f centroid_a = pose_a * centroid_model;
    const Eigen::Vector4f centroid_b = pose_b * centroid_model;

    const Eigen::Matrix3f rot_a = pose_a.block<3,3>(0,0);
    const Eigen::Matrix3f rot_b = pose_b.block<3,3>(0,0);

    const Eigen::Vector3f rotX_a = rot_a * Eigen::Vector3f::UnitX();
    const Eigen::Vector3f rotX_b = rot_b * Eigen::Vector3f::UnitX();
//    const Eigen::Vector3f rotY_a = rot_a * Eigen::Vector3f::UnitY();
//    const Eigen::Vector3f rotY_b = rot_b * Eigen::Vector3f::UnitY();
    const Eigen::Vector3f rotZ_a = rot_a * Eigen::Vector3f::UnitZ();
    const Eigen::Vector3f rotZ_b = rot_b * Eigen::Vector3f::UnitZ();

//    float angleX = pcl::rad2deg( acos( rotX_a.dot(rotX_b) ) );
//    float angleY = pcl::rad2deg( acos( rotY_a.dot(rotY_b) ) );
    float dotpz = rotZ_a.dot(rotZ_b);
    dotpz = std::min( 0.9999999f, std::max( -0.999999999f, dotpz) );
    float angleZ = pcl::rad2deg( acos( dotpz ) );

    float angleXY = 0.f;
    if( !is_rotation_invariant )
    {
        float dotpxy = rotX_a.dot(rotX_b );
        dotpxy = std::min( 0.9999999f, std::max( -0.999999999f, dotpxy) );
        angleXY = pcl::rad2deg ( acos ( dotpxy ) );

        if( is_rotational_symmetric )
            angleXY = std::min<float>(angleXY, fabs( 180.f - angleXY) );
    }

//    std::cout << " error_rotxy: " << angleXY << " error_rotx: " << angleX << " error_roty: " << angleY << " error_rotz: " << angleZ << std::endl;

    trans_error = (centroid_a.head(3)-centroid_b.head(3)).norm();
    rot_error = std::max<float>(angleXY, angleZ);

    VLOG(1) << "translation error: " << trans_error << ", rotational error: " << angleXY << "(xy), " << angleZ << "(z), is rotational invariant? " << is_rotation_invariant << ", is rotational symmetric? " << is_rotational_symmetric;

    if(trans_error > translation_error_threshold_m || rot_error > rotation_error_threshold_deg)
        return true;

    return false;
}

void
RecognitionEvaluator::checkMatchvector(const std::vector< std::pair<int, int> > &rec2gt,
                                       const std::vector<Hypothesis> &rec_hyps,
                                       const std::vector<Hypothesis> &gt_hyps,
                                       const Eigen::Vector4f &model_centroid,
                                       std::vector<float> &translation_error,
                                       std::vector<float> &rotational_error,
                                       size_t &tp, size_t &fp, size_t &fn,
                                       bool is_rotation_invariant,
                                       bool is_rotational_symmetric)
{
    translation_error = std::vector<float>(rec2gt.size(), -1000.f);
    rotational_error = std::vector<float>(rec2gt.size(), -1000.f);
    tp = fp = fn = 0;

    for(size_t i=0; i<rec2gt.size(); i++)
    {
        int rec_id = rec2gt[i].first;
        int gt_id = rec2gt[i].second;

        VLOG(1) << "Checking rec_id " << rec_id << " and gt_id " << gt_id;

        if(gt_id < 0)
        {
            VLOG(1) << "Adding false positve because there is not a single ground-truth object with the same instance name";
            fp++;
            continue;
        }

        const Hypothesis &gt_hyp = gt_hyps [ gt_id ];

        if( rec_id < 0 )
        {
            if( gt_hyp.occlusion < occlusion_threshold) // only count if the gt object is not occluded
            {
                fn++;
                VLOG(1) << "Adding false negative because there is not a single detected object with the same instance name";
            }

            continue;
        }

        const Hypothesis &rec_hyp = rec_hyps [ rec_id ] ;

        if( computeError( rec_hyp.pose, gt_hyp.pose, model_centroid, translation_error[i], rotational_error[i], is_rotation_invariant, is_rotational_symmetric))
        {
            fp++;

            if( gt_hyp.occlusion < occlusion_threshold)
            {
                fn++;
                VLOG(1) << "Adding false negative";
            }
            else
                VLOG(1) << "Adding false positve";
        }
        else
        {
            VLOG(1) << "Adding true positve";
            tp++;
        }
    }
}

std::vector<std::vector<int> >
PermGenerator(int n, int k)
{
    std::vector<std::vector<int> > possible_permutations;

    std::vector<int> d(n);
    std::iota(d.begin(),d.end(),0);
    do
    {
        std::vector<int> p (k);
        for (int i = 0; i < k; i++)
            p[i] = d[i];

        possible_permutations.push_back(p);
        std::reverse(d.begin()+k,d.end());
    } while (next_permutation(d.begin(),d.end()));

    return possible_permutations;
}

std::vector< std::pair<int, int> >
RecognitionEvaluator::selectBestMatch (const std::vector<Hypothesis> &rec_hyps,
                                       const std::vector<Hypothesis> &gt_hyps,
                                       const Eigen::Vector4f &model_centroid,
                                       size_t &tp, size_t &fp, size_t &fn,
                                       std::vector<float> &translation_errors,
                                       std::vector<float> &rotational_errors,
                                       bool is_rotation_invariant,
                                       bool is_rotational_symmetric)
{
    float best_fscore = -1;
    tp=0, fp=0, fn=0;

    std::vector< std::pair<int, int> > best_match;

    size_t k = std::min(rec_hyps.size(), gt_hyps.size());
    size_t n = std::max(rec_hyps.size(), gt_hyps.size());

    std::vector<std::vector<int> > perms = PermGenerator( n, k );

    for(const std::vector<int> &perm : perms)
    {
        std::vector< std::pair<int, int> > rec2gt_matches ( k );

        size_t tp_tmp=0, fp_tmp=0, fn_tmp=0;

        if( rec_hyps.size() < n)
        {
            boost::dynamic_bitset<> taken_gts(n,0);

            for( size_t i=0; i< k; i++)
            {
                std::pair<int, int> &p = rec2gt_matches[i];
                p.first = i;
                p.second = perm[i];
                taken_gts.set(perm[i]);
            }

            for(size_t i=0; i<n; i++)
            {
                if(!taken_gts[i] && gt_hyps [i].occlusion < occlusion_threshold)
                    fn_tmp++;
            }
        }
        else
        {
            boost::dynamic_bitset<> taken_recs(n,0);
            for( size_t i=0; i< k; i++)
            {
                std::pair<int, int> &p = rec2gt_matches[i];
                p.first = perm[i];
                p.second = i;
                taken_recs.set(perm[i]);
            }
            for(size_t i=0; i<n; i++)
            {
                if(!taken_recs[i])
                    fp_tmp++;
            }
        }

        std::vector<float> translation_errors_tmp(k);
        std::vector<float> rotational_errors_tmp(k);
        for( size_t i=0; i< k; i++)
        {
            const std::pair<int,int> &x = rec2gt_matches[i];
            VLOG(1) <<x.first<<"/"<<x.second<< " " << std::endl;

          const Hypothesis &rec_hyp = rec_hyps [ x.first ] ;
          const Hypothesis &gt_hyp = gt_hyps [ x.second ] ;

          if( computeError( rec_hyp.pose, gt_hyp.pose, model_centroid, translation_errors_tmp[i], rotational_errors_tmp[i], is_rotation_invariant, is_rotational_symmetric))
          {

              if( gt_hyp.occlusion < occlusion_threshold)
              {
                  fn_tmp++;
                  fp_tmp++;
                  VLOG(1) << "Adding false negative and false positive";
              }
              else
              {
                  if(translation_errors_tmp[i] > translation_error_threshold_m )    //ignore rotation erros for occluded objects
                  {
                      fp_tmp++;
                      VLOG(1) << "Adding false positve but ignoring false negative due to occlusion";
                  }
                  else
                      VLOG(1) << "Ignoring due to occlusion";
              }
          }
          else
          {
              VLOG(1) << "Adding true positve";
              tp_tmp++;
          }

        }

        float recall = 1.f;
        if (tp_tmp+fn_tmp) // if there are some ground-truth objects
            recall = (float)tp_tmp / (tp_tmp + fn_tmp);

        float precision = 1.f;
        if(tp_tmp+fp_tmp)   // if there are some recognized objects
            precision = (float)tp_tmp / (tp_tmp + fp_tmp);

        float fscore = 0.f;
        if ( precision+recall>std::numeric_limits<float>::epsilon() )
            fscore = 2.f * precision * recall / (precision + recall);

        if ( (fscore > best_fscore) ) // || (fscore==best_fscore && translation_errors_tmp/tp_tmp < translation_errors/tp))
        {
            best_fscore = fscore;
            translation_errors = translation_errors_tmp;
            rotational_errors = rotational_errors_tmp;
            tp = tp_tmp;
            fp = fp_tmp;
            fn = fn_tmp;
            best_match = rec2gt_matches;
        }

//        std::vector<float> translation_errors_tmp;
//        std::vector<float> rotational_errors_tmp;
//        size_t tp_tmp, fp_tmp, fn_tmp;
//        checkMatchvector(rec2gt_matches, rec_hyps, gt_hyps, model_centroid, translation_errors_tmp, rotational_errors_tmp,
//                         tp_tmp, fp_tmp, fn_tmp, is_rotation_invariant, is_rotational_symmetric);

//        float recall = 1.f;
//        if (tp_tmp+fn_tmp) // if there are some ground-truth objects
//            recall = (float)tp_tmp / (tp_tmp + fn_tmp);

//        float precision = 1.f;
//        if(tp_tmp+fp_tmp)   // if there are some recognized objects
//            precision = (float)tp_tmp / (tp_tmp + fp_tmp);

//        float fscore = 0.f;
//        if ( precision+recall>std::numeric_limits<float>::epsilon() )
//            fscore = 2.f * precision * recall / (precision + recall);

//        if ( (fscore > best_fscore) ) // || (fscore==best_fscore && translation_errors_tmp/tp_tmp < translation_errors/tp))
//        {
//            best_fscore = fscore;
//            translation_errors = translation_errors_tmp;
//            rotational_errors = rotational_errors_tmp;
//            tp = tp_tmp;
//            fp = fp_tmp;
//            fn = fn_tmp;
//            best_match = rec2gt_matches;
//        }
    }

    VLOG(1) << "BEST MATCH: ";
    for(auto &x:best_match)
    {
      VLOG(1)<<x.first<<"/"<<x.second<< " ";
    }
    VLOG(1) << std::endl;

    return best_match;
}

std::map<std::string, std::vector<Hypothesis> >
RecognitionEvaluator::readHypothesesFromFile( const std::string &filename )
{
    std::map<std::string, std::vector<Hypothesis> > hypotheses;

    std::ifstream anno_f ( filename.c_str() );
    std::string line;
    while (std::getline(anno_f, line))
    {
        std::istringstream iss(line);
        std::string model_name, occlusion_tmp;

        Hypothesis h;
        iss >> model_name >> occlusion_tmp;
        occlusion_tmp = occlusion_tmp.substr( 1, occlusion_tmp.length() - 3 );
        std::istringstream os(occlusion_tmp);
        float visible;
        os >> visible;
        h.occlusion = 1.f-visible;

        for(size_t i=0; i<16; i++)
            iss >> h.pose(i / 4, i % 4);

        auto pose_it = hypotheses.find( model_name );
        if( pose_it != hypotheses.end() )
            pose_it->second.push_back( h ) ;
        else
            hypotheses[model_name] = std::vector<Hypothesis>(1, h);
    }

    return hypotheses;
}

void
RecognitionEvaluator::visualizeResults(const typename pcl::PointCloud<PointT>::Ptr &input_cloud, const bf::path & gt_path, const bf::path &recognition_results_path)
{
    std::map<std::string, std::vector<Hypothesis> > gt_hyps = readHypothesesFromFile( gt_path.string() );
    std::map<std::string, std::vector<Hypothesis> > rec_hyps = readHypothesesFromFile( recognition_results_path.string() );

    static pcl::visualization::PCLVisualizer::Ptr vis;
    static int vp1, vp2, vp3;
    if(!vis)
    {
        vis.reset (new pcl::visualization::PCLVisualizer ("results"));
        vis->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2));
        vis->createViewPort(0, 0, 1, 0.33, vp1);
        vis->createViewPort(0, 0.33, 1, 0.66, vp2);
        vis->createViewPort(0, 0.66, 1, 1, vp3);
    }

    vis->removeAllPointClouds();
    vis->removeAllShapes();
#if PCL_VERSION >= 100800
    vis->removeAllCoordinateSystems();
#endif

    input_cloud->sensor_orientation_ = Eigen::Quaternionf::Identity();
    input_cloud->sensor_origin_ = Eigen::Vector4f::Zero(4);
    vis->addPointCloud(input_cloud, "scene", vp1);
    vis->addText("scene", 10, 10, 14, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "scene", vp1);
    vis->addText("ground-truth", 10, 10, 14, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "gt", vp2);
    vis->addText("recognition results", 10, 10, 14, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "rec", vp3);

    pcl::visualization::PointCloudColorHandlerCustom<PointT> gray (input_cloud, 255, 255, 255);
    vis->addPointCloud(input_cloud, gray, "input_vp2", vp2);
    vis->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "input_vp2");
    vis->addPointCloud(input_cloud, gray, "input_vp3", vp3);
    vis->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "input_vp3");

    for( const auto &m : models )
    {

        auto it = rec_hyps.find( m.first );
        if ( it != rec_hyps.end() )
        {
            typename pcl::PointCloud<PointT>::ConstPtr model_cloud = m.second.cloud;
            size_t counter = 0;
            for( const Hypothesis &hyp_vis : it->second )
            {
                typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                pcl::transformPointCloud(*model_cloud, *model_aligned, hyp_vis.pose);
                std::stringstream unique_id; unique_id << "rec_" << m.first << "_" << counter++;
                vis->addPointCloud(model_aligned, unique_id.str(), vp3);
            }
        }

        it = gt_hyps.find( m.first );
        if ( it != gt_hyps.end() )
        {
            typename pcl::PointCloud<PointT>::ConstPtr model_cloud = m.second.cloud;
            size_t counter = 0;
            for( const Hypothesis &hyp_vis : it->second )
            {
                typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                pcl::transformPointCloud(*model_cloud, *model_aligned, hyp_vis.pose);
                std::stringstream unique_id; unique_id << "gt_" << m.first << "_" << counter++;
                vis->addPointCloud(model_aligned, unique_id.str(), vp2);
            }
        }
    }

    vis->spin();

}

void
RecognitionEvaluator::compute_recognition_rate (size_t &total_tp, size_t &total_fp, size_t &total_fn)
{
    std::stringstream description;
    description << "Tool to compute object instance recognition rate." << std::endl <<
                   "==================================================" << std::endl <<
                   "This will generate a text file containing:" << std::endl <<
                   "Column 1: annotation file" << std::endl <<
                   "Column 2: true positives" << std::endl <<
                   "Column 3: false positives" << std::endl <<
                   "Column 4: false negatives" << std::endl <<
                   "Column 5: accumulated translation error of all true positive objects" << std::endl <<
                   "Column 6: accumulated rotational error of all true positive objects" << std::endl <<
                   "Column 7: elapsed time in ms" << std::endl <<
                   "==================================================" << std::endl <<
                   "** Allowed options";

    bf::path out_path = out_dir;
    out_path /= "recognition_results.txt";
    v4r::io::createDirForFileIfNotExist(out_path.string());
    std::ofstream of ( out_path.string().c_str() );

    std::vector<std::string> annotation_files = v4r::io::getFilesInDirectory( gt_dir, ".*.anno", true );

    total_tp = 0;
    total_fp = 0;
    total_fn = 0;

    for( const std::string anno_file : annotation_files )
    {
        bf::path gt_path = gt_dir;
        gt_path /= anno_file;

        std::string rec_file = anno_file;
        if(use_generated_hypotheses_)
            boost::replace_last( rec_file, ".anno", ".generated_hyps");

        bf::path rec_path = or_dir;
        rec_path /= rec_file;

        if(!v4r::io::existsFile(rec_path.string()))
            continue;

        std::map<std::string, std::vector<Hypothesis> > gt_hyps = readHypothesesFromFile( gt_path.string() );
        std::map<std::string, std::vector<Hypothesis> > rec_hyps = readHypothesesFromFile( rec_path.string() );

        size_t tp_view = 0;
        size_t fp_view = 0;
        size_t fn_view = 0;
        double sum_translation_error_view = 0.;
        double sum_rotational_error_view = 0.;

        if(vis_)
        {
            vis_->removeAllPointClouds();
            vis_->removeAllShapes();
#if PCL_VERSION >= 100800
            vis_->removeAllCoordinateSystems();
#endif
        }

        pcl::PointCloud<PointT>::Ptr all_hypotheses;
        pcl::PointCloud<PointT>::Ptr all_groundtruth_objects;

        if (save_images_to_disk_)
        {
            all_hypotheses.reset (new pcl::PointCloud<PointT>);
            all_groundtruth_objects.reset(new pcl::PointCloud<PointT>);
        }

        for( const auto &m : models )
        {
            std::vector<Hypothesis> rec_hyps_tmp, gt_hyps_tmp;

            auto it = rec_hyps.find( m.first );
            if ( it != rec_hyps.end() )
                rec_hyps_tmp = it->second;

            it = gt_hyps.find( m.first );
            if ( it != gt_hyps.end() )
                gt_hyps_tmp = it->second;

            size_t tp_tmp=0, fp_tmp=0, fn_tmp=0;
            std::vector<float> translation_errors_tmp;
            std::vector<float> rotational_errors_tmp;
            std::vector< std::pair<int, int> > matches;

            if( gt_hyps_tmp.empty() && rec_hyps_tmp.empty() )
                continue;
//            else if( gt_hyps_tmp.empty() )
//            {
//                fp_tmp = rec_hyps_tmp.size();
//                matches = std::vector<std::pair<int,int> > (rec_hyps_tmp.size());
//                for(size_t r_id=0; r_id<rec_hyps_tmp.size(); r_id++)
//                    matches[r_id] = std::pair<int,int>(r_id, -1);
//            }
//            else if( rec_hyps_tmp.empty() )
//            {
//                for(const Hypothesis &gt_hyp : gt_hyps_tmp )
//                {
//                    if( gt_hyp.occlusion < occlusion_threshold) // only count if the gt object is not occluded
//                        fn_tmp++;

//                    matches = std::vector<std::pair<int,int> > (gt_hyps_tmp.size());
//                    for(size_t gt_id=0; gt_id<gt_hyps_tmp.size(); gt_id++)
//                        matches[gt_id] = std::pair<int,int>(-1, gt_id);
//                }
//            }
//            else
            {
                const Eigen::Vector4f &centroid = m.second.centroid;
                matches = selectBestMatch(rec_hyps_tmp, gt_hyps_tmp, centroid, tp_tmp, fp_tmp, fn_tmp,
                                          translation_errors_tmp, rotational_errors_tmp,
                                          m.second.is_rotation_invariant_, m.second.is_rotational_symmetric_);
            }

            tp_view+=tp_tmp;
            fp_view+=fp_tmp;
            fn_view+=fn_tmp;

            float sum_translation_error_tmp = 0.f;
            float sum_rotational_error_tmp = 0.f;

            for(size_t t_id=0; t_id<translation_errors_tmp.size(); t_id++)
            {
                if( translation_errors_tmp[t_id]>0.f )
                {
                    sum_translation_error_tmp += translation_errors_tmp[t_id];
                    sum_rotational_error_tmp += rotational_errors_tmp[t_id];
                }
            }
            sum_translation_error_view += sum_translation_error_tmp;
            sum_rotational_error_view += sum_rotational_error_tmp;

            if(visualize_)
            {
                if(!vis_)
                {
                    vis_.reset (new pcl::visualization::PCLVisualizer ("results"));
                    vis_->createViewPort(0, 0, 1, 0.33, vp1_);
                    vis_->createViewPort(0, 0.33, 1, 0.66, vp2_);
                    vis_->createViewPort(0, 0.66, 1, 1, vp3_);
                    vis_->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2));
                    vis_->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2), vp1_);
                    vis_->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2), vp2_);
                    vis_->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2), vp3_);
                }

                size_t counter = 0;
                for( const Hypothesis &rec_hyp : rec_hyps_tmp )
                {
                    typename pcl::PointCloud<PointT>::ConstPtr model_cloud = m.second.cloud;
                    typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                    pcl::transformPointCloud(*model_cloud, *model_aligned, rec_hyp.pose);

                    std::stringstream unique_id; unique_id << m.first << "_" << counter++;
#if PCL_VERSION >= 100800
                        Eigen::Matrix4f tf_tmp = rec_hyp.pose;
                        Eigen::Matrix3f rot_tmp  = tf_tmp.block<3,3>(0,0);
                        Eigen::Vector3f trans_tmp = tf_tmp.block<3,1>(0,3);
                        Eigen::Affine3f affine_trans;
                        affine_trans.fromPositionOrientationScale(trans_tmp, rot_tmp, Eigen::Vector3f::Ones());
                        std::stringstream co_id; co_id << m.first << "_co_" << counter;
                        vis_->addCoordinateSystem(0.1f, affine_trans, co_id.str(), vp3_);
#endif
                    vis_->addPointCloud(model_aligned, unique_id.str(), vp3_);
                }

                for( const Hypothesis &gt_hyp : gt_hyps_tmp )
                {
                    typename pcl::PointCloud<PointT>::ConstPtr model_cloud = m.second.cloud;
                    typename pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                    pcl::transformPointCloud(*model_cloud, *model_aligned, gt_hyp.pose);

                    std::stringstream unique_id; unique_id << m.first << "_" << counter++;
#if PCL_VERSION >= 100800
                        Eigen::Matrix4f tf_tmp = gt_hyp.pose;
                        Eigen::Matrix3f rot_tmp  = tf_tmp.block<3,3>(0,0);
                        Eigen::Vector3f trans_tmp = tf_tmp.block<3,1>(0,3);
                        Eigen::Affine3f affine_trans;
                        affine_trans.fromPositionOrientationScale(trans_tmp, rot_tmp, Eigen::Vector3f::Ones());
                        std::stringstream co_id; co_id << m.first << "_co_" << counter;
                        vis_->addCoordinateSystem(0.1f, affine_trans, co_id.str(), vp3_);
#endif
                    vis_->addPointCloud(model_aligned, unique_id.str(), vp2_);
                }
            }
        }

        if(save_images_to_disk_)
        {
            if( visualize_errors_only_ && fp_view == 0)
                continue;

            const std::string img_output_dir = "/tmp/recognition_output_images/";
            v4r::Camera::Ptr kinect (new v4r::Camera(525.f, 525.f, 640, 480, 319.5, 239.5));

            std::string scene_name (anno_file);
            boost::replace_last( scene_name, ".anno", ".pcd");
            bf::path scene_path = test_dir;
            scene_path /= scene_name;
            pcl::PointCloud<PointT>::Ptr scene_cloud (new pcl::PointCloud<PointT>);
            pcl::io::loadPCDFile( scene_path.string(), *scene_cloud);
            boost::replace_last( scene_name, ".pcd", "");

            v4r::PCLOpenCVConverter<PointT> ocv;
            ocv.setCamera(kinect);
            ocv.setInputCloud( all_hypotheses );
            ocv.setBackgroundColor(255, 255, 255);
            ocv.setRemoveBackground(false);
            cv::Mat all_hypotheses_img = ocv.getRGBImage();
            bf::path img_path = img_output_dir; img_path /= scene_name; img_path /= "all_hypotheses.jpg";
            v4r::io::createDirForFileIfNotExist(img_path.string());
            cv::imwrite( img_path.string(), all_hypotheses_img);
            ocv.setInputCloud( all_groundtruth_objects );
            cv::Mat all_groundtruth_objects_img = ocv.getRGBImage();
            cv::imwrite( img_output_dir + "/" + scene_name + "/all_groundtruth_objects.jpg", all_groundtruth_objects_img);

            ocv.setInputCloud( scene_cloud );
            cv::Mat scene_cloud_img = ocv.getRGBImage();
            cv::imwrite( img_output_dir + "/" + scene_name + "/scene.jpg", scene_cloud_img);
        }

        // get time measurements
        size_t time_view = 0;
        {
            std::map<std::string, size_t> time_measurements;
            std::string time_file = anno_file;
            boost::replace_last(time_file, ".anno", ".times");
            bf::path time_path = or_dir;
            time_path /= time_file;

            std::ifstream time_f ( time_path.string() );
            std::string line;
            while (std::getline(time_f, line))
            {
                size_t elapsed_time;
                std::istringstream iss(line);
                iss >> elapsed_time;
                std::stringstream elapsed_time_ss; elapsed_time_ss << elapsed_time;

                const std::string time_description = line.substr( elapsed_time_ss.str().length() + 1 );
                time_measurements[time_description] = elapsed_time;
            }
            time_f.close();

            for(const auto &t_map:time_measurements)
            {
                VLOG(1) << t_map.first << ": " << t_map.second;
                if ( (t_map.first == "Computing normals" ) ||
                     (t_map.first == "Removing planes" ) ||
                     (t_map.first == "Generation of object hypotheses" ) ||
                     (t_map.first == "Computing noise model" ) ||
                     (t_map.first == "Noise model based cloud integration" ) ||
                     (t_map.first == "Verification of object hypotheses" ) )
                {
                    VLOG(1) << "count!";
                    time_view += t_map.second;
                }
            }
        }

        std::cout << anno_file << ": " << tp_view << " " << fp_view << " " << fn_view << " " << sum_translation_error_view << " " << sum_rotational_error_view << " " << time_view <<std::endl;
        of << anno_file << " " << tp_view << " " << fp_view << " " << fn_view << " " << sum_translation_error_view << " " << sum_rotational_error_view << " " << time_view << std::endl;

        total_tp += tp_view;
        total_fp += fp_view;
        total_fn += fn_view;

        if(visualize_)
        {
            if( visualize_errors_only_ && fp_view == 0)
                continue;

            std::string scene_name (anno_file);
            boost::replace_last( scene_name, ".anno", ".pcd");
            bf::path scene_path = test_dir;
            scene_path /= scene_name;
            pcl::PointCloud<PointT>::Ptr scene_cloud (new pcl::PointCloud<PointT>);
            pcl::io::loadPCDFile( scene_path.string(), *scene_cloud);
            //reset view point - otherwise this messes up PCL's visualization (this does not affect recognition results)
            scene_cloud->sensor_orientation_ = Eigen::Quaternionf::Identity();
            scene_cloud->sensor_origin_ = Eigen::Vector4f::Zero(4);
            vis_->addPointCloud(scene_cloud, "scene", vp1_);

//            pcl::visualization::PointCloudColorHandlerCustom<PointT> gray (scene_cloud, 255, 255, 255);
//            vis_->addPointCloud(scene_cloud, gray, "input_vp2", vp2_);
//            vis_->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "input_vp2");
//            vis_->addPointCloud(scene_cloud, gray, "input_vp3", vp3_);
//            vis_->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "input_vp3");

            vis_->addText( scene_name, 10, 10, 15, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "scene_text", vp1_);
            vis_->addText("ground-truth objects (occluded objects in blue, false ones in red, pose errors green)", 10, 10, 15, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "gt_text", vp2_);
            std::stringstream rec_text;
            rec_text << "recognized objects (tp: " << tp_view << ", fp: " << fp_view << ", fn: " << fn_view;
            if(tp_view)
            {
                rec_text << " trans_error: " << sum_translation_error_view/tp_view;
                rec_text << " rot_error: " << sum_rotational_error_view/tp_view;
            }
            rec_text << ")";
            vis_->addText(rec_text.str(), 10, 10, 15, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "rec_text", vp3_);
//            vis->resetCamera();
            vis_->spin();
        }
    }
    of.close();
}

std::string RecognitionEvaluator::getModels_dir() const
{
    return models_dir;
}

void RecognitionEvaluator::setModels_dir(const std::string &value)
{
    models_dir = value;
    loadModels();
}

std::string RecognitionEvaluator::getTest_dir() const
{
    return test_dir;
}

void RecognitionEvaluator::setTest_dir(const std::string &value)
{
    test_dir = value;
}

std::string RecognitionEvaluator::getOr_dir() const
{
    return or_dir;
}

void RecognitionEvaluator::setOr_dir(const std::string &value)
{
    or_dir = value;
}

std::string RecognitionEvaluator::getGt_dir() const
{
    return gt_dir;
}

void RecognitionEvaluator::setGt_dir(const std::string &value)
{
    gt_dir = value;
}

bool RecognitionEvaluator::getUse_generated_hypotheses() const
{
    return use_generated_hypotheses_;
}

void RecognitionEvaluator::setUse_generated_hypotheses(bool value)
{
    use_generated_hypotheses_ = value;
}

bool RecognitionEvaluator::getVisualize() const
{
    return visualize_;
}

void RecognitionEvaluator::setVisualize(bool value)
{
    visualize_ = value;
}

std::string RecognitionEvaluator::getOut_dir() const
{
    return out_dir;
}

void RecognitionEvaluator::setOut_dir(const std::string &value)
{
    out_dir = value;
}

void RecognitionEvaluator::loadModels()
{
    std::vector<std::string> model_filenames = v4r::io::getFilesInDirectory( models_dir, "3D_model.pcd", true );
    for(const std::string &model_fn : model_filenames)
    {
        pcl::PointCloud<PointT>::Ptr model_cloud (new pcl::PointCloud<PointT>);
        bf::path model_full_path = models_dir;
        model_full_path /= model_fn;
        pcl::io::loadPCDFile( model_full_path.string(), *model_cloud );

        bf::path model_path = model_fn;
        const std::string model_name = model_path.parent_path().string();
        Model m;
        m.cloud = model_cloud;
        m.is_rotational_symmetric_ = std::find(rotational_symmetric_objects_.begin(), rotational_symmetric_objects_.end(), model_name) != rotational_symmetric_objects_.end();
        m.is_rotation_invariant_ = std::find(rotational_invariant_objects_.begin(), rotational_invariant_objects_.end(), model_name) != rotational_invariant_objects_.end();
        pcl::compute3DCentroid(*m.cloud, m.centroid);

        // model identity is equal folder name -> remove \"/3D_model.pcd\" from filename
        models[ model_name ] = m;
    }
}

std::vector<std::string>
RecognitionEvaluator::init(const std::vector<std::string> &params)
{
    int verbosity = 0;

    po::options_description desc("Evaluation of object recognition\n==========================================\nAllowed options:\n");
    desc.add_options()
            ("help,h", "produce help message")
            ("groundtruth_dir,g", po::value<std::string>(&gt_dir), "Root directory containing annotation files (i.e. 4x4 ground-truth pose of each object with filename viewId_ModelId_ModelInstanceCounter.txt")
            ("rec_results_dir,r", po::value<std::string>(&or_dir), "Root directory containing the recognition results (same format as annotation files).")
            ("out_dir,o", po::value<std::string>(&out_dir)->default_value(out_dir), "Output directory where recognition results will be stored")
            ("trans_thresh", po::value<float>(&translation_error_threshold_m)->default_value(translation_error_threshold_m), "Maximal allowed translational error in metres")
            ("rot_thresh", po::value<float>(&rotation_error_threshold_deg)->default_value(rotation_error_threshold_deg), "Maximal allowed rotational error in degrees")
            ("occlusion_thresh", po::value<float>(&occlusion_threshold)->default_value(occlusion_threshold), "Occlusion threshold. Object with higher occlusion will be ignored in the evaluation")
            ("visualize,v", po::bool_switch(&visualize_), "visualize recognition results")
            ("visualize_errors_only", po::bool_switch(&visualize_errors_only_), "visualize only if there are errors (visualization must be on)")
            ("save_images_to_disk", po::bool_switch(&save_images_to_disk_), "if true, saves images to disk (visualization must be on)")
            ("highlight_errors", po::bool_switch(&highlight_errors_), "if true, highlights errors in the visualization")
            ("verbosity", po::value<int>(&verbosity)->default_value(verbosity), "verbosity level")
            ("models_dir,m", po::value<std::string>(&models_dir), "Only for visualization. Root directory containing the model files (i.e. filenames 3D_model.pcd).")
            ("test_dir,t", po::value<std::string>(&test_dir), "Only for visualization. Root directory containing the scene files.")
            ("use_generated_hypotheses", po::bool_switch(&use_generated_hypotheses_), "if true, computes recognition rate for all generated hypotheses instead of verified ones.")
            ;
    po::variables_map vm;
    po::parsed_options parsed = po::command_line_parser(params).options(desc).allow_unregistered().run();
    std::vector<std::string> unused_params = po::collect_unrecognized(parsed.options, po::include_positional);
    po::store(parsed, vm);
    if (vm.count("help")) { std::cout << desc << std::endl;}
    try  {  po::notify(vm); }
    catch( std::exception& e)  { std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl; }

    if(verbosity>=0)
    {
        FLAGS_logtostderr = 1;
        FLAGS_v = verbosity;
        std::cout << "Enabling verbose logging." << std::endl;
    }

    loadModels();

    return unused_params;
}

float
RecognitionEvaluator::compute_recognition_rate_over_occlusion()
{
    std::stringstream description;
    description << "Tool to compute object instance recognition rate." << std::endl <<
                   "==================================================" << std::endl <<
                   "This will generate a text file containing:" << std::endl <<
                   "Column 1: occlusion" << std::endl <<
                   "Column 2: is recognized" << std::endl <<
                   "==================================================" << std::endl <<
                   "** Allowed options";

    bf::path out_path = out_dir;
    out_path /= "results_occlusion.txt";

    v4r::io::createDirForFileIfNotExist( out_path.string() );
    std::ofstream f( out_path.string() );
    std::cout << "Writing results to " << out_path.string() << "..." << std::endl;

    std::vector<std::string> annotation_files = v4r::io::getFilesInDirectory( gt_dir, ".*.anno", true );

    size_t num_recognized = 0;
    size_t num_total = 0;
    for( const std::string anno_file : annotation_files )
    {
        bf::path gt_path = gt_dir;
        gt_path /= anno_file;

        std::string rec_file = anno_file;
        if( use_generated_hypotheses_ )
            boost::replace_last( rec_file, ".anno", ".generated_hyps");

        bf::path rec_path = or_dir;
        rec_path /= rec_file;

        if(!v4r::io::existsFile(rec_path.string()))
        {
            LOG(INFO) << "File " << rec_path.string() << " not found.";
            continue;
        }

        std::map<std::string, std::vector<Hypothesis> > gt_hyps = readHypothesesFromFile( gt_path.string() );
        std::map<std::string, std::vector<Hypothesis> > rec_hyps = readHypothesesFromFile( rec_path.string() );

        for(auto const &gt_model_hyps : gt_hyps)
        {
            const std::string &model_name_gt = gt_model_hyps.first;
            const Model &m = models[ model_name_gt ];
            const std::vector<Hypothesis> &hyps = gt_model_hyps.second;

            for(const Hypothesis &h_gt : hyps)
            {
                bool is_recognized = false;

                const Eigen::Matrix4f &gt_pose = h_gt.pose;

                float occlusion = h_gt.occlusion;

                const auto it = rec_hyps.find( model_name_gt );
                if (it != rec_hyps.end())
                {
                    const std::vector<Hypothesis> &rec_model_hyps = it->second;
                    for(const Hypothesis &h_rec: rec_model_hyps)
                    {
                        const Eigen::Matrix4f &rec_pose = h_rec.pose;
                        float trans_error, rot_error;
                        if(! computeError( gt_pose, rec_pose, m.centroid, trans_error, rot_error, m.is_rotation_invariant_, m.is_rotational_symmetric_ ) )
                            is_recognized = true;
                    }
                }
                num_total++;

                if(is_recognized)
                    num_recognized++;

                f << occlusion << " " << is_recognized << std::endl;
            }
        }
    }
    f.close();
    std::cout << "Done!" << std::endl;

    return (float)num_recognized/num_total;
}

void
RecognitionEvaluator::checkIndividualHypotheses()
{

    std::vector<std::string> annotation_files = io::getFilesInDirectory( gt_dir, ".*.anno", true );

    pcl::visualization::PCLVisualizer::Ptr vis;
    int vp1, vp2;
    if(visualize_)
    {
        vis.reset (new pcl::visualization::PCLVisualizer ("results"));
        vis->setBackgroundColor(vis_params_->bg_color_(0), vis_params_->bg_color_(1), vis_params_->bg_color_(2));
        vis->createViewPort(0, 0, 1, 0.5, vp1);
        vis->createViewPort(0, 0.5, 1, 1, vp2);
    }

    for( const std::string anno_file : annotation_files )
    {
        bf::path gt_path = gt_dir;
        gt_path /= anno_file;

        std::string rec_file = anno_file;
        if(use_generated_hypotheses_)
            boost::replace_last( rec_file, ".anno", ".generated_hyps");

        bf::path rec_path = or_dir;
        rec_path /= rec_file;

        std::map<std::string, std::vector<Hypothesis> > gt_hyps = readHypothesesFromFile( gt_path.string() );
        std::map<std::string, std::vector<Hypothesis> > rec_hyps = readHypothesesFromFile( rec_path.string() );

        if(visualize_)
        {
            vis->removeAllPointClouds();
            vis->removeAllShapes(vp1);
#if PCL_VERSION >= 100800
            vis->removeAllCoordinateSystems(vp1);
#endif
            std::string scene_name (anno_file);
            boost::replace_last( scene_name, ".anno", ".pcd");

            bf::path scene_path = test_dir;
            scene_path /= scene_name;
            pcl::PointCloud<PointT>::Ptr scene_cloud (new pcl::PointCloud<PointT>);
            pcl::io::loadPCDFile( scene_path.string(), *scene_cloud);

            //reset view point - otherwise this messes up PCL's visualization (this does not affect recognition results)
            scene_cloud->sensor_orientation_ = Eigen::Quaternionf::Identity();
            scene_cloud->sensor_origin_ = Eigen::Vector4f::Zero(4);

            vis->addPointCloud(scene_cloud, "scene", vp1);

            pcl::visualization::PointCloudColorHandlerCustom<PointT> gray (scene_cloud, 255, 255, 255);
            vis->addPointCloud(scene_cloud, gray, "input_vp2", vp2);
            vis->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "input_vp2");
            vis->addText( scene_name, 10, 10, 15, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "scene_text", vp1);
        }

        for( const auto &m : models )
        {
            std::vector<Hypothesis> rec_hyps_tmp, gt_hyps_tmp;

            auto it = rec_hyps.find( m.first );
            if ( it != rec_hyps.end() )
                rec_hyps_tmp = it->second;

            it = gt_hyps.find( m.first );
            if ( it != gt_hyps.end() )
                gt_hyps_tmp = it->second;

            for( const Hypothesis &h : rec_hyps_tmp )
            {
                bool is_correct = false;
                float translation_error = std::numeric_limits<double>::max();
                float rotational_error = std::numeric_limits<double>::max();
                int best_matching_gt_id = -1;

                if( !gt_hyps_tmp.empty() )
                {
                    best_matching_gt_id = -1;
                    translation_error = std::numeric_limits<float>::max();
                    rotational_error = std::numeric_limits<float>::max();

                    for(size_t gt_id = 0; gt_id<gt_hyps_tmp.size(); gt_id++)
                    {
                        const Hypothesis &gt_hyp = gt_hyps_tmp[gt_id];
                        float trans_error_tmp, rot_error_tmp;
                        if( !computeError( h.pose, gt_hyp.pose, m.second.centroid, trans_error_tmp, rot_error_tmp, m.second.is_rotation_invariant_, m.second.is_rotational_symmetric_))
                            is_correct = true;

                        if(trans_error_tmp < translation_error)
                        {
                            translation_error = trans_error_tmp;
                            rotational_error = rot_error_tmp;
                            best_matching_gt_id = gt_id;
                        }
                    }
                }

                if(visualize_)
                {
                    vis->removePointCloud("model_cloud", vp2);
                    vis->removeAllShapes(vp2);
        #if PCL_VERSION >= 100800
                    vis->removeAllCoordinateSystems(vp2);
        #endif
                    pcl::PointCloud<PointT>::Ptr model_cloud = m.second.cloud;
                    pcl::PointCloud<PointT>::Ptr model_aligned(new pcl::PointCloud<PointT>());
                    pcl::transformPointCloud(*model_cloud, *model_aligned, h.pose);

                    vis->addPointCloud(model_aligned, "model_cloud", vp2);

                    if(is_correct)
                        vis->setBackgroundColor( 0, 255, 0, vp2);
                    else
                        vis->setBackgroundColor( 255, 0, 0, vp2);

#if PCL_VERSION >= 100800
                    Eigen::Matrix4f tf_tmp = h.pose;
                    Eigen::Matrix3f rot_tmp  = tf_tmp.block<3,3>(0,0);
                    Eigen::Vector3f trans_tmp = tf_tmp.block<3,1>(0,3);
                    Eigen::Affine3f affine_trans;
                    affine_trans.fromPositionOrientationScale(trans_tmp, rot_tmp, Eigen::Vector3f::Ones());
                    vis->addCoordinateSystem(0.1f, affine_trans, "model_co", vp2);
#endif
                    if(best_matching_gt_id>=0)
                    {
                        const Hypothesis &gt_hyp = gt_hyps_tmp[best_matching_gt_id];
                        pcl::PointXYZ center_rec, center_gt;
                        center_rec.getVector4fMap() = h.pose * m.second.centroid;
                        vis->addSphere(center_rec, 0.01, 0,0,255, "center_rec", vp2);
                        center_gt.getVector4fMap() = gt_hyp.pose * m.second.centroid;
                        vis->addSphere(center_gt, 0.01, 125,125,255, "center_gt", vp2);
                        vis->addLine(center_rec, center_gt, 0, 0, 255, "distance", vp2);
                        std::stringstream model_txt;
                        model_txt.precision(2);
                        model_txt << "Transl. error: " << translation_error*100.f << "cm; rotational error: " <<  rotational_error << "deg; occlusion: " << gt_hyp.occlusion;
                        vis->addText( model_txt.str(), 10, 10, 15, vis_params_->text_color_(0), vis_params_->text_color_(1), vis_params_->text_color_(2), "model_text", vp2);
                    }
//                    vis->resetCamera();
                    vis->spin();
                }
            }
        }
    }
}

Eigen::MatrixXi
RecognitionEvaluator::compute_confusion_matrix()
{
//    std::stringstream description;
//    description << "Tool to compute object instance recognition rate." << std::endl <<
//                   "==================================================" << std::endl <<
//                   "This will generate a text file containing:" << std::endl <<
//                   "Column 1: occlusion" << std::endl <<
//                   "Column 2: is recognized" << std::endl <<
//                   "==================================================" << std::endl <<
//                   "** Allowed options";

    bf::path out_path = out_dir;
    out_path /= "confusion_matrix.txt";

    v4r::io::createDirForFileIfNotExist( out_path.string() );
    std::ofstream f( out_path.string() );
    std::cout << "Writing results to " << out_path.string() << "..." << std::endl;

    std::vector<std::string> annotation_files = v4r::io::getFilesInDirectory( gt_dir, ".*.anno", true );

    Eigen::MatrixXi confusion_matrix = Eigen::MatrixXi::Zero( models.size(), models.size());

    std::map<std::string, int> modelname2modelid;
    size_t id=0;
    for(const auto &m:models)
    {
        modelname2modelid[m.first] = id;
//        f << id << ": " << m.first << std::endl;
        id++;
    }

    for( const std::string anno_file : annotation_files )
    {
        bf::path gt_path = gt_dir;
        gt_path /= anno_file;

        std::string rec_file = anno_file;
        if( use_generated_hypotheses_ )
            boost::replace_last( rec_file, ".anno", ".generated_hyps");

        bf::path rec_path = or_dir;
        rec_path /= rec_file;

        if(!v4r::io::existsFile(rec_path.string()))
        {
            LOG(WARNING) << "Recognition path " << rec_path.string() << " does not exist!";
            continue;
        }

        Eigen::MatrixXi tmp_confusion_matrix = Eigen::MatrixXi::Zero( models.size(), models.size());

        std::map<std::string, std::vector<Hypothesis> > gt_hyps_all_models = readHypothesesFromFile( gt_path.string() );
        std::map<std::string, std::vector<Hypothesis> > rec_hyps = readHypothesesFromFile( rec_path.string() );

        for(auto const &gt_model_hyps : gt_hyps_all_models)
        {
            const std::string &model_name_gt = gt_model_hyps.first;
            const Model &m = models[ model_name_gt ];

            for(const Hypothesis &h_gt : gt_model_hyps.second)
            {
                float occlusion = h_gt.occlusion;
                float lowest_trans_error = std::numeric_limits<float>::max();
                std::string best_match = "";

                for(auto const &rec_model_hyps : rec_hyps)
                {
                    const std::string &rec_model_name = rec_model_hyps.first;
                    for (const Hypothesis &h_rec : rec_model_hyps.second)
                    {
                        const Eigen::Vector4f centroid_a = h_gt.pose * m.centroid;
                        const Eigen::Vector4f centroid_b = h_rec.pose * m.centroid;

                        //ignore z

                        float trans_error = (h_gt.pose.block<2,1>(0,3)-h_rec.pose.block<2,1>(0,3)).norm();
                        VLOG(1) << h_gt.pose;
                        VLOG(1) << h_rec.pose;
                        VLOG(1) << trans_error;

                        if(trans_error < lowest_trans_error)
                        {
                            best_match = rec_model_name;
                            lowest_trans_error = trans_error;
                        }
                    }
                }

                if( !best_match.empty() && lowest_trans_error < translation_error_threshold_m)
                {
                    tmp_confusion_matrix( modelname2modelid[model_name_gt], modelname2modelid[best_match] ) ++;
                }
            }
        }

        VLOG(1) << tmp_confusion_matrix << std::endl << std::endl
                  << "view accuracy: " << tmp_confusion_matrix.trace() << " / " <<
                     tmp_confusion_matrix.sum() << " (" <<
                     (float)tmp_confusion_matrix.trace() / tmp_confusion_matrix.sum() << ")" << std::endl;

        if(visualize_)
        {
            std::string scene_name (anno_file);
            boost::replace_last( scene_name, ".anno", ".pcd");

            bf::path scene_path = test_dir;
            scene_path /= scene_name;
            pcl::PointCloud<PointT>::Ptr scene_cloud (new pcl::PointCloud<PointT>);
            pcl::io::loadPCDFile( scene_path.string(), *scene_cloud);

            visualizeResults( scene_cloud, gt_path, rec_path);
        }

        confusion_matrix += tmp_confusion_matrix;
    }

    std::cout << confusion_matrix << std::endl << std::endl
              << "view accuracy: " << confusion_matrix.trace() << " / " <<
                 confusion_matrix.sum() << " (" <<
                 (float)confusion_matrix.trace() / confusion_matrix.sum() << ")" << std::endl;

    f << confusion_matrix;
    f.close();
    std::cout << "Done!" << std::endl;

    return confusion_matrix;
}

}
}


#ifndef PYTHONIC_INCLUDE_NUMPY_ABS_HPP
#define PYTHONIC_INCLUDE_NUMPY_ABS_HPP

#include "pythonic/include/types/numpy_op_helper.hpp"
#include "pythonic/include/utils/meta.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include "pythonic/include/utils/functor.hpp"

#include <boost/simd/function/abs.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    // boost simd does ! handle bool specialization, which is indeed a
    // corner case...
    template <class T>
    auto abs(T const &v) -> decltype(boost::simd::abs(v))
    {
      return boost::simd::abs(v);
    }
    constexpr bool abs(bool const &v)
    {
      return v;
    }
    template <class T>
    T abs(std::complex<T> const v)
    {
      return std::abs(v);
    }
  }

#define NUMPY_NARY_FUNC_NAME abs
#ifdef USE_BOOST_SIMD
#define NUMPY_NARY_FUNC_SYM wrapper::abs
#else
#define NUMPY_NARY_FUNC_SYM std::abs
#endif
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <iotbx/error.h>
#include <cctbx/sgtbx/space_group.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>

namespace iotbx { namespace boost_python { namespace dtrek_ext {

  namespace af = scitbx::af;

  class anomalous_combined
  {
    public:
      anomalous_combined(
        cctbx::sgtbx::space_group const& space_group,
        af::const_ref<cctbx::miller::index<> > const& miller_indices,
        af::const_ref<double> const& data_plus,
        af::const_ref<double> const& sigmas_plus,
        af::const_ref<double> const& data_minus,
        af::const_ref<double> const& sigmas_minus)
      {
        IOTBX_ASSERT(data_plus.size() == miller_indices.size());
        IOTBX_ASSERT(sigmas_plus.size() == miller_indices.size());
        IOTBX_ASSERT(data_minus.size() == miller_indices.size());
        IOTBX_ASSERT(sigmas_minus.size() == miller_indices.size());
        for(std::size_t i=0;i<miller_indices.size();i++) {
          miller_indices_.push_back(miller_indices[i]);
          if (!space_group.is_centric(miller_indices[i])) {
            miller_indices_.push_back(-miller_indices[i]);
            data_.push_back(data_plus[i]);
            data_.push_back(data_minus[i]);
            sigmas_.push_back(sigmas_plus[i]);
            sigmas_.push_back(sigmas_minus[i]);
          }
          else {
            double sum_w_data = 0;
            double sum_w = 0;
            if (sigmas_plus[i] > 0) {
              double w = 1 / (sigmas_plus[i]*sigmas_plus[i]);
              sum_w_data += w * data_plus[i];
              sum_w += w;
            }
            if (sigmas_minus[i] > 0) {
              double w = 1 / (sigmas_minus[i]*sigmas_minus[i]);
              sum_w_data += w * data_minus[i];
              sum_w += w;
            }
            if (sum_w == 0) {
              data_.push_back(data_plus[i]);
              sigmas_.push_back(sigmas_plus[i]);
            }
            else {
              data_.push_back(sum_w_data / sum_w);
              sigmas_.push_back(1/std::sqrt(sum_w));
            }
          }
        }
      }

      af::shared<cctbx::miller::index<> >
      miller_indices() const { return miller_indices_; }

      af::shared<double>
      data() const { return data_; }

      af::shared<double>
      sigmas() const { return sigmas_; }

    private:
      af::shared<cctbx::miller::index<> > miller_indices_;
      af::shared<double> data_;
      af::shared<double> sigmas_;
  };

  struct anomalous_combined_wrappers
  {
    typedef anomalous_combined w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("anomalous_combined", no_init)
        .def(init<cctbx::sgtbx::space_group const&,
                  af::const_ref<cctbx::miller::index<> > const&,
                  af::const_ref<double> const&,
                  af::const_ref<double> const&,
                  af::const_ref<double> const&,
                  af::const_ref<double> const&>())
        .def("miller_indices", &w_t::miller_indices)
        .def("data", &w_t::data)
        .def("sigmas", &w_t::sigmas)
      ;
    }
  };

  void init_module()
  {
    anomalous_combined_wrappers::wrap();
  }

}}} // namespace iotbx::boost_python::dtrek_ext

BOOST_PYTHON_MODULE(iotbx_dtrek_ext)
{
  iotbx::boost_python::dtrek_ext::init_module();
}