//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_NBTRUE_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_FUNCTIONS_SIMD_SSE_AVX_NBTRUE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/reduction/functions/nbtrue.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::nbtrue_, boost::simd::tag::avx_,
                         (A0),
                         ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                        )
  {
    typedef typename meta::scalar_of<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename meta::scalar_of<A0>::type sctype;
      typedef simd::native<sctype, boost::simd::tag::sse_ >  svtype;
      svtype a00 = _mm256_extractf128_si256(a0, 0);
      svtype a01 = _mm256_extractf128_si256(a0, 1);
      return nbtrue(a00)+nbtrue(a01);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is double
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::nbtrue_, boost::simd::tag::avx_,
                         (A0),
                         ((simd_<double_<A0>,boost::simd::tag::avx_>))
                        )
  {
    typedef typename meta::scalar_of<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      int r = _mm256_movemask_pd(is_nez(a0));
      return   double((r&1)+(r>>1&1)+((r>>2)&1)+(r>>3));
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is float
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::nbtrue_, boost::simd::tag::avx_,
                         (A0),
                         ((simd_<single_<A0>,boost::simd::tag::avx_>))
                        )
  {
    typedef typename meta::scalar_of<A0>::type  result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      int r = _mm256_movemask_ps(is_nez(a0));
      return   result_type((r&1)+((r>>1)&1)+((r>>2)&1)+(r>>3&1)+((r>>4)&1)+((r>>5)&1)+(r>>6&1)+(r>>7));
      //      return __builtin_popcount(_mm_movemask_ps(isnez(cast<type>(a0))));
    }
  };
} } }
#endif
#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *        Wakker, K.F. "astro I, AE4-874", Delft University of Technology, 2007.
 *        Howell, K.C. Three-dimensional, periodic, 'Halo' orbits, Celestial Mechanics, 32. 53-71,
 *          1984.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "tudat/simulation/simulation.h"
#include "tudat/basics/testMacros.h"
#include "tudat/simulation/propagation_setup/propagationCR3BPFullProblem.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat;
using namespace propagators;
using namespace orbital_element_conversions;

//! Test if state derivative for circular restricted three-body problem is computed correctly.
BOOST_AUTO_TEST_CASE( testFullPropagationCircularRestrictedThreeBodyProblem )
{
    double initialTime = 0.0;
    double finalTime = 120000000.0;

    std::vector < std::string > bodiesCR3BP;
    bodiesCR3BP.push_back( "Sun" );
    bodiesCR3BP.push_back( "Earth" );

    simulation_setup::SystemOfBodies bodies = setupBodyMapCR3BP(
                physical_constants::ASTRONOMICAL_UNIT, "Sun", "Earth", "Spacecraft" );

    // Spacecraft properties
    bodies.at( "Spacecraft" )->setConstantBodyMass( 100.0 );

    // Initialization of the spacecraft state
    Eigen::Vector6d initialState;
    initialState[0] = 2.991957413820000e+10;
    initialState[1] = 1.295555563704656e+11;
    initialState[2] = 0.0;
    initialState[3] = -2.579433850734350e+04;
    initialState[4] = 5.956947312313238e+03;
    initialState[5] = 0.0;

    // Define propagator settings variables.
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;
    bodiesToPropagate.push_back( "Spacecraft" );
    centralBodies.push_back( "SSB" );

    basic_astrodynamics::AccelerationMap accelerationModelMap = setupAccelerationMapCR3BP(
                "Sun", "Earth", bodiesToPropagate.at( 0 ), centralBodies.at( 0 ), bodies );

    // Create integrator settings
    const double fixedStepSize = 1000.0;
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >
            ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize );

    // calculate the difference between CR3BP and full problem
    Eigen::Vector6d stateDifference = getFinalStateDifferenceFullPropagationWrtCR3BP(
                initialTime, finalTime, initialState, integratorSettings, accelerationModelMap,
                bodiesToPropagate, centralBodies,bodies, bodiesCR3BP );

    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( stateDifference( i ) ), 1.0 );
        BOOST_CHECK_SMALL( std::fabs( stateDifference( i + 3 ) ), 1.0E-6 );
    }

}

}

}


#include "InstanceLoss.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/ActiveLearningData.h"

using Eigen::VectorXd;

