#include <Eigen/Dense>
#include <iostream> // must go before other things because R defines a "length" macro
#include <nimble/RcppUtils.h>
#include "nimble/RcppNimbleUtils.h"
//#include <nimble/ModelClassUtils.h>
//#include <nimble/accessorClasses.h>
#include <nimble/nimbleGraph.h>
#include <nimble/EigenTypedefs.h>
#include <nimble/dists.h>

#include <R_ext/Rdynload.h>

#define FUN(name, numArgs) \
  {#name, (DL_FUNC) &name, numArgs}

#define CFUN(name, numArgs) \
  {"R_"#name, (DL_FUNC) &name, numArgs}

R_CallMethodDef CallEntries[] = {
 FUN(C_dwish_chol, 5),
 FUN(C_rwish_chol, 3),
 FUN(C_dinvwish_chol, 5),
 FUN(C_rinvwish_chol, 3),
 FUN(C_ddirch, 3),
 FUN(C_rdirch, 1),
 FUN(C_dmulti, 4),
 FUN(C_rmulti, 2),
 FUN(C_dcat, 3),
 FUN(C_rcat, 2),
 FUN(C_dt_nonstandard, 5),
 FUN(C_rt_nonstandard, 4),
 FUN(C_qt_nonstandard, 6),
 FUN(C_pt_nonstandard, 6),
 FUN(C_dmnorm_chol, 5),
 FUN(C_rmnorm_chol, 3),
 FUN(C_dinterval, 4),
 FUN(C_rinterval, 3),
 FUN(C_dmvt_chol, 6),
 FUN(C_rmvt_chol, 4),
 FUN(C_dexp_nimble, 3),
 FUN(C_rexp_nimble, 2),
 FUN(C_pexp_nimble, 4),
 FUN(C_qexp_nimble, 4),
 FUN(C_ddexp, 4),
 FUN(C_rdexp, 3),
 FUN(C_pdexp, 5),
 FUN(C_qdexp, 5),
 FUN(C_dinvgamma, 4),
 FUN(C_rinvgamma, 3),
 FUN(C_pinvgamma, 5),
 FUN(C_qinvgamma, 5),
 FUN(C_dsqrtinvgamma, 4),
 FUN(C_rsqrtinvgamma, 3),
 FUN(C_dcar_normal, 8),
 FUN(C_dcar_proper, 10),
 FUN(C_rcar_proper, 9),
 FUN(C_nimEigen, 4),
 FUN(C_nimSvd, 3),
 FUN(fastMatrixInsert, 4),
 FUN(matrix2ListDouble, 5),
 FUN(matrix2ListInt, 5),
 FUN(C_rankSample, 4),
 FUN(parseVar, 1),
 FUN(makeParsedVarList, 1),
 FUN(C_setGraph, 7),
 FUN(C_anyStochDependencies, 1),
 FUN(C_anyStochParents, 1),
 FUN(C_getDependencies, 4),
 FUN(C_getDependencyPathCountOneNode, 2),
 {NULL, NULL, 0}
};

/* Arrange to register the routines so that they can be checked and also accessed by
   their equivalent name as an R symbol, e.g. .Call(setNodeModelPtr, ....)
 */
extern "C"
void
R_init_nimble(DllInfo *dll)
{
    R_registerRoutines(dll, NULL, CallEntries, NULL, NULL);
    R_useDynamicSymbols(dll, FALSE);
}


/*
 Copyright (C) 2016 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.
*/

/*!
  \file qlw/trades/equityforward.cpp
  \brief Equity Forward data model
  \ingroup portfolio
*/

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/equityforward.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/equityforward.hpp>
#include <ored/portfolio/referencedata.hpp>
#include <ored/utilities/currencycheck.hpp>
#include <ql/errors.hpp>
#include <qle/instruments/equityforward.hpp>

using namespace QuantLib;
using namespace std;

namespace ore {
namespace data {

void EquityForward::build(const boost::shared_ptr<EngineFactory>& engineFactory) {
    Currency ccy = parseCurrencyWithMinors(currency_);

    // get the equity currency from the market
    Currency equityCcy = engineFactory->market()->equityCurve(eqName())->currency();

    // ensure forward currency matches the equity currency
    QL_REQUIRE(!equityCcy.empty(), "No equity currency in equityCurve for equity " << eqName());
    QL_REQUIRE(ccy == equityCcy, "EquityForward currency " << ccy << " does not match equity currency " << equityCcy << " for trade " << id());

    // convert strike to the major currency if needed
    Real strike;
    if (!strikeCurrency_.empty()) {
        Currency strikeCcy = parseCurrencyWithMinors(strikeCurrency_);
        strike = convertMinorToMajorCurrency(strikeCurrency_, strike_);
        // ensure strike currency matches equity currency
        QL_REQUIRE(strikeCcy == equityCcy, "Stike currency " << ccy << " does not match equity currency " << equityCcy << " for trade " << id());
    } else {
        WLOG("No Strike Currency provide for trade " << id() << ", assuming trade currency " << ccy);
        strike = convertMinorToMajorCurrency(currency_, strike_);
    }

    QuantLib::Position::Type longShort = parsePositionType(longShort_);
    Date maturity = parseDate(maturityDate_);

    string name = eqName();

    boost::shared_ptr<Instrument> inst =
        boost::make_shared<QuantExt::EquityForward>(name, ccy, longShort, quantity_, maturity, strike);

    // Pricing engine
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<EquityForwardEngineBuilder> eqFwdBuilder =
        boost::dynamic_pointer_cast<EquityForwardEngineBuilder>(builder);
    inst->setPricingEngine(eqFwdBuilder->engine(name, ccy));

    // set up other Trade details
    instrument_ = boost::shared_ptr<InstrumentWrapper>(new VanillaInstrument(inst));
    npvCurrency_ = ccy.code();
    maturity_ = maturity;
    // Notional - we really need todays spot to get the correct notional.
    // But rather than having it move around we use strike * quantity
    notional_ = strike * quantity_;
    notionalCurrency_ = ccy.code();

    additionalData_["underlyingSecurityId"] = name;
    additionalData_["strike"] = strike;
    additionalData_["strikeCurrency"] = strikeCurrency_;
    additionalData_["quantity"] = quantity_;
}

void EquityForward::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* eNode = XMLUtils::getChildNode(node, "EquityForwardData");

    longShort_ = XMLUtils::getChildValue(eNode, "LongShort", true);
    maturityDate_ = XMLUtils::getChildValue(eNode, "Maturity", true);
    XMLNode* tmp = XMLUtils::getChildNode(eNode, "Underlying");
    if (!tmp)
        tmp = XMLUtils::getChildNode(eNode, "Name");
    equityUnderlying_.fromXML(tmp);
    currency_ = XMLUtils::getChildValue(eNode, "Currency", true);
    strike_ = XMLUtils::getChildValueAsDouble(eNode, "Strike", true);
    strikeCurrency_ = XMLUtils::getChildValue(eNode, "StrikeCurrency", false);
    quantity_ = XMLUtils::getChildValueAsDouble(eNode, "Quantity", true);
}

XMLNode* EquityForward::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* eNode = doc.allocNode("EquityForwardData");
    XMLUtils::appendNode(node, eNode);

    XMLUtils::addChild(doc, eNode, "LongShort", longShort_);
    XMLUtils::addChild(doc, eNode, "Maturity", maturityDate_);
    XMLUtils::appendNode(eNode, equityUnderlying_.toXML(doc));
    XMLUtils::addChild(doc, eNode, "Currency", currency_);
    XMLUtils::addChild(doc, eNode, "Strike", strike_);
    if (!strikeCurrency_.empty())
        XMLUtils::addChild(doc, eNode, "StrikeCurrency", strikeCurrency_);
    XMLUtils::addChild(doc, eNode, "Quantity", quantity_);
    return node;
}

std::map<AssetClass, std::set<std::string>>
EquityForward::underlyingIndices(const boost::shared_ptr<ReferenceDataManager>& referenceDataManager) const {
    return {{AssetClass::EQ, std::set<std::string>({eqName()})}};
}

} // namespace data
} // namespace ore


#include <data/Geometry.hpp>
#include <engine/Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Qhull.h"
#include "QhullFacetList.h"
#include "QhullVertexSet.h"

#include <array>