InstanceLoss::InstanceLoss(InferenceDataView_ptr data, ActiveLearningData_ptr alData,
						   double lossWeight)
: _data(data), _alData(alData), _lossWeight(lossWeight) {}

double InstanceLoss::operator()(VectorXd& gradient, size_t thread_idx,
		size_t n_threads) const
{
	double loss = 0.0;
	for (int i=static_cast<int>(thread_idx); i<static_cast<int>(_data->nInstances()); i+=static_cast<int>(n_threads)) {
		// ann will be 1 if positive, -1 if negative, 0 if unannotated
		if (int ann=_alData->instanceAnnotation(i)) {
			bool pos_example = (ann == 1);

			if (_data->hasFV(i)) {
				double pred = pos_example ? _data->prediction(i)
					: (1.0 - _data->prediction(i));
				loss += _lossWeight * log(pred);
				if (pos_example) {
					gradient += _lossWeight *
						(1.0 - _data->prediction(i)) * _data->features(i);
				} else {
					gradient -= _lossWeight * (_data->prediction(i)*_data->features(i));
				}
			}
		}
	}
	_debug_loss = loss;
	return loss;
}

double InstanceLoss::forInstance(int inst, Eigen::SparseVector<double>& gradient) const {
	double ret = 0.0;
	if (int ann=_alData->instanceAnnotation(inst)) {
		bool pos_example = (ann == 1);

		if (_data->hasFV(inst)) {
			double pred = pos_example ? _data->prediction(inst)
				: (1.0 - _data->prediction(inst));
			ret += _lossWeight * log(pred);
			if (pos_example) {
				gradient += _lossWeight *
					(1.0 - _data->prediction(inst)) * _data->features(inst);
			} else {
				gradient -= _lossWeight * (_data->prediction(inst)*_data->features(inst));
			}
		}
	}
	return ret;
}

std::wstring InstanceLoss::status() const {
	return L"inst_loss = " + boost::lexical_cast<std::wstring>(_debug_loss);
}

void InstanceLoss::snapshot() {
}


/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#include <SofaConstraint/LMConstraintDirectSolver.h>
#include <sofa/core/visual/VisualParams.h>
#include <SofaConstraint/ContactDescription.h>
#include <sofa/core/ObjectFactory.h>
#include <Eigen/QR>
#include <Eigen/SVD>

namespace sofa
{

namespace component
{

namespace constraintset
{

LMConstraintDirectSolver::LMConstraintDirectSolver()
    : solverAlgorithm(initData(&solverAlgorithm, "solverAlgorithm", "Algorithm used to solve the system W.Lambda=c"))
{
    //Add here other algo
    sofa::helper::OptionsGroup algo(1,"SVD");
    solverAlgorithm.setValue(algo);
}

bool LMConstraintDirectSolver::buildSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2)
{
    bool sucess = LMConstraintSolver::buildSystem(cParams, res1, res2);

    return sucess;
}

bool LMConstraintDirectSolver::solveSystem(const core::ConstraintParams* cParams, MultiVecId res1, MultiVecId res2)
{
    //First, do n iterations of Gauss Seidel
    bool success = LMConstraintSolver::solveSystem(cParams, res1, res2);

    if (cParams->constOrder() != core::ConstraintParams::VEL) return success;

    //Then process to a direct solution of the system

    //We need to find all the constraint related to contact
    // 1. extract the information about the state of the contact and build the new L, L^T matrices
    // 2. build the full system
    // 3. solve

    //------------------------------------------------------------------
    // extract the information about the state of the contact
    //------------------------------------------------------------------

    //************************************************************
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("AnalyseConstraints");
#endif
    const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();

    JacobianRows rowsL ; rowsL.reserve(numConstraint);
    JacobianRows rowsLT; rowsLT.reserve(numConstraint);
    helper::vector< unsigned int > rightHandElements;

    analyseConstraints(LMConstraints, cParams->constOrder(),
            rowsL, rowsLT, rightHandElements);

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("AnalyseConstraints");
#endif
    if (rowsL.empty() || rowsLT.empty()) return success;

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("BuildFullSystem");
#endif
    //------------------------------------------------------------------
    // build c: right hand term
    //------------------------------------------------------------------
    VectorEigen previousC(c);
    //TODO: change newC by c
    c=VectorEigen::Zero(rowsL.size());
    unsigned int idx=0;
    for (helper::vector<unsigned int >::const_iterator it=rightHandElements.begin(); it!=rightHandElements.end(); ++it)
        c[idx++]=previousC[*it];

    //------------------------------------------------------------------
    // build the L and LT matrices
    //------------------------------------------------------------------

    DofToMatrix LMatricesDirectSolver;
    DofToMatrix LTMatricesDirectSolver;
    for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end(); ++it)
    {
        //------------------------------------------------------------------
        const SparseMatrixEigen& matrix= it->second;
        //Init the manipulator with the full matrix
        linearsolver::LMatrixManipulator manip;
        manip.init(matrix);

        //------------------------------------------------------------------
        SparseMatrixEigen  L (rowsL.size(),  matrix.cols());
        L.reserve(rowsL.size()*matrix.cols());
        manip.buildLMatrix(rowsL ,L);
        L.finalize();
        LMatricesDirectSolver.insert (std::make_pair(it->first,L ));

        //------------------------------------------------------------------
        SparseMatrixEigen  LT(rowsLT.size(), matrix.cols());
        LT.reserve(rowsLT.size()*matrix.cols());
        manip.buildLMatrix(rowsLT,LT);
        LT.finalize();
        LTMatricesDirectSolver.insert(std::make_pair(it->first,LT));
    }

    //------------------------------------------------------------------
    // build the full system
    //------------------------------------------------------------------
    const  int rows=rowsL.size();
    const  int cols=rowsLT.size();
    SparseColMajorMatrixEigen Wsparse(rows,cols);
    buildLeftRectangularMatrix(invMassMatrix, LMatricesDirectSolver, LTMatricesDirectSolver, Wsparse,invMass_Ltrans);

    //------------------------------------------------------------------
    // conversion from sparse to dense matrix
    //------------------------------------------------------------------
    Lambda=VectorEigen::Zero(rows);

    W=MatrixEigen::Zero(rows,cols);

    SparseMatrixEigen Wresult(Wsparse);
    for (int k=0; k<Wresult.outerSize(); ++k)
        for (SparseMatrixEigen::InnerIterator it(Wresult,k); it; ++it) W(it.row(),it.col()) = it.value();

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("BuildFullSystem");
#endif

    //------------------------------------------------------------------
    // Solve the system
    //------------------------------------------------------------------
    const std::string &algo=solverAlgorithm.getValue().getSelectedItem() ;
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::TRACE_ARGUMENT arg1;
    arg1.push_back(std::make_pair("Algorithm", algo));
    arg1.push_back(std::make_pair("Dimension", printDimension(W)));
    sofa::simulation::Visitor::printNode("DirectSolveSystem", "",arg1);
#endif
    if(algo == "SVD")
    {
        Eigen::JacobiSVD< MatrixEigen > solverSVD(W);
        VectorEigen invSingularValues(solverSVD.singularValues());

        for (int i=0; i<invSingularValues.size(); ++i)
        {
            if (invSingularValues[i] < 1e-10) invSingularValues[i]=0;
            else invSingularValues[i]=1/invSingularValues[i];
        }
        Lambda.noalias() = solverSVD.matrixV()*invSingularValues.asDiagonal()*solverSVD.matrixU().transpose()*c;
    }