namespace Data
{
    Geometry::Geometry(std::vector<Vector3> bravais_vectors, intfield n_cells, std::vector<Vector3> cell_atoms,
        intfield cell_atom_types, scalar lattice_constant) :
        bravais_vectors(bravais_vectors), n_cells(n_cells),
        n_cell_atoms(cell_atoms.size()), cell_atoms(cell_atoms), lattice_constant(lattice_constant),
        nos(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]), cell_atom_types(cell_atom_types),
        n_cells_total(n_cells[0] * n_cells[1] * n_cells[2])
    {
        for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
        {
            // Get x,y,z of component of atom positions in unit of length (instead of in units of a,b,c)
            Vector3 build_array = bravais_vectors[0] * cell_atoms[iatom][0] + bravais_vectors[1] * cell_atoms[iatom][1] + bravais_vectors[2] * cell_atoms[iatom][2];
            cell_atoms[iatom] = lattice_constant * build_array;
        }

        // Generate positions and atom types
        this->positions = vectorfield(nos);
        this->atom_types = intfield(nos, 0);
        Engine::Vectormath::Build_Spins(positions, atom_types, cell_atoms, cell_atom_types, bravais_vectors, n_cells);

        // Calculate some info
        this->calculateBounds();
        this->calculateUnitCellBounds();
        this->calculateDimensionality();

        // Calculate center of the System
        this->center = 0.5 *  (this->bounds_min + this->bounds_max);

        // Calculate the type of geometry
        this->calculateGeometryType();

        // For updates of triangulation and tetrahedra
        this->last_update_n_cell_step = -1;
        this->last_update_n_cells = intfield(3, -1);
    }

    std::vector<tetrahedron_t> compute_delaunay_triangulation_3D(const std::vector<vector3_t> & points)
    {
        const int ndim = 3;
        std::vector<tetrahedron_t> tetrahedra;
        tetrahedron_t tmp_tetrahedron;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        orgQhull::QhullFacetList facet_list = qhull.facetList();
        for(const auto & facet : facet_list)
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_tetrahedron[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                tetrahedra.push_back(tmp_tetrahedron);
            }
        }
        return tetrahedra;
    }

    std::vector<triangle_t> compute_delaunay_triangulation_2D(const std::vector<vector2_t> & points)
    {
        const int ndim = 2;
        std::vector<triangle_t> triangles;
        triangle_t tmp_triangle;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        for(const auto & facet : qhull.facetList())
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_triangle[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                triangles.push_back(tmp_triangle);
            }
        }
        return triangles;
    }

    const std::vector<triangle_t>& Geometry::triangulation(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if ( (n_cells[0]/n_cell_step < 2 && n_cells[0] > 1) ||
             (n_cells[1]/n_cell_step < 2 && n_cells[1] > 1) ||
             (n_cells[2]/n_cell_step < 2 && n_cells[2] > 1) )
        {
            _triangulation.clear();
            return _triangulation;
        }

        // 2D: triangulation
        if (dimensionality == 2)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                _triangulation.clear();

                std::vector<vector2_t> points;
                points.resize(positions.size());

                int icell = 0, idx;
                for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                {
                    for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                    {
                        for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                        {
                            for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                            {
                                idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                points[icell].x = positions[idx][0];
                                points[icell].y = positions[idx][1];
                                ++icell;
                            }
                        }
                    }
                }
                _triangulation = compute_delaunay_triangulation_2D(points);
            }
        }// endif 2D
        // 0D, 1D and 3D give no triangulation
        else
        {
            _triangulation.clear();
        }
        return _triangulation;
    }

    const std::vector<tetrahedron_t>& Geometry::tetrahedra(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if (n_cells[0]/n_cell_step < 2 || n_cells[1]/n_cell_step < 2 || n_cells[2]/n_cell_step < 2)
        {
            _tetrahedra.clear();
            return _tetrahedra;
        }

        // 3D: Tetrahedra
        if (dimensionality == 3)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                // If we have only one spin in the basis our lattice is a simple regular geometry
                bool is_simple_regular_geometry = n_cell_atoms == 1;

                // If we have a simple regular geometry everything can be calculated by hand
                if (is_simple_regular_geometry)
                {
                    _tetrahedra.clear();
                    int cell_indices[] = {
                        0, 1, 5, 3,
                        1, 3, 2, 5,
                        3, 2, 5, 6,
                        7, 6, 5, 3,
                        4, 7, 5, 3,
                        0, 4, 3, 5
                        };
                    int x_offset = 1;
                    int y_offset = n_cells[0]/n_cell_step;
                    int z_offset = n_cells[0]/n_cell_step*n_cells[1]/n_cell_step;
                    int offsets[] = {
                        0, x_offset, x_offset+y_offset, y_offset,
                        z_offset, x_offset+z_offset, x_offset+y_offset+z_offset, y_offset+z_offset
                        };

                    for (int ix = 0; ix < (n_cells[0]-1)/n_cell_step; ix++)
                    {
                        for (int iy = 0; iy < (n_cells[1]-1)/n_cell_step; iy++)
                        {
                            for (int iz = 0; iz < (n_cells[2]-1)/n_cell_step; iz++)
                            {
                                int base_index = ix*x_offset+iy*y_offset+iz*z_offset;
                                for (int j = 0; j < 6; j++)
                                {
                                    tetrahedron_t tetrahedron;
                                    for (int k = 0; k < 4; k++)
                                    {
                                        int index = base_index + offsets[cell_indices[j*4+k]];
                                        tetrahedron[k] = index;
                                    }
                                    _tetrahedra.push_back(tetrahedron);
                                }
                            }
                        }
                    }
                }
                // For general basis cells we calculate the Delaunay tetrahedra
                else
                {
                    std::vector<vector3_t> points;
                    points.resize(positions.size());

                    int icell = 0, idx;
                    for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                    {
                        for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                        {
                            for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                            {
                                for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                                {
                                    idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                    points[icell].x = positions[idx][0];
                                    points[icell].y = positions[idx][1];
                                    points[icell].z = positions[idx][2];
                                    ++icell;
                                }
                            }
                        }
                    }
                    _tetrahedra = compute_delaunay_triangulation_3D(points);
                }
            }
        } // endif 3D
        // 0-2 D gives no tetrahedra
        else
        {
            _tetrahedra.clear();
        }
        return _tetrahedra;
    }

    std::vector<Vector3> Geometry::BravaisVectorsSC()
    {
        return { { 1,0,0 },
                 { 0,1,0 },
                 { 0,0,1 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsFCC()
    {
        return { { 0.5,0.0,0.5 },
                 { 0.5,0.5,0.0 },
                 { 0.0,0.5,0.5 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsBCC()
    {
        return { { 0.5, 0.5,-0.5 },
                 { -0.5, 0.5,-0.5 },
                 { 0.5,-0.5, 0.5 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D60()
    {
        return { { 1,   0,                0 },
                 { 0.5, 0.5*std::sqrt(3), 0 },
                 { 0,   0,                1 } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D120()
    {
        return { {  1,   0,                0 },
                 { -0.5, 0.5*std::sqrt(3), 0 },
                 {  0,   0,                1 } };
    }

    void Geometry::calculateDimensionality()
    {
        int dims_basis = 0, dims_translations = 0;
        Vector3 test_vec_basis, test_vec_translations;

        // ----- Find dimensionality of the basis -----
        if      (n_cell_atoms == 1) dims_basis = 0;
        else if (n_cell_atoms == 2) dims_basis = 1;
        else if (n_cell_atoms == 3) dims_basis = 2;
        else
        {
            // Get basis atoms relative to the first atom
            Vector3 v0 = cell_atoms[0];
            std::vector<Vector3> b_vectors(n_cell_atoms-1);
            for (int i = 1; i < n_cell_atoms; ++i)
            {
                b_vectors[i-1] = cell_atoms[i] - v0;
            }
            // Calculate basis dimensionality
            // test vec is along line
            test_vec_basis = b_vectors[0];
            //		is it 1D?
            int n_parallel = 0;
            for (unsigned int i = 1; i < b_vectors.size(); ++i)
            {
                if (std::abs(b_vectors[i].dot(test_vec_basis) - 1.0) < 1e-9) ++n_parallel;
                // else n_parallel will give us the last parallel vector
                // also the if-statement for dims_basis=1 wont be met
                else break;
            }
            if (n_parallel == b_vectors.size() - 1)
            {
                dims_basis = 1;
            }
            else
            {
                // test vec is normal to plane
                test_vec_basis = b_vectors[0].cross(b_vectors[n_parallel+1]);
                //		is it 2D?
                int n_in_plane = 0;
                for (unsigned int i = 2; i < b_vectors.size(); ++i)
                {
                    if (std::abs(b_vectors[i].dot(test_vec_basis)) < 1e-9) ++n_in_plane;
                }
                if (n_in_plane == b_vectors.size() - 2)
                {
                    dims_basis = 2;
                }
                else
                {
                    this->dimensionality = 3;
                    return;
                }
            }
        }

        // ----- Find dimensionality of the translations -----
        //		The following are zero if the corresponding pair is parallel
        double t01, t02, t12;
        t01 = std::abs(bravais_vectors[0].dot(bravais_vectors[1]) - 1.0);
        t02 = std::abs(bravais_vectors[0].dot(bravais_vectors[2]) - 1.0);
        t12 = std::abs(bravais_vectors[1].dot(bravais_vectors[2]) - 1.0);
        //		Check if pairs are linearly independent
        int n_independent_pairs = 0;
        if (t01>1e-9 && n_cells[0] > 1 && n_cells[1] > 1) ++n_independent_pairs;
        if (t02>1e-9 && n_cells[0] > 1 && n_cells[2] > 1) ++n_independent_pairs;
        if (t12>1e-9 && n_cells[1] > 1 && n_cells[2] > 1) ++n_independent_pairs;
        //		Calculate translations dimensionality
        if (n_cells[0] == 1 && n_cells[1] == 1 && n_cells[2] == 1) dims_translations = 0;
        else if (n_independent_pairs == 0)
        {
            dims_translations = 1;
            // test vec is along the line
            for (int i=0; i<3; ++i) if (n_cells[i] > 1) test_vec_translations = bravais_vectors[i];
        }
        else if (n_independent_pairs < 3)
        {
            dims_translations = 2;
            // test vec is normal to plane
            int n = 0;
            std::vector<Vector3> plane(2);
            for (int i = 0; i < 3; ++i)
            {
                if (n_cells[i] > 1) plane[n] = bravais_vectors[i];
                ++n;
            }
            test_vec_translations = plane[0].cross(plane[1]);
        }
        else
        {
            this->dimensionality = 3;
            return;
        }

        // ----- Calculate dimensionality of system -----
        test_vec_basis.normalize();
        test_vec_translations.normalize();
        //		If one dimensionality is zero, only the other counts
        if (dims_basis == 0)
        {
            this->dimensionality = dims_translations;
            return;
        }
        else if (dims_translations == 0)
        {
            this->dimensionality = dims_basis;
            return;
        }
        //		If both are linear or both are planar, the test vectors should be parallel if the geometry is 1D or 2D
        else if (dims_basis == dims_translations)
        {
            if (std::abs(test_vec_basis.dot(test_vec_translations) - 1.0) < 1e-9)
            {
                this->dimensionality = dims_basis;
                return;
            }
            else if (dims_basis == 1)
            {
                this->dimensionality = 2;
                return;
            }
            else if (dims_basis == 2)
            {
                this->dimensionality = 3;
                return;
            }
        }
        //		If one is linear (1D), and the other planar (2D) then the test vectors should be orthogonal if the geometry is 2D
        else if ( (dims_basis == 1 && dims_translations == 2) || (dims_basis == 2 && dims_translations == 1) )
        {
            if (std::abs(test_vec_basis.dot(test_vec_translations)) < 1e-9)
            {
                this->dimensionality = 2;
                return;
            }
            else
            {
                this->dimensionality = 3;
                return;
            }
        }
    }

    void Geometry::calculateBounds()
    {
        this->bounds_max.setZero();
        this->bounds_min.setZero();
        for (int iatom = 0; iatom < nos; ++iatom)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                if (this->positions[iatom][dim] < this->bounds_min[dim]) this->bounds_min[dim] = positions[iatom][dim];
                if (this->positions[iatom][dim] > this->bounds_max[dim]) this->bounds_max[dim] = positions[iatom][dim];
            }
        }
    }

    void Geometry::calculateUnitCellBounds()
    {
        this->cell_bounds_max.setZero();
        this->cell_bounds_min.setZero();
        for (unsigned int ivec = 0; ivec < bravais_vectors.size(); ++ivec)
        {
            for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
            {
                auto neighbour1 = cell_atoms[iatom] + bravais_vectors[ivec];
                auto neighbour2 = cell_atoms[iatom] - bravais_vectors[ivec];
                for (int dim = 0; dim < 3; ++dim)
                {
                    if (neighbour1[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour1[dim];
                    if (neighbour1[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour1[dim];
                    if (neighbour2[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour2[dim];
                    if (neighbour2[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour2[dim];
                }
            }
        }
        this->cell_bounds_min *= 0.5;
        this->cell_bounds_max *= 0.5;
    }

    void Geometry::calculateGeometryType()
    {
        // Automatically try to determine GeometryType
        // Single-atom unit cell
        if (cell_atoms.size() == 1)
        {
            // If the basis vectors are orthogonal, it is a rectilinear lattice
            if (std::abs(bravais_vectors[0].dot(bravais_vectors[1])) < 1e-6 &&
                std::abs(bravais_vectors[0].dot(bravais_vectors[2])) < 1e-6)
            {
                // If equidistant it is simple cubic
                if (bravais_vectors[0].norm() == bravais_vectors[1].norm() == bravais_vectors[2].norm())
                    this->classifier = BravaisLatticeType::SC;
                // Otherwise only rectilinear
                else
                    this->classifier = BravaisLatticeType::Rectilinear;
            }
        }
        // Regular unit cell with multiple atoms (e.g. bcc, fcc, hex)
        //else if (n_cell_atoms == 2)
        // Irregular unit cells arranged on a lattice (e.g. B20 or custom)
        /*else if (n_cells[0] > 1 || n_cells[1] > 1 || n_cells[2] > 1)
        {
            this->classifier = BravaisLatticeType::Lattice;
        }*/
        // A single irregular unit cell
        else
        {
            this->classifier = BravaisLatticeType::Irregular;
        }
    }
}


// Copyright 2020 Tier IV, Inc.
//
// 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 "turn_signal_decider/frenet_coordinate.hpp"

#include <Eigen/Dense>

#include <limits>
#include <vector>

namespace
{
Eigen::Vector3d convertToEigenPt(const geometry_msgs::msg::Point geom_pt)
{
  return Eigen::Vector3d(geom_pt.x, geom_pt.y, geom_pt.z);
}

std::vector<geometry_msgs::msg::Point> convertToPointArray(
  const autoware_planning_msgs::msg::PathWithLaneId & path)
{
  std::vector<geometry_msgs::msg::Point> point_array;
  for (const auto & pt : path.points) {
    point_array.push_back(pt.point.pose.position);
  }
  return point_array;
}
}  // namespace

namespace turn_signal_decider
{
bool convertToFrenetCoordinate3d(
  const autoware_planning_msgs::msg::PathWithLaneId & path,
  const geometry_msgs::msg::Point & search_point_geom, FrenetCoordinate3d * frenet_coordinate)
{
  const auto linestring = convertToPointArray(path);
  return convertToFrenetCoordinate3d(linestring, search_point_geom, frenet_coordinate);
}

// returns false when search point is off the linestring
bool convertToFrenetCoordinate3d(
  const std::vector<geometry_msgs::msg::Point> & linestring,
  const geometry_msgs::msg::Point & search_point_geom, FrenetCoordinate3d * frenet_coordinate)
{
  if (linestring.empty()) {
    return false;
  }

  const auto search_pt = convertToEigenPt(search_point_geom);
  bool found = false;
  double min_distance = std::numeric_limits<double>::max();

  // get frenet coordinate based on points
  // this is done because linestring is not differentiable at vertices
  {
    double accumulated_length = 0;

    for (std::size_t i = 0; i < linestring.size(); i++) {
      const auto geom_pt = linestring.at(i);
      const auto current_pt = convertToEigenPt(geom_pt);
      const auto current2search_pt = (search_pt - current_pt);
      // update accumulated length
      if (i != 0) {
        const auto p1 = convertToEigenPt(linestring.at(i - 1));
        const auto p2 = current_pt;
        accumulated_length += (p2 - p1).norm();
      }
      // update frenet coordinate

      const double tmp_distance = current2search_pt.norm();
      if (tmp_distance < min_distance) {
        found = true;
        min_distance = tmp_distance;
        frenet_coordinate->distance = tmp_distance;
        frenet_coordinate->length = accumulated_length;
      }
    }
  }

  // get frenet coordinate based on lines
  {
    auto prev_geom_pt = linestring.front();
    double accumulated_length = 0;
    for (const auto & geom_pt : linestring) {
      const auto start_pt = convertToEigenPt(prev_geom_pt);
      const auto end_pt = convertToEigenPt(geom_pt);

      const auto line_segment = end_pt - start_pt;
      const double line_segment_length = line_segment.norm();
      const auto direction = line_segment / line_segment_length;
      const auto start2search_pt = (search_pt - start_pt);

      double tmp_length = direction.dot(start2search_pt);
      if (tmp_length >= 0 && tmp_length <= line_segment_length) {
        double tmp_distance = direction.cross(start2search_pt).norm();
        if (tmp_distance < min_distance) {
          found = true;
          min_distance = tmp_distance;
          frenet_coordinate->distance = tmp_distance;
          frenet_coordinate->length = accumulated_length + tmp_length;
        }
      }
      accumulated_length += line_segment_length;
      prev_geom_pt = geom_pt;
    }
  }
  return found;
}
}  // namespace turn_signal_decider


#include <cmath>
#include <string>
#include <vector>

#include <boost/asio.hpp>

#include <goby/middleware/marshalling/dccl.h>
#include <goby/middleware/marshalling/protobuf.h>

#include <goby/middleware/io/line_based/serial.h>
#include <goby/util/linebasedcomms/nmea_sentence.h>
#include <goby/zeromq/application/multi_thread.h>

#include "config.pb.h"
#include "messages/gps.pb.h"
#include "messages/groups.h"

using protobuf::GPSCommand;
using protobuf::GPSPosition;

using AppBase = goby::zeromq::MultiThreadApplication<GPSDriverConfig>;
using ThreadBase = goby::middleware::SimpleThread<GPSDriverConfig>;
namespace si = boost::units::si;

using goby::glog;

class GPSParseThread : public ThreadBase
{
  public:
    GPSParseThread(const GPSDriverConfig& cfg) : ThreadBase(cfg)
    {
        // subscribe to data from the serial port
        interthread().subscribe<groups::gps_raw_in, goby::middleware::protobuf::IOData>(
            [this](std::shared_ptr<const goby::middleware::protobuf::IOData> io_msg) {
                parse_line(io_msg->data());
            });
    }
    ~GPSParseThread() {}

    void parse_line(const std::string& line)
    {
        glog.is_verbose() && glog << "GPSParseThread: " << line << std::endl;

        try
        {
            goby::util::NMEASentence nmea(line);

            if (nmea.sentence_id() == "GGA")
            {
                GPSPosition gps;

                enum
                {
                    TALKER = 0,
                    UTC = 1,
                    LAT = 2,
                    LAT_HEMI = 3,
                    LON = 4,
                    LON_HEMI = 5,
                    FIX_QUALITY = 6
                };

                gps.set_time(nmea_time_to_seconds(nmea.as<double>(UTC)));
                gps.set_latitude(
                    nmea_geo_to_decimal(nmea.as<double>(LAT), nmea.as<char>(LAT_HEMI)));
                gps.set_longitude(
                    nmea_geo_to_decimal(nmea.as<double>(LON), nmea.as<char>(LON_HEMI)));

                interprocess().publish<groups::gps_data>(gps);
            }
        }
        catch (goby::util::bad_nmea_sentence& e)
        {
            glog.is_warn() && glog << "Invalid NMEA sentence: " << e.what() << std::endl;
        }
    }

  private:
    // given a time in "NMEA form", returns the value as seconds since the start of the day
    // NMEA form is HHMMSS.SSSS where "H" is hours, "M" is minutes, "S" is seconds or fractional seconds
    double nmea_time_to_seconds(double nmea_time)
    {
        double hours = std::floor(nmea_time / 1e4);
        nmea_time -= hours * 1e4;
        double minutes = std::floor(nmea_time / 1e2);
        nmea_time -= minutes * 1e2;
        double seconds = nmea_time;

        return hours * 3600 + minutes * 60 + seconds;
    }

    // given a latitude or longitude in "NMEA form" and the hemisphere character ('N', 'S', 'E' or 'W'), returns the value as decimal degrees
    // NMEA form is DDDMM.MMMM or DDMM.MMMM where "D" is degrees, and "M" is minutes
    double nmea_geo_to_decimal(double nmea_geo, char hemi)
    {
        // DDMM.MMMM
        double deg_int = std::floor(nmea_geo / 1e2);
        double deg_frac = (nmea_geo - (deg_int * 1e2)) / 60;

        double sign = 1;
        if (hemi == 'S' || hemi == 'W')
            sign = -1;

        return sign * (deg_int + deg_frac);
    }
};

class GPSAnalyzeThread : public ThreadBase
{
  public:
    GPSAnalyzeThread(const GPSDriverConfig& cfg) : ThreadBase(cfg)
    {
        interthread().subscribe<groups::gps_data, GPSPosition>([](const GPSPosition& pos) {
            glog.is_verbose() && glog << "GPSAnalyzeThread: " << pos.ShortDebugString()
                                      << std::endl;
        });
    }
};

class GPSDriver : public AppBase
{
  public:
    using SerialThread =
        goby::middleware::io::SerialThreadLineBased<groups::gps_raw_in, groups::gps_raw_out>;

    GPSDriver()
    {
        interprocess().subscribe<groups::gps_control, GPSCommand>(
            [this](const GPSCommand& cmd) { this->incoming_command(cmd); });

        launch_thread<SerialThread>(cfg().serial());
        launch_thread<GPSParseThread>();
        launch_thread<GPSAnalyzeThread>();
    }

    void incoming_command(const GPSCommand& cmd)
    {
        glog.is_verbose() && glog << "GPSDriver (main thread): incoming command: "
                                  << cmd.ShortDebugString() << std::endl;
        try
        {
            if (cmd.read_gps())
                launch_thread<SerialThread>(cfg().serial());
            else
                join_thread<SerialThread>();
        }
        catch (goby::Exception& e)
        {
            glog.is_verbose() && glog << "Did not process command. Reason: " << e.what()
                                      << std::endl;
        }
    }
};

// Use a custom configurator to modify the configuration after default protobuf parser to set default serial parameters for a GPS.
class GPSDriverConfigurator
    : public goby::middleware::ProtobufConfigurator<GPSDriverConfig>
{
  public:
    GPSDriverConfigurator(int argc, char* argv[])
        : goby::middleware::ProtobufConfigurator<GPSDriverConfig>(argc, argv)
    {
        GPSDriverConfig& cfg = mutable_cfg();
        goby::middleware::protobuf::SerialConfig& serial_cfg = *cfg.mutable_serial();

        // set default baud
        if (!serial_cfg.has_baud())
            serial_cfg.set_baud(4800);
        // set default end of line
        if (!serial_cfg.has_end_of_line())
            serial_cfg.set_end_of_line("\r\n");
    }
};

int main(int argc, char* argv[]) { return goby::run<GPSDriver>(GPSDriverConfigurator(argc, argv)); }


//  (C) Copyright John Maddock and Steve Cleary 2000.
//  Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

//  See http://www.boost.org for most recent version including documentation.
//
//  macros and helpers for working with integral-constant-expressions.

#ifndef BOOST_ICE_TYPE_TRAITS_HPP
#define BOOST_ICE_TYPE_TRAITS_HPP

#ifndef BOOST_CONFIG_HPP
#include <boost/config.hpp>
#endif

namespace boost{
namespace type_traits{

typedef char yes_type;
typedef double no_type;

template <bool b>
struct ice_not
{ BOOST_STATIC_CONSTANT(bool, value = true); };
template <>
struct ice_not<true>
{ BOOST_STATIC_CONSTANT(bool, value = false); };

template <bool b1, bool b2, bool b3 = false, bool b4 = false, bool b5 = false, bool b6 = false, bool b7 = false>
struct ice_or;
template <bool b1, bool b2, bool b3, bool b4, bool b5, bool b6, bool b7>
struct ice_or
{
   BOOST_STATIC_CONSTANT(bool, value = true);
};
template <>
struct ice_or<false, false, false, false, false, false, false>
{
   BOOST_STATIC_CONSTANT(bool, value = false);
};

template <bool b1, bool b2, bool b3 = true, bool b4 = true, bool b5 = true, bool b6 = true, bool b7 = true>
struct ice_and;
template <bool b1, bool b2, bool b3, bool b4, bool b5, bool b6, bool b7>
struct ice_and
{
   BOOST_STATIC_CONSTANT(bool, value = false);
};
template <>
struct ice_and<true, true, true, true, true, true, true>
{
   BOOST_STATIC_CONSTANT(bool, value = true);
};

template <int b1, int b2>
struct ice_eq
{
   BOOST_STATIC_CONSTANT(bool, value = (b1 == b2));
};

template <int b1, int b2>
struct ice_ne
{
   BOOST_STATIC_CONSTANT(bool, value = (b1 != b2));
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
template <int b1, int b2>
const bool ice_eq<b1,b2>::value;
template <int b1, int b2>
const bool ice_ne<b1,b2>::value;
#endif

} // namespace type_traits

} // namespace boost

#endif // BOOST_ICE_TYPE_TRAITS_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/fms.hpp>
#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( fms,  BOOST_SIMD_TYPES)
{
  using boost::simd::fms;
  using boost::simd::tag::fms_;

  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<fms_(T,T,T)>::type, T);

  NT2_TEST_EQUAL(fms(T(2),T(2),T(4)) , T(0));
  NT2_TEST_EQUAL(fms(T(2),T(2),T(-4)), T(8));
  NT2_TEST_EQUAL(fms(T(2),T(-2),T(4)), T(-8));
  NT2_TEST_EQUAL(fms(T(-2),T(2),T(4)), T(-8));
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_ltz  _real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ltz;
  using r_t = decltype(is_ltz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_ltz(bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Minf<T>()), r_t(true));
  STF_EQUAL(is_ltz(bs::Nan<T>()), r_t(false));
#endif
  STF_EQUAL(is_ltz(bs::Mzero<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Half<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ltz(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Quarter<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_ltz _signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ltz;
   using r_t = decltype(is_ltz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ltz(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ltz(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_ltz _unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ltz;
  using r_t = decltype(is_ltz(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ltz(bs::One<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Two<T>()), r_t(false));
  STF_EQUAL(is_ltz(bs::Zero<T>()), r_t(false));
}

STF_CASE ( "is_ltz _bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ltz;
  using r_t = decltype(is_ltz(bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_ltz(true), false);
  STF_EQUAL(is_ltz(false), false);
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */

#include <iostream>
#include <string>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/point_cloud_color_handlers.h>

#include "rtDDPvMF/rtDDPvMF.hpp"
//#include "rtDDPvMF/realtimeDDPvMF_openni.hpp"
#include "bbTrans/lower_bound_S3.h"
#include "bbTrans/upper_bound_indep_S3.h"
#include "bbTrans/upper_bound_convex_S3.h"
#include "bbTrans/branch_and_bound.h"
#include "bbTrans/vmf_mm.h"
#include "bbTrans/node.h"
#include "dpOptTrans/dp_vmf_opt_rot.h"
#include "dpOptTrans/pc_helper.h"

#include <boost/program_options.hpp>
namespace po = boost::program_options;

bool ComputevMFMMfromDepth(
  shared_ptr<RtDDPvMF> pRtDDPvMF,
  const CfgRtDDPvMF& cfg,
    const std::string& path, bool display,
    cv::Mat *dI,
    cv::Mat *gI,
    std::vector<::bb::vMF<3>>* vmfs) {

  std::cout<<"reading depth image from "<<path<<std::endl;
  cv::Mat depth = cv::imread(path, CV_LOAD_IMAGE_ANYDEPTH);
  std::cout<<"type: "<<int(depth.type()) <<" != "<<int(CV_16U) <<std::endl;

  string pathRgb(path);
  pathRgb.replace(path.length()-5,1,"rgb");
  std::cout<<"reading rgb image from "<<pathRgb<<std::endl;
  cv::Mat gray = cv::imread(pathRgb, CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat rgb = cv::imread(pathRgb);
  *gI = gray;

  if (display) {
    cv::Mat dI(depth.rows,depth.cols,CV_8UC1);
    depth.convertTo(dI,CV_8UC1,255./4000.,-19.);
    cv::imshow("d",dI);
    cv::imshow("rgb",rgb);
    cv::waitKey(0);
  }
  cv::Mat nI;
  cv::Mat zI;
  cv::Mat Iout;
  MatrixXf centroids;
  VectorXf concentrations;
  VectorXf proportions;

//  uint32_t T = 10;
//  for(uint32_t i=0; i<T; ++i)
  pRtDDPvMF->compute(reinterpret_cast<uint16_t*>(depth.data),
      depth.cols,depth.rows);
  Iout = pRtDDPvMF->overlaySeg(rgb,false,true);
  *dI = pRtDDPvMF->smoothDepthImg();
  //      cv::Mat Iout = pRtDDPvMF->overlaySeg(gray);
  if(display) {
    nI = pRtDDPvMF->normalsImg();
    zI = pRtDDPvMF->labelsImg(true);
    cv::waitKey(0);
  }
  centroids = pRtDDPvMF->GetCentroids();
  const VectorXu z = pRtDDPvMF->labels();
  uint32_t K = pRtDDPvMF->GetK();
  proportions = pRtDDPvMF->GetCounts();
  proportions /= proportions.sum();
  std::cout << "counts and proportions: " << std::endl;
  std::cout << pRtDDPvMF->GetCounts().transpose() << " normalized: "
    << proportions.transpose() << std::endl;
  concentrations = VectorXf::Zero(K);

  cv::Mat normals = pRtDDPvMF->normalsImgRaw();
  depth = pRtDDPvMF->smoothDepth();
  MatrixXf xSum = MatrixXf::Zero(3,K);
  for (uint32_t i=0; i<normals.cols; ++i)
    for (uint32_t j=0; j<normals.rows; ++j)
      if(z(normals.cols*j +i) < K) {
        Eigen::Map<Matrix<float,3,1> > q(&(normals.at<cv::Vec3f>(j,i)[0]));
        float d = depth.at<float>(j,i);
        float scale = d/cfg.f_d;
        xSum.col(z(normals.cols*j +i)) += q*scale;
      }
  std::cout<< pRtDDPvMF->GetxSums() << " vs weighted: " << xSum << std::endl;
  for (uint32_t k = 0; k < K; ++k) {
    concentrations(k) = (xSum.col(k)).norm();
    std::cout << xSum.col(k).transpose() << " || " <<  xSum.col(k).norm() << std::endl;
  }

  if(display)  {
    cv::imshow("dS",*dI);
    cv::imshow("normals",nI);
    cv::imshow("zI",zI);
    cv::imshow("out",Iout);
    cv::waitKey(0);
  }
  *dI = depth;

  for(uint32_t k=0; k<centroids.cols(); ++k) {
    vmfs->push_back(::bb::vMF<3>(centroids.col(k).cast<double>(),
          concentrations(k), proportions(k)));
  }
  return true;
};

pcl::PointCloud<pcl::PointXYZI>::Ptr DepthImageToPc(const cv::Mat& dI, const cv::Mat& gI, float f_d, const Eigen::Quaterniond q) {
  pcl::PointCloud<pcl::PointXYZI>::Ptr pc(new
      pcl::PointCloud<pcl::PointXYZI>(dI.cols, dI.rows));
  for (uint32_t i=0; i<dI.rows; ++i)
    for (uint32_t j=0; j<dI.cols; ++j) {
//      std::cout << dI.at<float>(i,j) << " ";
      pc->at(j,i).intensity = static_cast<float>(gI.at<uint8_t>(i,j))/255.;
//      std::cout << pc->at(j,i).intensity << " " << uint32_t(gI.at<uint8_t>(i,j)) << "; " ;
      pc->at(j,i).z = dI.at<float>(i,j);
      pc->at(j,i).x = (j-float(dI.cols-1.)*0.5) * pc->at(j,i).z / f_d;
      pc->at(j,i).y = (i-float(dI.rows-1.)*0.5) * pc->at(j,i).z / f_d;
      Eigen::Map<Matrix<float,3,1>> p(&(pc->at(j,i).x));
      p = q._transformVector(p.cast<double>()).cast<float>();
    }
  pc->is_dense = false;
  return pc;
}

int main(int argc, char** argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("lambdaDeg,l", po::value<double>(), "lambda in degree for dp and ddp")
    ("in_a,a", po::value<string>(), "path to first input file")
    ("in_b,b", po::value<string>(), "path to second input file")
    ("out,o", po::value<string>(), "path to output file")
    ("display,d", "display results")
    ("B,B", po::value<int>(), "B for guided filter")
    ("eps", po::value<float>(), "eps for guided filter")
    ("f_d,f", po::value<float>(), "focal length of depth camera")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  CfgRtDDPvMF cfg;
  cfg.f_d = 540.;
  cfg.beta = 1e5;
  cfg.nFramesSurvive_ = 1; // DPvMFMM
  cfg.pathOut = std::string("../results/");
  double lambdaDeg = 93.;
  if(vm.count("lambdaDeg")) lambdaDeg = vm["lambdaDeg"].as<double>();

  cfg.lambdaFromDeg(lambdaDeg);
  cfg.QfromFrames2Survive(cfg.nFramesSurvive_);

  string pathA = "";
  string pathB = "";
  //  string mode = "";
  cudaPcl::CfgSmoothNormals cfgNormals;
  cfgNormals.f_d = 540.;
  cfgNormals.eps = 0.2*0.2;
  cfgNormals.B = 9;
  cfgNormals.compress = true;
  //  if(vm.count("mode")) mode = vm["mode"].as<string>();
  if(vm.count("in_a")) pathA = vm["in_a"].as<string>();
  if(vm.count("in_b")) pathB = vm["in_b"].as<string>();
  if(vm.count("eps")) cfgNormals.eps = vm["eps"].as<float>();
  if(vm.count("f_d")) cfgNormals.f_d = vm["f_d"].as<float>();
  if(vm.count("B")) cfgNormals.B = uint32_t( vm["B"].as<int>());

  findCudaDevice(argc,(const char**)argv);

  std::cout<<"rtDDPvMFmeans lambdaDeg="<<cfg.lambdaDeg_<<" beta="<<cfg.beta
    <<"nFramesSurvive="<<cfg.nFramesSurvive_<<std::endl;
  std::cout<<"output path: "<<cfg.pathOut<<std::endl;

  std::vector<bb::vMF<3>> vmfs_A;

  cv::Mat dIa, gIa;
  shared_ptr<RtDDPvMF> pRtDDPvMF_A;
  pRtDDPvMF_A = shared_ptr<RtDDPvMF>(new RtDDPvMF(cfg,cfgNormals));
  ComputevMFMMfromDepth(pRtDDPvMF_A, cfg, pathA, vm.count("display"), &dIa, &gIa, &vmfs_A);
  bb::vMFMM<3> vmf_mm_A(vmfs_A);

  cv::Mat dIb, gIb;
  std::vector<bb::vMF<3>> vmfs_B;
  shared_ptr<RtDDPvMF> pRtDDPvMF_B;
  pRtDDPvMF_B = shared_ptr<RtDDPvMF>(new RtDDPvMF(cfg,cfgNormals));
  ComputevMFMMfromDepth(pRtDDPvMF_B, cfg, pathB, vm.count("display"), &dIb, &gIb, &vmfs_B);
  bb::vMFMM<3> vmf_mm_B(vmfs_B);

  std::cout << "A:" << std::endl;
  for (uint32_t i=0; i<vmf_mm_A.GetK(); ++i)
    std::cout << vmf_mm_A.Get(i).GetMu().transpose()
      << " tau " << vmf_mm_A.Get(i).GetTau()
      << " pi " << vmf_mm_A.Get(i).GetPi() << std::endl;

  std::cout << "B:" << std::endl;
  for (uint32_t i=0; i<vmf_mm_B.GetK(); ++i)
    std::cout << vmf_mm_B.Get(i).GetMu().transpose()
      << " tau " << vmf_mm_B.Get(i).GetTau()
      << " pi " << vmf_mm_B.Get(i).GetPi() << std::endl;

  std::list<bb::NodeS3> nodes = bb::GenerateNotesThatTessellateS3();
  bb::LowerBoundS3 lower_bound(vmf_mm_A, vmf_mm_B);
  bb::UpperBoundIndepS3 upper_bound(vmf_mm_A, vmf_mm_B);
  bb::UpperBoundConvexS3 upper_bound_convexity(vmf_mm_A, vmf_mm_B);

  bb::BranchAndBound<bb::NodeS3> bb(lower_bound, upper_bound_convexity);
  double eps = 1e-4 / 180. * M_PI;
  uint32_t max_lvl = 60;
  uint32_t max_it = 300;
  bb::NodeS3 node_star = bb.Compute(nodes, eps, max_lvl, max_it);

  std::cout << "optimum quaternion: "
    << node_star.GetTetrahedron().GetCenter().transpose()
    << std::endl;

  Eigen::Quaterniond q0;
  Eigen::Quaterniond qStar = node_star.GetTetrahedron().GetCenterQuaternion();
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcA(DepthImageToPc(dIa, gIa, cfg.f_d, q0));
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcB(DepthImageToPc(dIb, gIb, cfg.f_d, qStar));
  std::cout << pcA->width << " " << pcA->height << std::endl;
  std::cout << pcB->width << " " << pcB->height << std::endl;

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->initCameraParameters ();
//  int v1(0);
//  viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
  viewer->setBackgroundColor (0, 0, 0);
//  viewer_->setBackgroundColor (255, 255, 255);
  viewer->addCoordinateSystem (1.0);
  pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> rgbA(pcA,"intensity");
  pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> rgbB(pcB,"intensity");
  viewer->addPointCloud<pcl::PointXYZI> (pcA, rgbA, "sample cloud1");
  viewer->addPointCloud<pcl::PointXYZI> (pcB, rgbB, "sample cloud2");

  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  if(vm.count("out"))
  {
    std::cout<<" writing out put to "<<std::endl
      <<(vm["out"].as<string>()+"_rgbLabels.png")<<std::endl
      <<vm["out"].as<string>()+"_cRmf.csv"<<std::endl;
//    cv::imwrite(vm["out"].as<string>()+"_rgbLabels.png",Iout);
//    ofstream out((vm["out"].as<string>()+"_cRmf.csv").data(),
//        ofstream::out);
//    for(uint32_t i=0; i<centroids.rows();++i)
//    {
//      for(uint32_t j=0; j<centroids.cols()-1;++j)
//        out << centroids(i,j)<<" ";
//      out << centroids(i,centroids.cols()-1)<<std::endl;
//    }
//    for(uint32_t i=0; i<concentrations.size()-1;++i)
//      out << concentrations(i) << " ";
//    out << concentrations(concentrations.size()-1) << std::endl;
//    for(uint32_t i=0; i<proportions.size()-1;++i)
//      out << proportions(i) << " ";
//    out << proportions(proportions.size()-1) << std::endl;
//    out.close();
  }
  std::cout<<cudaDeviceReset()<<std::endl;
  return (0);
}


/*
 * scan.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/python/slice.hpp>
#include <string>
#include <sstream>
#include <scitbx/constants.h>
#include <dxtbx/model/scan.h>
#include <boost/operators.hpp>
#include <dxtbx/model/boost_python/to_from_dict.h>

namespace dxtbx { namespace model { namespace boost_python {

  using namespace boost::python;
  using scitbx::deg_as_rad;
  using scitbx::rad_as_deg;

  static
  vec2<double> rad_as_deg(vec2<double> angles) {
    angles[0] = rad_as_deg(angles[0]);
    angles[1] = rad_as_deg(angles[1]);
    return angles;
  }

  static
  vec2<double> deg_as_rad(vec2<double> angles) {
    angles[0] = deg_as_rad(angles[0]);
    angles[1] = deg_as_rad(angles[1]);
    return angles;
  }

  std::string scan_to_string(const Scan &scan) {
    std::stringstream ss;
    ss << scan;
    return ss.str();
  }

  struct ScanPickleSuite : boost::python::pickle_suite {
    static
    boost::python::tuple getinitargs(const Scan &obj) {
      return boost::python::make_tuple(
        obj.get_image_range(),
        rad_as_deg(obj.get_oscillation()),
        obj.get_exposure_times(),
        obj.get_epochs(),
        obj.get_batch_offset());
    }
  };

  boost::python::dict MaptoPythonDict(ExpImgRangeMap map) {
    ExpImgRangeMap::iterator iter;
    boost::python::dict dictionary;
    for (iter = map.begin(); iter != map.end(); ++iter) {
      scitbx::af::shared<vec2<int> > val = iter->second;
      boost::python::list result;
      for (int k=0; k < val.size(); ++k){
        result.append(val[k]);
      }
      dictionary[iter->first] = result;
    }
    return dictionary;
  }

  template <>
  boost::python::dict to_dict<Scan>(const Scan &obj) {
    boost::python::dict result;
    result["image_range"] = obj.get_image_range();
    result["batch_offset"] = obj.get_batch_offset();
    result["oscillation"] = rad_as_deg(obj.get_oscillation());
    result["exposure_time"] = boost::python::list(obj.get_exposure_times());
    result["epochs"] = boost::python::list(obj.get_epochs());
    boost::python::dict valid_image_ranges = MaptoPythonDict(obj.get_valid_image_ranges_map());
    result["valid_image_ranges"] = valid_image_ranges;
    return result;
  }

  inline
  scitbx::af::shared<double> make_exposure_times(
    std::size_t num, boost::python::list obj) {
    scitbx::af::shared<double> result((scitbx::af::reserve(num)));
    std::size_t nl = boost::python::len(obj);
    DXTBX_ASSERT(num > 0 && nl <= num);
    if (nl == 0) {
      result.push_back(0.0);
      nl = 1;
    } else {
      for (std::size_t i = 0; i < nl; ++i) {
        result.push_back(boost::python::extract<double>(obj[i]));
      }
    }
    for (std::size_t i = nl; i < num; ++i) {
      result.push_back(result.back());
    }
    return result;
  }

  inline
  scitbx::af::shared<double> make_epochs(
    std::size_t num, boost::python::list obj) {
    scitbx::af::shared<double> result((scitbx::af::reserve(num)));
    std::size_t nl = boost::python::len(obj);
    DXTBX_ASSERT(num > 0 && nl <= num);
    if (nl == 0) {
      for (std::size_t i = 0; i < num; ++i) {
        result.push_back(0.0);
      }
    } else if (nl == 1) {
      for (std::size_t i = 0; i < num; ++i) {
        result.push_back(boost::python::extract<double>(obj[0]));
      }
    } else if (nl < num) {
      for (std::size_t i = 0; i < nl; ++i) {
        result.push_back(boost::python::extract<double>(obj[i]));
      }
      double e0 = result[result.size()-1];
      double de = e0 - result[result.size()-2];
      for (std::size_t i = 0; i < num-nl; ++i) {
        result.push_back(e0 + (i+1) * de);
      }
    } else {
      for (std::size_t i = 0; i < num; ++i) {
        result.push_back(boost::python::extract<double>(obj[i]));
      }
    }
    return result;
  }

  template <>
  Scan* from_dict<Scan>(boost::python::dict obj) {
    vec2<int> ir = boost::python::extract< vec2<int> >(obj["image_range"]);
    std::size_t bo= boost::python::extract< std::size_t >(obj["batch_offset"]);
    vec2<double> osc = deg_as_rad(
      boost::python::extract< vec2<double> >(obj["oscillation"]));
    DXTBX_ASSERT(ir[1] >= ir[0]);
    std::size_t num = ir[1] - ir[0] + 1;
    Scan*  scan = new Scan(ir, osc,
      make_exposure_times(num,
        boost::python::extract<boost::python::list>(
          obj.get("exposure_time", boost::python::list()))),
      make_epochs(num,
        boost::python::extract<boost::python::list>(
          obj.get("epochs", boost::python::list()))),
      bo);
    boost::python::dict rangemap = boost::python::extract<boost::python::dict >(
      obj["valid_image_ranges"]);
    boost::python::list keys = rangemap.keys();
    boost::python::list values = rangemap.values();
    for (int i = 0; i < len(keys); ++i) {
      boost::python::extract<std::string> extracted_key(keys[i]);
      scitbx::af::shared<vec2<int> > result;
      int n_tuples = boost::python::len(values[i]);
      for (int n = 0; n < n_tuples; ++n){
        result.push_back(boost::python::extract<vec2<int> >(values[i][n]));
      }
      std::string key = extracted_key;
      scan->set_valid_image_ranges_array(key, result);
    }
    return scan;
  }

  static
  void set_valid_image_ranges(Scan &scan, std::string i, boost::python::list obj){
    int n = boost::python::len(obj);
    scitbx::af::shared<vec2 <int> > ranges;
    for (int k = 0; k < n; ++k){
      ranges.push_back(boost_python::extract<vec2 <int> >(obj[k]));
    }
    scan.set_valid_image_ranges_array(i, ranges);
  }

  static
  boost::python::list get_valid_image_ranges(Scan &scan, std::string i) {
    scitbx::af::shared<vec2<int> > ranges = scan.get_valid_image_ranges_key(i);
    boost::python::list result;
    if (ranges.size() != 0){
      for (int k = 0; k < ranges.size(); ++k){
        result.append(ranges[k]);
      }
    }
    return result;
  }

  static Scan* make_scan(vec2 <int> image_range, vec2 <double> oscillation,
                         int batch_offset, bool deg) {
    Scan *scan = NULL;
    if (deg) {
      scan = new Scan(image_range,
        vec2 <double> (
          deg_as_rad(oscillation[0]),
          deg_as_rad(oscillation[1])),
        batch_offset);
    } else {
      scan = new Scan(image_range, oscillation, batch_offset);
    }
    return scan;
  }

  static Scan* make_scan_w_epoch(vec2 <int> image_range,
      vec2 <double> oscillation,
      const scitbx::af::shared<double> &exposure_times,
      const scitbx::af::shared<double> &epochs,
      int batch_offset, bool deg) {
    Scan *scan = NULL;
    if (deg) {
      scan = new Scan(image_range,
        vec2 <double> (
          deg_as_rad(oscillation[0]),
          deg_as_rad(oscillation[1])),
        exposure_times, epochs,
        batch_offset);
    } else {
      scan = new Scan(image_range, oscillation, exposure_times, epochs,
                      batch_offset);
    }
    return scan;
  }

  static
  vec2<double> get_oscillation_range(const Scan &scan, bool deg) {
    vec2<double> range = scan.get_oscillation_range();
    return deg ? rad_as_deg(range) : range;
  }

  static
  vec2<double> get_oscillation(const Scan &scan, bool deg) {
    vec2<double> oscillation = scan.get_oscillation();
    return deg ? rad_as_deg(oscillation) : oscillation;
  }

  static
  void set_oscillation(Scan &scan, vec2<double> oscillation,
      bool deg) {
    if (deg) {
      oscillation = deg_as_rad(oscillation);
    }
    scan.set_oscillation(oscillation);
  }

   static
  vec2<double> get_image_oscillation(const Scan &scan, int image,
      bool deg) {
    vec2<double> oscillation = scan.get_image_oscillation(image);
    return deg ? rad_as_deg(oscillation) : oscillation;
  }

  static
  bool is_angle_valid(const Scan &scan, double angle, bool deg) {
    return scan.is_angle_valid(deg ? deg_as_rad(angle) : angle);
  }

  static
  scitbx::af::shared<bool> is_angle_valid_array(const Scan &scan, scitbx::af::const_ref<double> angle, bool deg) {
    scitbx::af::shared<bool> result(angle.size());
    for (std::size_t i = 0; i < angle.size(); ++i) {
      result[i] = scan.is_angle_valid(deg ? deg_as_rad(angle[i]) : angle[i]);
    }
    return result;
  }

  static
  double get_angle_from_image_index(const Scan &scan, double index,
      bool deg) {
    double angle = scan.get_angle_from_image_index(index);
    return deg ? rad_as_deg(angle) : angle;
  }

  static
  double get_angle_from_array_index(const Scan &scan, double index,
      bool deg) {
    double angle = scan.get_angle_from_array_index(index);
    return deg ? rad_as_deg(angle) : angle;
  }

  static
  scitbx::af::shared<double> get_angle_from_array_index_multiple(
      const Scan &scan, scitbx::af::const_ref<double> const &index,
      bool deg) {
    scitbx::af::shared<double> result((scitbx::af::reserve(index.size())));
    for(std::size_t i = 0; i < index.size(); i++) {
      result.push_back(get_angle_from_array_index(scan, index[i], deg));
    }
    return result;
  }

  static
  double get_image_index_from_angle(const Scan &scan, double angle,
      bool deg) {
    return scan.get_image_index_from_angle(deg ? deg_as_rad(angle) : angle);
  }

  static
  double get_array_index_from_angle(const Scan &scan, double angle,
      bool deg) {
    return scan.get_array_index_from_angle(
      deg ? deg_as_rad(angle) : angle);
  }

  static
  scitbx::af::shared<double> get_array_index_from_angle_multiple(
      const Scan &scan, scitbx::af::const_ref<double> const &angle,
      bool deg) {
    scitbx::af::shared<double> result((scitbx::af::reserve(angle.size())));
    for(std::size_t i = 0; i < angle.size(); i++) {
      result.push_back(get_array_index_from_angle(scan, angle[i], deg));
    }
    return result;
  }

  static
  scitbx::af::shared< vec2<double> > get_image_indices_with_angle(
      const Scan &scan, double angle, bool deg) {
    return scan.get_image_indices_with_angle(deg ? deg_as_rad(angle) : angle);
  }

  static
  scitbx::af::shared< vec2<double> > get_array_indices_with_angle(
      const Scan &scan, double angle, bool deg) {
    return scan.get_array_indices_with_angle(
      deg ? deg_as_rad(angle) : angle);
  }

  static
  Scan getitem_single(const Scan &scan, int index)
  {
    return scan[index];
  }

  static
  Scan getitem_slice(const Scan &scan, const slice index)
  {
    // Ensure no step
    DXTBX_ASSERT(index.step() == object());

    // Get start index
    int start = 0, stop = 0;
    if (index.start() == object()) {
      start = 0;
    } else {
      start = extract<int>(index.start());
    }

    // Get stop index
    if (index.stop() == object()) {
      stop = scan.get_num_images();
    } else {
      stop = extract<int>(index.stop());
    }

    // Check ranges
    DXTBX_ASSERT(start >= 0);
    DXTBX_ASSERT(stop <= scan.get_num_images());
    DXTBX_ASSERT(start < stop);

    // Create the new epoch array
    int first_image_index = scan.get_image_range()[0] + start;
    int last_image_index = scan.get_image_range()[0] + stop - 1;
    scitbx::af::shared<double> new_epochs(stop - start);
    for (std::size_t i = 0; i < new_epochs.size(); ++i) {
      new_epochs[i] = scan.get_image_epoch(first_image_index + i);
    }

    // Create the new epoch array
    scitbx::af::shared<double> new_exposure_times(stop - start);
    for (std::size_t i = 0; i < new_exposure_times.size(); ++i) {
      new_exposure_times[i] = scan.get_image_exposure_time(first_image_index + i);
    }

    // Create the new scan object
    return Scan(
      vec2<int>(first_image_index, last_image_index),
      scan.get_image_oscillation(first_image_index),
      new_exposure_times, new_epochs, scan.get_batch_offset());
  }

  void scan_swap(Scan &lhs, Scan &rhs) {
    std::swap(lhs, rhs);
  }

  void export_scan()
  {
    // Export ScanBase
    class_ <ScanBase> ("ScanBase");

    // Export Scan : ScanBase
    class_ <Scan, boost::shared_ptr<Scan>, bases <ScanBase> > ("Scan")
      .def(init<const Scan&>())
      .def("__init__",
          make_constructor(
          &make_scan,
          default_call_policies(), (
          arg("image_range"),
          arg("oscillation"),
          arg("batch_offset") = 0,
          arg("deg") = true)))
      .def("__init__",
          make_constructor(
          &make_scan_w_epoch,
          default_call_policies(), (
          arg("image_range"),
          arg("oscillation"),
          arg("exposure_times"),
          arg("epochs"),
          arg("batch_offset") = 0,
          arg("deg") = true)))
      .def("get_image_range",
        &Scan::get_image_range)
      .def("get_valid_image_ranges",
        get_valid_image_ranges)
      .def("set_valid_image_ranges",
        set_valid_image_ranges)
      .def("set_image_range",
        &Scan::set_image_range)
      .def("get_batch_offset",
        &Scan::get_batch_offset)
      .def("set_batch_offset",
        &Scan::set_batch_offset)
      .def("get_batch_for_image_index",
        &Scan::get_batch_for_image_index)
      .def("get_batch_for_array_index",
        &Scan::get_batch_for_array_index)
      .def("get_batch_range",
        &Scan::get_batch_range)
      .def("get_array_range",
        &Scan::get_array_range)
      .def("get_oscillation",
        &get_oscillation, (
          arg("deg") = true))
      .def("set_oscillation",
        &set_oscillation, (
          arg("deg") = true))
      .def("get_exposure_times",
        &Scan::get_exposure_times)
      .def("set_exposure_times",
        &Scan::set_exposure_times)
      .def("get_epochs",
        &Scan::get_epochs)
      .def("set_epochs",
        &Scan::set_epochs)
      .def("get_num_images",
        &Scan::get_num_images)
      .def("get_image_oscillation",
        &get_image_oscillation, (
          arg("index"),
          arg("deg") = true))
      .def("get_image_epoch",
        &Scan::get_image_epoch, (
          arg("index")))
      .def("get_oscillation_range",
        &get_oscillation_range, (
          arg("deg") = true))
      .def("is_angle_valid",
        &is_angle_valid, (
          arg("angle"),
          arg("deg") = true))
      .def("is_angle_valid",
        &is_angle_valid_array, (
          arg("angle"),
          arg("deg") = true))
      .def("is_image_index_valid",
        &Scan::is_image_index_valid, (
          arg("index")))
      .def("is_array_index_valid",
        &Scan::is_array_index_valid, (
          arg("index")))
      .def("is_batch_valid",
        &Scan::is_batch_valid, (
          arg("batch")))
      .def("get_angle_from_image_index",
        &get_angle_from_image_index, (
          arg("index"),
          arg("deg") = true))
      .def("get_angle_from_array_index",
        &get_angle_from_array_index, (
          arg("index"),
          arg("deg") = true))
      .def("get_angle_from_array_index",
        &get_angle_from_array_index_multiple, (
          arg("index"),
          arg("deg") = true))
      .def("get_image_index_from_angle",
        &get_image_index_from_angle, (
          arg("angle"),
          arg("deg") = true))
      .def("get_array_index_from_angle",
        &get_array_index_from_angle, (
          arg("angle"),
          arg("deg") = true))
      .def("get_array_index_from_angle",
        &get_array_index_from_angle_multiple, (
          arg("angle"),
          arg("deg") = true))
      .def("get_image_indices_with_angle",
        &get_image_indices_with_angle, (
          arg("angle"),
          arg("deg") = true))
      .def("get_array_indices_with_angle",
        &get_array_indices_with_angle, (
          arg("angle"),
          arg("deg") = true))
      .def("__getitem__", &getitem_single)
      .def("__getitem__", &getitem_slice)
      .def(self == self)
      .def(self != self)
      .def(self < self)
      .def(self <= self)
      .def(self > self)
      .def(self >= self)
      .def(self += self)
      .def(self + self)
      .def("append", &Scan::append, (
            arg("rhs"),
            arg("scan_tolerance")=0.01))
      .def("__len__", &Scan::get_num_images)
      .def("__str__", &scan_to_string)
      .def("swap", &scan_swap)
      .def("to_dict", &to_dict<Scan>)
      .def("from_dict", &from_dict<Scan>,
        return_value_policy<manage_new_object>())
      .staticmethod("from_dict")
      .def_pickle(ScanPickleSuite());
  }

}}} // namespace = dxtbx::model::boost_python


#ifndef ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP
#define ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP

#include <aslam/backend/DesignVariableMappedVector.hpp>
#include <bsplines/BSplinePose.hpp>
#include <Eigen/StdVector>
#include <boost/ptr_container/ptr_vector.hpp>
#include <aslam/backend/TransformationExpression.hpp>
#include <aslam/backend/RotationExpression.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/VectorExpression.hpp>
#include <aslam/backend/ScalarExpression.hpp>

namespace aslam {
    namespace splines {

        class BSplinePoseDesignVariable
        {
        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            typedef aslam::backend::DesignVariableMappedVector<6> dv_t;

            /// \brief this guy takes a copy.
            BSplinePoseDesignVariable(const bsplines::BSplinePose & bsplinePose);

            virtual ~BSplinePoseDesignVariable();

            /// \brief get the spline.
            const bsplines::BSplinePose & spline();

            // \todo Return a Transformation expression, a Rotation expression, A Euclidean point expression, and lots of VectorExpressions.
            aslam::backend::TransformationExpression transformation(double tk);
            aslam::backend::RotationExpression orientation(double tk);
            aslam::backend::EuclideanExpression position(double tk);

            // Get a transformation expression where the time may change.
            aslam::backend::TransformationExpression transformationAtTime(const aslam::backend::ScalarExpression & time, double leftBuffer, double rightBuffer);
            aslam::backend::TransformationExpression transformationAtTime(const aslam::backend::ScalarExpression & time);

            // Fabio:
            aslam::backend::EuclideanExpression linearVelocity(double tk);
            aslam::backend::EuclideanExpression linearAcceleration(double tk);
            aslam::backend::EuclideanExpression angularVelocityBodyFrame(double tk);
            // Fabio:
            //aslam::backend::EuclideanExpression angularAccelerationBodyFrame(double tk);

      aslam::backend::EuclideanExpression linearAccelerationBodyFrame(double tk);

            size_t numDesignVariables();
            aslam::backend::DesignVariableMappedVector<6> * designVariable(size_t i);

            // Luc:
            Eigen::VectorXi getActiveDesignVariableIndices(double tk);
            // Fabio:
            std::vector<aslam::backend::DesignVariable *> getDesignVariables(double tk);

            // Fabio:
            // add one Segment at the end of the PoseSpline
            void addSegment(double t, Eigen::Matrix4d T);
            void addSegment2(double t, Eigen::Matrix4d T, double lambda);
            void removeSegment();

        private:
            /// \brief the internal spline.
            bsplines::BSplinePose _bsplinePose;

            /// \brief the vector of design variables.
            boost::ptr_vector< dv_t > _designVariables;

        };

    } // namespace splines
} // namespace aslam

#endif /* ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP */


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_IS_POSITIVE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_POSITIVE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is positive else @ref False.

    This function differs from @ref is_gtz for floating point arguments,
    because @ref Zero is positive but not greater than 0, and @ref Mzero is
    not positive and not greater than 0, It's probably @ref is_gtz that
    you want.

    @par Header <boost/simd/function/is_positive.hpp>

    @par Note

     Using `is_positive(x)` is similar to `bitofsign(x) == 0`
     for signed types and is always true for unsigned ones.

    @par Example:

      @snippet is_positive.cpp is_positive

    @par Possible output:

      @snippet is_positive.txt is_positive

  **/
  as_logical_t<Value> is_positive(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_positive.hpp>
#include <boost/simd/function/simd/is_positive.hpp>

#endif


#ifndef STAN_MCMC_HMC_HAMILTONIANS_DENSE_E_METRIC_HPP
#define STAN_MCMC_HMC_HAMILTONIANS_DENSE_E_METRIC_HPP

#include <stan/services/callbacks/logger.hpp>
#include <stan/math/prim/mat.hpp>
#include "base_hamiltonian.hpp"
#include "dense_e_point.hpp"
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <Eigen/Cholesky>

namespace stan {
  namespace mcmc {

    // Euclidean manifold with dense metric
    template <class Model, class BaseRNG>
    class dense_e_metric
      : public base_hamiltonian<Model, dense_e_point, BaseRNG> {
    public:
      explicit dense_e_metric(const Model& model)
        : base_hamiltonian<Model, dense_e_point, BaseRNG>(model) {}

      double T(dense_e_point& z) {
        return 0.5 * z.p.transpose() * z.inv_e_metric_ * z.p;
      }

      double tau(dense_e_point& z) {
        return T(z);
      }

      double phi(dense_e_point& z) {
        return this->V(z);
      }

      double dG_dt(dense_e_point& z, callbacks::logger& logger) {
        return 2 * T(z) - z.q.dot(z.g);
      }

      Eigen::VectorXd dtau_dq(dense_e_point& z, callbacks::logger& logger) {
        return Eigen::VectorXd::Zero(this->model_.num_params_r());
      }

      Eigen::VectorXd dtau_dp(dense_e_point& z) {
        return z.inv_e_metric_ * z.p;
      }

      Eigen::VectorXd dphi_dq(dense_e_point& z, callbacks::logger& logger) {
        return z.g;
      }

      void sample_p(dense_e_point& z, BaseRNG& rng) {
        typedef typename stan::math::index_type<Eigen::VectorXd>::type idx_t;
        boost::variate_generator<BaseRNG&, boost::normal_distribution<> >
          rand_dense_gaus(rng, boost::normal_distribution<>());

        Eigen::VectorXd u(z.p.size());

        for (idx_t i = 0; i < u.size(); ++i)
          u(i) = rand_dense_gaus();

        z.p = z.inv_e_metric_.llt().matrixU().solve(u);
      }
    };

  }  // mcmc
}  // stan
#endif


//--------------------------------------------------------------------
//
// LinearRegression.cpp
//
// Created on: Nov 27, 2017
// Author: Max
//
//--------------------------------------------------------------------

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <fstream>

#include <Launcher.h>
#include <LinearRegression.h>

static const char COMMENT_CHAR = '#';
static const char NUMBER_SEPARATOR_CHAR = ',';
// static const char PARAMETER_SEPARATOR_CHAR = ' ';
// static const char ATTRIBUTE_SEPARATOR_CHAR = ',';
static const char ATTRIBUTE_NAME_CHAR = ':';

using namespace std;

using namespace multifaq::params;
using namespace multifaq::dir;
using namespace multifaq::config;

namespace phoenix = boost::phoenix;
using namespace boost::spirit;

LinearRegression::LinearRegression(shared_ptr<Launcher> launcher) :
{
    _compiler = launcher->getCompiler();
    _td = launcher->getTreeDecomposition();
}

LinearRegression::~LinearRegression()
{
    delete[] _queryRootIndex;
}

void LinearRegression::run()
{
    loadFeatures();
    modelToQueries();
    _compiler->compile();
}

void LinearRegression::generateCode()
{}

// var_bitset LinearRegression::getFeatures()
// {
//     cout << _features << endl;

//     return _features;
// }

var_bitset LinearRegression::getCategoricalFeatures()
{
    return _categoricalFeatures;
}

void LinearRegression::modelToQueries()
{

    size_t numberOfFunctions = 0;
    size_t featureToFunction[NUM_OF_VARIABLES],
        secondaryFeatureToFunction[NUM_OF_VARIABLES];

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var))
        {
            if (_categoricalFeatures.test(var))
            {
                // TODO: TODO: TODO: Need to set the parameter here correctly!
                double* parameter = new double[1];
                parameter[0] = 0.0;
                _compiler->addFunction(
                    new Function({var}, Operation::lr_cat_parameter, parameter));
            }
            else
            {
                double* parameter = new double[1];
                parameter[0] = 0.0;

                _compiler->addFunction(
                    new Function({var}, Operation::lr_cont_parameter, parameter));
            }

            featureToFunction[var] = numberOfFunctions;
            numberOfFunctions++;
        }
    }

    size_t numberOfFeatures = _features.count();
    numberOfFunctions = 0;

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var) && !_categoricalFeatures.test(var))
        {
            _compiler->addFunction(new Function({var}, Operation::sum));
            secondaryFeatureToFunction[var] = numberOfFeatures + numberOfFunctions;
            numberOfFunctions++;
        }
    }

    for (size_t var = 0; var < NUM_OF_VARIABLES; ++var)
    {
        if (_features.test(var))
        {
            // Aggregate* agg = new Aggregate(1);
            // agg->_m[0] = 0;
            Aggregate* agg = new Aggregate();

            for (size_t otherVar = 0; otherVar < NUM_OF_VARIABLES; ++otherVar)
            {
                if (_features.test(otherVar))
                {
                    prod_bitset product;
                    product.set(featureToFunction[otherVar]);

                    if (!_categoricalFeatures.test(var))
                        product.set(secondaryFeatureToFunction[var]);

                    agg->_agg.push_back(product);

                    // TODO: Make sure this is correct!?!
                    // ++agg->_m[0];
                }
            }

            Query* query = new Query();
            query->_aggregates.push_back(agg);

            if (_categoricalFeatures[var])
            {
                query->_fVars.set(var);
                query->_rootID = _queryRootIndex[var];
            }
            else
            {
                query->_rootID = _td->_root->_id;
                // query->_rootID = 3;
            }

            _compiler->addQuery(query);
        }
    }
}

void LinearRegression::loadFeatures()
{
    _queryRootIndex = new size_t[NUM_OF_VARIABLES]();

    /* Load the two-pass variables config file into an input stream. */
    ifstream input(FEATURE_CONF);

    if (!input)
    {
        ERROR(FEATURE_CONF + " does not exist. \n");
        exit(1);
    }

    /* String and associated stream to receive lines from the file. */
    string line;
    stringstream ssLine;

    int numOfFeatures = 0;
    int degreeOfInteractions = 0;

    /* Ignore comment and empty lines at the top */
    while (getline(input, line))
    {
        if (line[0] == COMMENT_CHAR || line == "")
            continue;

        break;
    }

    /*
     * Extract number of labels, features and interactions from the config.
     * Parse the line with the three numbers; ignore spaces.
     */
    bool parsingSuccess =
        qi::phrase_parse(line.begin(), line.end(),

                         /* Begin Boost Spirit grammar. */
                         ((qi::int_[phoenix::ref(numOfFeatures) = qi::_1])
                          >> NUMBER_SEPARATOR_CHAR
                          >> (qi::int_[phoenix::ref(degreeOfInteractions) =
                                       qi::_1])),
                         /*  End grammar. */
                          ascii::space);

    assert(parsingSuccess && "The parsing of the features file has failed.");

    // std::vector<int> linearContinuousFeatures;
    // std::vector<int> linearCategoricalFeatures;

    /* Read in the features. */
    for (int featureNo = 0; featureNo < numOfFeatures; ++featureNo)
    {
        getline(input, line);
        if (line[0] == COMMENT_CHAR || line == "")
        {
            --featureNo;
            continue;
        }

        ssLine << line;

        string attrName;
        /* Extract the name of the attribute in the current line. */
        getline(ssLine, attrName, ATTRIBUTE_NAME_CHAR);

        string typeOfFeature;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, typeOfFeature, ATTRIBUTE_NAME_CHAR);

        string rootName;
        /* Extract the dimension of the current attribute. */
        getline(ssLine, rootName, ATTRIBUTE_NAME_CHAR);

        int attributeID = _td->getAttributeIndex(attrName);
        int categorical = stoi(typeOfFeature);
        int rootID = _td->getRelationIndex(rootName);

        if (attributeID == -1)
        {
            ERROR("Attribute |"+attrName+"| does not exist.");
            exit(1);
        }

        if (rootID == -1)
        {
            ERROR("Relation |"+rootName+"| does not exist.");
            exit(1);
        }

        if (featureNo == 0 && categorical == 1)
        {
            ERROR("The label needs to be continuous! ");
            exit(1);
        }

        _features.set(attributeID);

        if (categorical)
            _categoricalFeatures.set(attributeID);

        _queryRootIndex[attributeID] = rootID;

        /* Clear string stream. */
        ssLine.clear();
    }
}

void LinearRegression::computeGradient()
{
    _compiler->compile();
}


#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/list.hpp>
#include <scitbx/vec2.h>
#include <scitbx/vec3.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/shared.h>
#include <dxtbx/model/detector.h>
#include <dxtbx/model/panel.h>
#include <math.h>
#include <vector>

namespace kapton {

using dxtbx::model::Detector;
using scitbx::vec2;
using scitbx::vec3;

/**
 * Implementing a c++ version of the get_kapton_path function. Significant speedups
 * observed
 */

scitbx::af::shared<double> get_kapton_path_cpp(
  boost::python::list kapton_faces,
  scitbx::af::const_ref<vec3<double> > s1_flex) {
  scitbx::af::shared<double> kapton_path_mm;
  int nfaces = boost::python::len(kapton_faces);
  std::vector<Detector> all_kapton_faces;
  // Store all the detectors ahead of time to avoid making expensive extract calls every
  // time
  for (std::size_t i = 0; i < nfaces; ++i) {
    all_kapton_faces.push_back(boost::python::extract<Detector>(kapton_faces[i]));
  }
  for (scitbx::af::const_ref<vec3<double> >::const_iterator it = s1_flex.begin();
       it != s1_flex.end();
       ++it) {
    double max_d2 = -999.9;
    scitbx::af::shared<vec3<double> > intersection_xy_list;
    // Find out intersection points of s1 vector with all faces of kapton volume
    for (std::size_t j = 0; j < nfaces; ++j) {
      Detector detector = all_kapton_faces.at(j);
      try {
        vec2<double> px = detector[0].get_ray_intersection_px(*it);
        if (px[0] > 0.0 && px[1] > 0.0 && px[0] < detector[0].get_image_size()[0]
            && px[1] < detector[0].get_image_size()[1]) {
          intersection_xy_list.push_back(
            detector[0].get_lab_coord(detector[0].get_ray_intersection(*it)));
        }
      } catch (dxtbx::error) {
        // Do nothing
      }
    }  // kapton faces
    /*
     * Now find out the maximum path length through the kapton. That is the path
     * traversed by s1 vector if no intersection or intersects with only one face then
     * path length is set to 0.0
     */
    if (intersection_xy_list.size() == 0) {
      kapton_path_mm.push_back(0.0);
    } else if (intersection_xy_list.size() == 1) {
      kapton_path_mm.push_back(0.0);
    } else {
      double d2 = 0.0;
      for (std::size_t k1 = 0; k1 < intersection_xy_list.size() - 1; ++k1) {
        for (std::size_t k2 = k1 + 1; k2 < intersection_xy_list.size(); ++k2) {
          vec3<double> pt1 = intersection_xy_list[k1];
          vec3<double> pt2 = intersection_xy_list[k2];
          d2 = (pt1[0] - pt2[0]) * (pt1[0] - pt2[0])
               + (pt1[1] - pt2[1]) * (pt1[1] - pt2[1])
               + (pt1[2] - pt2[2]) * (pt1[2] - pt2[2]);
          if (d2 > max_d2) {
            max_d2 = d2;
          }
        }
      }
      double d = sqrt(max_d2);
      kapton_path_mm.push_back(d);
    }
  }  // s1
  return kapton_path_mm;
}
}  // namespace kapton

using namespace boost::python;
namespace kapton { namespace boost_python { namespace {

  void kapton_init_module() {
    using namespace boost::python;

    def("get_kapton_path_cpp", &kapton::get_kapton_path_cpp);
  }

}}}  // namespace kapton::boost_python::

BOOST_PYTHON_MODULE(dials_algorithms_integration_kapton_ext) {
  kapton::boost_python::kapton_init_module();
}


/**
 * \ file UniversalVariableDelayLineFilter.cpp
 */

#include <iostream>

#include <ATK/Delay/UniversalVariableDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#include <ATK/Tools/CachedSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>
#include <boost/scoped_array.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( UniversalVariableDelayLineFilter_sinus_line16000_delaysinus_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  boost::scoped_array<float> datadelay(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    datadelay[i] = 1 + i * .25f;
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);
  ATK::InPointerFilter<float> generatordelay(datadelay.get(), 1, PROCESSSIZE, false);
  generatordelay.set_output_sampling_rate(48000);

  ATK::UniversalVariableDelayLineFilter<float> filter(48000);
  filter.set_blend(0);
  filter.set_feedback(0);
  filter.set_feedforward(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_input_port(1, &generatordelay, 0);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);
  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 1; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i;
//    BOOST_REQUIRE_EQUAL(data[current - i - 1], outdata[current]);
  }
  for(int64_t i = 1; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+1;
//    BOOST_REQUIRE_EQUAL((data[current - i-1]*3 + data[current - i-2])/4, outdata[current]);
  }
  for(int64_t i = 1; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+2;
//    BOOST_REQUIRE_EQUAL((data[current - i-1] + data[current - i-2])/2, outdata[current]);
  }
  for(int64_t i = 1; i < PROCESSSIZE/4; ++i)
  {
    int64_t current = 4*i+3;
//    BOOST_REQUIRE_EQUAL((data[current - i-1] + data[current - i-2]*3)/4, outdata[current]);
  }
}

BOOST_AUTO_TEST_CASE( UniversalVariableDelayLineFilter_sinus_cachedgenerator_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> sinus(data.get(), 1, PROCESSSIZE, false);
  sinus.set_output_sampling_rate(48000);
  ATK::CachedSinusGeneratorFilter<float> generator(1, 1);
  generator.set_output_sampling_rate(48000);
  generator.set_volume((1e-3)*48000);
  generator.set_offset((1.5e-3)*48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::UniversalVariableDelayLineFilter<float> filter(128);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &sinus, 0);
  filter.set_input_port(1, &generator, 0);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);
}