    if (this->f_printLog.getValue())
    {
        sout << "W" <<  printDimension(W) <<  "  Lambda" << printDimension(Lambda) << "  c" << printDimension(c) << sendl;
        sout << "\nW     ===============================================\n" << W
                <<  "\nLambda===============================================\n" << Lambda
                <<  "\nc     ===============================================\n" << c << sendl;
    }

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("DirectSolveSystem");
#endif
    return success;

}

void LMConstraintDirectSolver::analyseConstraints(const helper::vector< sofa::core::behavior::BaseLMConstraint* > &LMConstraints, core::ConstraintParams::ConstOrder order,
        JacobianRows &rowsL,JacobianRows &rowsLT, helper::vector< unsigned int > &rightHandElements) const
{
    //Iterate among all the Sofa LMConstraint
    for (unsigned int componentConstraint=0; componentConstraint<LMConstraints.size(); ++componentConstraint)
    {
        sofa::core::behavior::BaseLMConstraint *constraint=LMConstraints[componentConstraint];
        //Find the constraint dealing with contact
        if (ContactDescriptionHandler* contactDescriptor=dynamic_cast<ContactDescriptionHandler*>(constraint))
        {
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            //Iterate among all the contacts
            for (helper::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const sofa::core::behavior::ConstraintGroup* group=*itGroup;
                const sofa::component::constraintset::ContactDescription& contact=contactDescriptor->getContactDescription(group);

                const unsigned int idxEquation=group->getConstraint(0).idx;

                switch(contact.state)
                {
                case VANISHING:
                {
                    //                    serr <<"Constraint " << idxEquation << " VANISHING" << sendl;
                    //0 equation
                    break;
                }
                case STICKING:
                {
                    //                    serr << "Constraint " <<idxEquation << " STICKING" << sendl;
                    const unsigned int i=rowsL.size();
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //3 equations
                    rowsLT.push_back(rowsL[i  ]);
                    rowsLT.push_back(rowsL[i+1]);
                    rowsLT.push_back(rowsL[i+2]);

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                case SLIDING:
                {
                    //                    serr << "Constraint " <<idxEquation << " SLIDING" << sendl;
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //1 equation with the response force along the Coulomb friction cone
                    rowsLT.push_back(linearsolver::LLineManipulator()
                            .addCombination(idxEquation  ,contact.coeff[0])
                            .addCombination(idxEquation+1,contact.coeff[1])
                            .addCombination(idxEquation+2,contact.coeff[2]));

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                }
            }
        }
        else
        {
            //Non contact constraints: we add all the equations
            const helper::vector< sofa::core::behavior::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            for (helper::vector< sofa::core::behavior::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const sofa::core::behavior::ConstraintGroup* group=*itGroup;
                std::pair< sofa::core::behavior::ConstraintGroup::EquationConstIterator,sofa::core::behavior::ConstraintGroup::EquationConstIterator> range=group->data();
                for ( sofa::core::behavior::ConstraintGroup::EquationConstIterator it=range.first; it!=range.second; ++it)
                {
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(it->idx));
                    rowsLT.push_back(rowsL.back());
                    rightHandElements.push_back(it->idx);
                }
            }
        }
    }
}

void LMConstraintDirectSolver::buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix,
        DofToMatrix& LMatrix, DofToMatrix& LTMatrix,
        SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const
{
    invMass_Ltrans.clear();
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second;
        const SparseMatrixEigen &L =LMatrix[dofs];
        const SparseMatrixEigen &LT=LTMatrix[dofs];

        SparseMatrixEigen invMass_LT=invMass*LT.transpose();

        invMass_Ltrans.insert(std::make_pair(dofs, invMass_LT));
        //SparseColMajorMatrixEigen temp=L*invMass_LT;
        LeftMatrix += L*invMass_LT;
    }
}
int LMConstraintDirectSolverClass = core::RegisterObject("A Direct Constraint Solver working specifically with LMConstraint based components")
        .add< LMConstraintDirectSolver >();

SOFA_DECL_CLASS(LMConstraintDirectSolver);

} // namespace constraintset

} // namespace component

} // namespace sofa


// Boost.Geometry

// Copyright (c) 2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP
#define BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/formulas/eccentricity_sqr.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/math/special_functions/atanh.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace formula_dispatch
{

template <typename ResultType, typename Geometry, typename Tag = typename tag<Geometry>::type>
struct authalic_radius_sqr
    : not_implemented<Tag>
{};

template <typename ResultType, typename Geometry>
struct authalic_radius_sqr<ResultType, Geometry, srs_sphere_tag>
{
    static inline ResultType apply(Geometry const& geometry)
    {
        return math::sqr<ResultType>(get_radius<0>(geometry));
    }
};

template <typename ResultType, typename Geometry>
struct authalic_radius_sqr<ResultType, Geometry, srs_spheroid_tag>
{
    static inline ResultType apply(Geometry const& geometry)
    {
        ResultType const a2 = math::sqr<ResultType>(get_radius<0>(geometry));
        ResultType const e2 = formula::eccentricity_sqr<ResultType>(geometry);

        return apply(a2, e2);
    }

    static inline ResultType apply(ResultType const& a2, ResultType const& e2)
    {
        ResultType const c0 = 0;

        if (math::equals(e2, c0))
        {
            return a2;
        }

        ResultType const e = math::sqrt(e2);
        ResultType const c2 = 2;

        //ResultType const b2 = math::sqr(get_radius<2>(geometry));
        //return a2 / c2 + b2 * boost::math::atanh(e) / (c2 * e);

        ResultType const c1 = 1;
        return (a2 / c2) * ( c1 + (c1 - e2) * boost::math::atanh(e) / e );
    }
};

} // namespace formula_dispatch
#endif // DOXYGEN_NO_DISPATCH

#ifndef DOXYGEN_NO_DETAIL
namespace formula
{

template <typename ResultType, typename Geometry>
inline ResultType authalic_radius_sqr(Geometry const& geometry)
{
    return formula_dispatch::authalic_radius_sqr<ResultType, Geometry>::apply(geometry);
}

} // namespace formula
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_FORMULAS_AUTHALIC_RADIUS_SQR_HPP


#include <QApplication>
#include <QKeyEvent>
#include <QBuffer>
#include <QFileInfo>
#include "QImageWidget.h"
#include "QKinectGrabber.h"
#include "QKinectIO.h"
#include <iostream>
#include <iterator>
#include <array>

#include "Volumetric_helper.h"
#include "Timer.h"
#include <Eigen/Dense>
#include <cuda_runtime.h>
#include <vector_types.h>
#include "cuda_kernels/cuda_kernels.h"

#include "helper_cuda.h"
#include "helper_image.h"

static void export_obj_float4(const std::string& filename, const std::vector<float4>& vertices, const std::vector<float4>& normals)
{
	std::ofstream file;
	file.open(filename);
	//for (const float4 v : vertices)
	//	file << std::fixed << "v " << v.x << ' ' << v.y << ' ' << v.z << std::endl;
	//for (const float4 n : normals)
	//	file << std::fixed << "vn " << n.x << ' ' << n.y << ' ' << n.z << std::endl;

	for (int i = 0; i < vertices.size(); ++i)
	{
		const float4& v = vertices[i];
		const float4& n = normals[i];
		file << std::fixed << "v "
			<< v.x << ' ' << v.y << ' ' << v.z << ' '
			<< ((n.x * 0.5) + 0.5) * 255 << ' ' << ((n.y * 0.5) + 0.5) * 255 << ' ' << ((n.z * 0.5) + 0.5) * 255
			<< std::endl;
	}

	file.close();
}

void run_back_projection_with_normal_estimate(
	std::vector<float4>& vertices,
	std::vector<float4>& normals,
	const std::vector<ushort>& depth_buffer,
	uint width,
	uint height,
	ushort max_depth)
{
	StopWatchInterface *kernel_timer = nullptr;

	ushort* h_depth_buffer = (ushort*)depth_buffer.data();

	size_t in_pitch, out_pitch;

	ushort* d_depth_buffer = nullptr;
	// copy image data to array
	checkCudaErrors(cudaMallocPitch(&d_depth_buffer, &in_pitch, sizeof(ushort) * width, height));
	checkCudaErrors(cudaMemcpy2D(
		d_depth_buffer,
		in_pitch,
		h_depth_buffer,
		sizeof(ushort) * width,
		sizeof(ushort) * width,
		height,
		cudaMemcpyHostToDevice));

	float4* d_vertex_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_vertex_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	float4* d_normal_buffer;
	checkCudaErrors(cudaMallocPitch(
		&d_normal_buffer,
		&out_pitch,
		width * sizeof(float4),
		height));

	sdkCreateTimer(&kernel_timer);
	sdkStartTimer(&kernel_timer);

	Eigen::Matrix4f h_inverse_projection = perspective_matrix_inverse<float>(fov_y, aspect_ratio, near_plane, far_plane);
	//bilateralFilter_normal_estimate_float4((OutputPixelType*)dOutputImage, (InputPixelType*)dInputImage, width, height, in_pitch, out_pitch, max_depth, euclidean_delta, filter_radius, iterations, kernel_timer);
	back_projection_with_normal_estimation(d_vertex_buffer, d_normal_buffer, d_depth_buffer, width, height, max_depth, in_pitch, out_pitch, h_inverse_projection.data());

	checkCudaErrors(cudaDeviceSynchronize());
	sdkStopTimer(&kernel_timer);
	std::cout << "Kernel Timer                              : " << kernel_timer->getTime() << " msec" << std::endl;
	sdkDeleteTimer(&kernel_timer);

	vertices.resize(depth_buffer.size());
	normals.resize(depth_buffer.size());

	cudaMemcpy2D(
		vertices.data(),
		sizeof(float4) * width,
		d_vertex_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	cudaMemcpy2D(
		normals.data(),
		sizeof(float4) * width,
		d_normal_buffer,
		out_pitch,
		sizeof(float4) * width,
		height,
		cudaMemcpyDeviceToHost);

	checkCudaErrors(cudaFree(d_depth_buffer));
	checkCudaErrors(cudaFree(d_vertex_buffer));
	checkCudaErrors(cudaFree(d_normal_buffer));
}

void convertKinectFrame2QImage(const KinectFrame& frame, QImage& depthImage)
{
	QVector<QRgb> colorTable;
	for (int i = 0; i < 256; ++i)
		colorTable.push_back(qRgb(i, i, i));
	depthImage = QImage(frame.depth_width(), frame.depth_height(), QImage::Format::Format_Indexed8);
	depthImage.setColorTable(colorTable);

	// set pixels to depth image
	for (int y = 0; y < depthImage.height(); y++)
	{
		for (int x = 0; x < depthImage.width(); x++)
		{
			const unsigned short depth = frame.depth[y * frame.depth_width() + x];
			depthImage.scanLine(y)[x] = static_cast<uchar>((float)depth / (float)frame.depth_max_distance() * 255.f);;
		}
	}
}

int main(int argc, char **argv)
{
	if (argc < 2)
	{
		std::cerr << "Usage: NormalEstimate_gpu.exe ../../data/room.knt " << std::endl;
		return EXIT_FAILURE;
	}

	std::string filename = argv[1];
	Timer timer;

	timer.start();
	KinectFrame frame;
	QKinectIO::loadFrame(QString::fromStdString(filename), frame);
	timer.print_interval("Importing kinect frame (.knt)             : ");

	timer.start();
	std::vector<float4> vertices, normals;
	run_back_projection_with_normal_estimate(vertices, normals, frame.depth, frame.depth_width(), frame.depth_height(), frame.depth_max_distance());
	timer.print_interval("Running normal estimate in GPU            : ");

	timer.start();
	QString frame_filename = QFileInfo(filename.c_str()).absolutePath() + '/' + QFileInfo(filename.c_str()).fileName().remove(".knt") + "_normal_estimate.obj";
	export_obj_float4(frame_filename.toStdString(), vertices, normals);
	timer.print_interval("Exporting obj                             : ");

}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/transpose_graph.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

int
main()
{
  using namespace boost;
  typedef int weight_t;
  typedef adjacency_list < vecS, vecS, bidirectionalS,
    property < vertex_name_t, char > > graph_t;

  enum { a, b, c, d, e, f, g, N };
  graph_t G(N);
  property_map < graph_t, vertex_name_t >::type
    name_map = get(vertex_name, G);
  char name = 'a';
  graph_traits < graph_t >::vertex_iterator v, v_end;
  for (tie(v, v_end) = vertices(G); v != v_end; ++v, ++name)
    name_map[*v] = name;

  typedef std::pair < int, int >E;
  E edge_array[] = { E(a, c), E(a, d), E(b, a), E(b, d), E(c, f),
    E(d, c), E(d, e), E(d, f), E(e, b), E(e, g), E(f, e), E(f, g)
  };
  for (int i = 0; i < 12; ++i)
    add_edge(edge_array[i].first, edge_array[i].second, G);

  print_graph(G, name_map);
  std::cout << std::endl;

  graph_t G_T;
  transpose_graph(G, G_T);

  print_graph(G_T, name_map);

  graph_traits < graph_t >::edge_iterator ei, ei_end;
  for (tie(ei, ei_end) = edges(G); ei != ei_end; ++ei)
    assert(edge(target(*ei, G), source(*ei, G), G_T).second == true);
  return 0;
}