/*    Copyright (c) 2010-2018, 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.
 */

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Dense>
#include <math.h>
#include <iostream>

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanagan.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanaganModel.h"
#include "tudat/astro/LowThrustTrajectories/simsFlanaganOptimisationSetup.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/hodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/baseFunctionsHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/compositeFunctionHodographicShaping.h"
#include "tudat/astro/LowThrustTrajectories/ShapeBasedMethods/createBaseFunctionHodographicShaping.h"
#include "pagmo/algorithms/de1220.hpp"
#include "Problems/applicationOutput.h"
#include "tudat/astro/basic_astro/celestialBodyConstants.h"

int main( )
{
    using namespace tudat;
    using namespace tudat::input_output;
    using namespace tudat::simulation_setup;
    using namespace tudat::shape_based_methods;
    using namespace tudat::low_thrust_trajectories;
    using namespace shape_based_methods;

    spice_interface::loadStandardSpiceKernels( );

    double julianDate = 9264.5 * physical_constants::JULIAN_DAY;
    double timeOfFlight = 1000.0 * physical_constants::JULIAN_DAY;
    int numberOfRevolutions = 2;

    // Ephemeris departure body.
    ephemerides::EphemerisPointer pointerToDepartureBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions>(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter );

    // Ephemeris arrival body.
    ephemerides::EphemerisPointer pointerToArrivalBodyEphemeris = std::make_shared< ephemerides::ApproximatePlanetPositions >(
                ephemerides::ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mars );

    // Retrieve cartesian state at departure and arrival.
    Eigen::Vector6d cartesianStateDepartureBody = pointerToDepartureBodyEphemeris->getCartesianState( julianDate );
    Eigen::Vector6d cartesianStateArrivalBody = pointerToArrivalBodyEphemeris->getCartesianState( julianDate + timeOfFlight );

    double maximumThrust = 5.0;
    double specificImpulse = 3000.0;
    double mass = 2800.0;
    int numberSegments = 500;

    std::string bodyToPropagate = "Borzi";
    std::string centralBody = "Sun";

    // Create central, departure and arrival bodies.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );

    std::map< std::string, std::shared_ptr< simulation_setup::BodySettings > > bodySettings =
            simulation_setup::getDefaultBodySettings( bodiesToCreate );

    std::string frameOrigin = "SSB";
    std::string frameOrientation = "ECLIPJ2000";

    // Define central body ephemeris settings.
    bodySettings[ centralBody ]->ephemerisSettings = std::make_shared< simulation_setup::ConstantEphemerisSettings >(
                ( Eigen::Vector6d( ) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ).finished( ), frameOrigin, frameOrientation );

    bodySettings[ centralBody ]->ephemerisSettings->resetFrameOrientation( frameOrientation );
    bodySettings[ centralBody ]->rotationModelSettings->resetOriginalFrame( frameOrientation );

    // Create system of bodies.
    simulation_setup::SystemOfBodies bodies = createBodies( bodySettings );

    bodies[ bodyToPropagate ] = std::make_shared< simulation_setup::Body >( );
    bodies.at( bodyToPropagate )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                                         std::shared_ptr< interpolators::OneDimensionalInterpolator
                                                         < double, Eigen::Vector6d > >( ), frameOrigin, frameOrientation ) );

    setGlobalFrameBodyEphemerides( bodies, frameOrigin, frameOrientation );

    // Set vehicle mass.
    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    double frequency = 2.0 * mathematical_constants::PI / timeOfFlight;
    double scaleFactor = 1.0 / timeOfFlight;

    // Initialize free coefficients vector for radial velocity function.
    Eigen::VectorXd freeCoefficientsRadialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for normal velocity function.
    Eigen::VectorXd freeCoefficientsNormalVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Initialize free coefficients vector for axial velocity function.
    Eigen::VectorXd freeCoefficientsAxialVelocityFunction = Eigen::VectorXd::Zero( 0 );

    // Get recommended base functions for the radial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstRadialVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondRadialVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdRadialVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( frequency );

    // Create components of the radial velocity composite function.
    std::vector< std::shared_ptr< BaseFunctionHodographicShaping > > radialVelocityFunctionComponents;
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondRadialVelocityBaseFunctionSettings ) );
    radialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, thirdRadialVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the normal velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstNormalVelocityBaseFunctionSettings =
            std::make_shared< BaseFunctionHodographicShapingSettings >( );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondNormalVelocityBaseFunctionSettings =
            std::make_shared< PowerFunctionHodographicShapingSettings >( 1.0, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdNormalVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( frequency );

    // Get recommended base functions for normal velocity composition function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > normalVelocityFunctionComponents;
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( constant, firstNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPower, secondNormalVelocityBaseFunctionSettings ) );
    normalVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, thirdNormalVelocityBaseFunctionSettings ) );

    // Create base function settings for the components of the axial velocity composite function.
    std::shared_ptr< BaseFunctionHodographicShapingSettings > firstAxialVelocityBaseFunctionSettings =
            std::make_shared< TrigonometricFunctionHodographicShapingSettings >( ( numberOfRevolutions + 0.5 ) * frequency );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > secondAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >
            ( 3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );
    std::shared_ptr< BaseFunctionHodographicShapingSettings > thirdAxialVelocityBaseFunctionSettings =
            std::make_shared< PowerTimesTrigonometricFunctionHodographicShapingSettings >(
                3.0, ( numberOfRevolutions + 0.5 ) * frequency, scaleFactor );

    // Get recommended base functions for axial velocity composition function.
    std::vector< std::shared_ptr< shape_based_methods::BaseFunctionHodographicShaping > > axialVelocityFunctionComponents;
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( cosine, firstAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerCosine, secondAxialVelocityBaseFunctionSettings ) );
    axialVelocityFunctionComponents.push_back(
                createBaseFunctionHodographicShaping( scaledPowerSine, thirdAxialVelocityBaseFunctionSettings ) );

    // Create hodographic-shaping object with defined velocity functions and boundary conditions.
    std::shared_ptr< shape_based_methods::HodographicShaping > hodographicShaping =
            std::make_shared< shape_based_methods::HodographicShaping >(
                cartesianStateDepartureBody, cartesianStateArrivalBody, timeOfFlight, numberOfRevolutions,
                bodies, bodyToPropagate, centralBody, radialVelocityFunctionComponents, normalVelocityFunctionComponents,
                axialVelocityFunctionComponents, freeCoefficientsRadialVelocityFunction, freeCoefficientsNormalVelocityFunction,
                freeCoefficientsAxialVelocityFunction );

    // Save results
    int numberOfSteps = 10000;
    double stepSize = timeOfFlight / static_cast< double >( numberOfSteps );

    // Define specific impulse function.
    std::function< double( const double ) > specificImpulseFunction = [ = ]( const double time )
    {
        return specificImpulse;
    };

    // Define integrator settings.
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< double > > ( numerical_integrators::rungeKutta4, 0.0, stepSize );

    std::vector< double > epochsToSaveResults;
    for ( int i = 0 ; i <= numberOfSteps ; i++ )
    {
        epochsToSaveResults.push_back( i * stepSize );
    }

    std::map< double, Eigen::Vector6d > hodographicShapingTrajectory;
    std::map< double, Eigen::VectorXd > hodographicShapingMassProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustProfile;
    std::map< double, Eigen::VectorXd > hodographicShapingThrustAcceleration;
    hodographicShaping->getTrajectory( epochsToSaveResults, hodographicShapingTrajectory );
    hodographicShaping->getMassProfile( epochsToSaveResults, hodographicShapingMassProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping->getThrustProfile( epochsToSaveResults, hodographicShapingThrustProfile, specificImpulseFunction, integratorSettings );
    hodographicShaping->getThrustAccelerationProfile( epochsToSaveResults, hodographicShapingThrustAcceleration,
                                                      specificImpulseFunction, integratorSettings );

    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    std::shared_ptr< ShapeBasedMethod > shapeBased = std::dynamic_pointer_cast< ShapeBasedMethod >( hodographicShaping );

    std::function< Eigen::Vector3d( const double ) > initialGuessThrustFromShaping =
            getInitialGuessFunctionFromShaping( shapeBased, numberSegments, timeOfFlight, specificImpulseFunction, integratorSettings );

    std::vector< double > initialGuessVector = convertToSimsFlanaganThrustModel( initialGuessThrustFromShaping, maximumThrust,
                                                                                timeOfFlight, ( numberSegments + 1 ) / 2, numberSegments / 2 );

    std::map< double, Eigen::Vector3d > initialGuessThrustProfile;
    std::map< double, Eigen::Vector3d > initialGuessThrustAccelerationProfile;
    std::map< double, Eigen::Vector1d > initialGuessMassProfile;

    for ( unsigned int i = 0 ; i < epochsToSaveResults.size( ) ; i++ )
    {
        double currentTime = epochsToSaveResults[ i ];
        initialGuessThrustProfile[ currentTime ] =  initialGuessThrustFromShaping( currentTime );

        if ( i == 0 )
        {
            initialGuessMassProfile[ currentTime ] = ( Eigen::Vector1d( ) << mass ).finished( );
        }
        else
        {
            initialGuessMassProfile[ currentTime ] = ( Eigen::Vector1d( ) << initialGuessMassProfile[ epochsToSaveResults[ i - 1 ] ][ 0 ]
                    - initialGuessThrustProfile[ currentTime ].norm( ) /
                    ( specificImpulse * physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION ) * stepSize ).finished( );
        }

        initialGuessThrustAccelerationProfile[ currentTime ] = initialGuessThrustProfile[ currentTime ] / initialGuessMassProfile[ currentTime ][ 0 ];

    }

    bodies[ bodyToPropagate ]->setConstantBodyMass( mass );

    // Define optimisation algorithm.
    algorithm optimisationAlgorithm{ pagmo::de1220() };

    std::shared_ptr< OptimisationSettings > optimisationSettings = std::make_shared< OptimisationSettings >(
                optimisationAlgorithm, 10, 1024, 1.0e-6, std::make_pair( initialGuessVector, 0.3 ) );

    SimsFlanagan simsFlanagan = SimsFlanagan(
                cartesianStateDepartureBody, cartesianStateArrivalBody, maximumThrust, specificImpulseFunction, numberSegments,
                timeOfFlight, bodies, bodyToPropagate, centralBody, optimisationSettings );

    std::map< double, Eigen::Vector6d > SimsFlanaganTrajectory;
    std::map< double, Eigen::VectorXd > SimsFlanaganMassProfile;
    std::map< double, Eigen::VectorXd > SimsFlanaganThrustProfile;
    std::map< double, Eigen::VectorXd > SimsFlanaganThrustAcceleration;
    simsFlanagan.getTrajectory( epochsToSaveResults, SimsFlanaganTrajectory );
    simsFlanagan.getMassProfile( epochsToSaveResults, SimsFlanaganMassProfile, specificImpulseFunction, integratorSettings );
    simsFlanagan.getThrustProfile( epochsToSaveResults, SimsFlanaganThrustProfile, specificImpulseFunction, integratorSettings );
    simsFlanagan.getThrustAccelerationProfile( epochsToSaveResults, SimsFlanaganThrustAcceleration, specificImpulseFunction, integratorSettings );

    input_output::writeDataMapToTextFile( hodographicShapingTrajectory,
                                          "hodographicShapingTrajectory.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingMassProfile,
                                          "hodographicShapingMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustProfile,
                                          "hodographicShapingThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( hodographicShapingThrustAcceleration,
                                          "hodographicShapingThrustAcceleration.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganTrajectory,
                                          "SimsFlanaganTrajectory.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganMassProfile,
                                          "SimsFlanaganMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganThrustProfile,
                                          "SimsFlanaganThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( SimsFlanaganThrustAcceleration,
                                          "SimsFlanaganThrustAcceleration.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessThrustProfile,
                                          "initialGuessThrustProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessThrustAccelerationProfile,
                                          "initialGuessThrustAccelerationProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    input_output::writeDataMapToTextFile( initialGuessMassProfile,
                                          "initialGuessMassProfile.dat",
                                          tudat_pagmo_applications::getOutputPath( ),
                                          "",
                                          std::numeric_limits< double >::digits10,
                                          std::numeric_limits< double >::digits10,
                                          "," );

    std::cout << "DELTAV SIMS FLANAGAN: " << simsFlanagan.computeDeltaV( ) << "\n\n";
    std::cout << "DELTAV SHAPE BASED: " << hodographicShaping->computeDeltaV( ) << "\n\n";

    // Final statement.
    // The exit code EXIT_SUCCESS indicates that the program was successfully executed.
    return EXIT_SUCCESS;
}


/*------------------------------------------------------------------------------
* preceph.c : precise ephemeris and clock functions
*
*          Copyright (C) 2007-2013 by T.TAKASU, All rights reserved.
*
* references :
*     [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
*         12 February, 2007
*     [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
*         27 August, 1998
*     [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
*     [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
*         Space Technology Library, 2004
*-----------------------------------------------------------------------------*/

#include <unordered_map>
#include <iostream>
#include <string>
#include <array>
#include <map>
#include <ctype.h>

using std::string;
using std::array;
using std::map;

#include <boost/log/trivial.hpp>

#include "eigenIncluder.hpp"
#include "streamTrace.hpp"
#include "navigation.hpp"
#include "constants.h"
#include "station.hpp"
#include "algebra.hpp"
#include "gTime.hpp"
#include "common.hpp"
#include "tides.hpp"
#include "enums.h"

map<string, map<E_Sys, array<double, 3>>> stationRBiasMap;

//     double rbias[2][3]		= {}; /* receiver dcb (0:p1-p2, 1:p1-c1, 2:p2-c2) (m) */

#define NMAX        10              /* order of polynomial interpolation */
#define MAXDTE      900.0           /* max time difference to ephem time (s) */
#define EXTERR_CLK  1E-3            /* extrapolation error for clock (m/s) */
#define EXTERR_EPH  5E-7            /* extrapolation error for ephem (m/s^2) */

/* read satellite antenna parameters -------------------------------------------
* read satellite antenna parameters
* args   : char   *file       I   antenna parameter file
*          gtime_t time       I   time
*          nav_t  *nav        IO  navigation data
* return : status (1:ok,0:error)
* notes  : only support antex format for the antenna parameter file
*-----------------------------------------------------------------------------*/
/*extern int readsap(char *file, gtime_t time, nav_t *nav)
{
	pcvs_t pcvs={0};
	pcv_t pcv0={0},*pcv;
	int i;

	trace(3,"readsap : file=%s time=%s\n",file,time.to_string(0).c_str());

	if (!readpcv(file,&pcvs)) return 0;

	for (i=0;i<MAXSAT;i++) {
		pcv=searchpcv(i+1,"",time,&pcvs);
		nav->pcvs[i]=pcv?*pcv:pcv0;
	}
	free(pcvs.pcv);
	return 1;
}*/
/* read dcb parameters file --------------------------------------------------*/
int readdcb(string file, nav_t *nav)
{
	//todo aaron, use maps for these rather than arrays,
	FILE *fp;
	double cbias;
	char buff[256],str1[32],str2[32]="";
	int type=0;
	SatSys Sat;
//     trace(3,"readdcbf: file=%s\n",file);

	if (!(fp=fopen(file.c_str(), "r")))
	{
//         trace(2,"dcb parameters file open error: %s\n",file);
		return 0;
	}
	while (fgets(buff,sizeof(buff),fp))
	{
		if      (strstr(buff,"DIFFERENTIAL (P1-P2) CODE BIASES"))	type=1;
		else if (strstr(buff,"DIFFERENTIAL (P1-C1) CODE BIASES"))	type=2;
		else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES"))	type=3;

		if	(!type
			||sscanf(buff,"%s %s",str1,str2) < 1)
			continue;

		if ((cbias = str2num(buff,26,9)) == 0)
			continue;

		if  ( !strcmp(str1,"G")
			||!strcmp(str1,"R"))
		{
			E_Sys sys;
			if (str1[0] == 'G')		sys = E_Sys::GPS;
			if (str1[0] == 'R')		sys = E_Sys::GLO;

			auto& rbias = stationRBiasMap[str2][sys];

			/* receiver dcb */
			rbias[type-1] = cbias * 1E-9 * CLIGHT; /* ns -> m */ //todo aaron, this looks like it had issues to begin with

		}
		else if (Sat = SatSys(str1), Sat)
		{
			/* satellite dcb */
			if (type == 1)		nav->satNavMap[Sat].cBias_P1_P2		= cbias * 1E-9 * CLIGHT; /* ns -> m */
			if (type == 2)		nav->satNavMap[Sat].cBiasMap[F1]	= cbias * 1E-9 * CLIGHT; /* ns -> m */
			if (type == 3)		nav->satNavMap[Sat].cBiasMap[F2]	= cbias * 1E-9 * CLIGHT; /* ns -> m */
		}
	}
	fclose(fp);

	return 1;
}

/* add satellite fcb ---------------------------------------------------------*/
// int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
// 				const double *bias, const double *std)	//todo aaron, is this broken?
// {
//     fcbd_t *nav_fcb;
//     int i,j;
//
//     if (nav->nf>0&&fabs(timediff(ts,nav->fcb[nav->nf-1].ts))<=1e-3) {
//         for (i=0;i<3;i++) {
//             nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i];
//             nav->fcb[nav->nf-1].std [sat-1][i]=std [i];
//         }
//         return 1;
//     }
//     if (nav->nf>=nav->nfmax) {
//         nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2;
//         if (!(nav_fcb=(fcbd_t *)realloc(nav->fcb,sizeof(fcbd_t)*nav->nfmax))) {
//             free(nav->fcb); nav->nf=nav->nfmax=0;
//             return 0;
//         }
//         nav->fcb=nav_fcb;
//     }
//     for (i=0;i<MAXSAT;i++) for (j=0;j<3;j++) {
//         nav->fcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0;
//     }
//     for (i=0;i<3;i++) {
//         nav->fcb[nav->nf].bias[sat-1][i]=bias[i];
//         nav->fcb[nav->nf].std [sat-1][i]=std [i];
//     }
//     nav->fcb[nav->nf  ].ts=ts;
//     nav->fcb[nav->nf++].te=te;
// 	return 1;
// }
/* read satellite fcb file ---------------------------------------------------*/
// int readfcbf(const char *file, nav_t *nav)
// {
// 	FILE *fp;
// 	gtime_t ts,te;
// 	double ep1[6],ep2[6],bias[3]={0},std[3]={0};
// 	char buff[1024],str[32],*p;
// 	int sat;
//
// //     trace(3,"readfcbf: file=%s\n",file);
//
// 	if (!(fp=fopen(file,"r"))) {
// //         trace(2,"fcb parameters file open error: %s\n",file);
// 		return 0;
// 	}
// 	while (fgets(buff,sizeof(buff),fp)) {
// 		if ((p=strchr(buff,'#'))) *p='\0';
// 		if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
// 				"%lf %lf %lf %lf %lf %lf",ep1,ep1+1,ep1+2,ep1+3,ep1+4,ep1+5,
// 				ep2,ep2+1,ep2+2,ep2+3,ep2+4,ep2+5,str,bias,std,bias+1,std+1,
// 				bias+2,std+2)<17) continue;
// 		if (!(sat=SatSys(str))) continue;
// 		ts=epoch2time(ep1);
// 		te=epoch2time(ep2);
// 		if (!addfcb(nav,ts,te,sat,bias,std)) return 0;
// 	}
// 	fclose(fp);
// 	return 1;
// }
/* compare satellite fcb -----------------------------------------------------*/
// bool cmpfcb(fcbd_t&p1, fcbd_t&p2)
// {
// 	fcbd_t *q1=&p1,*q2=&p2;
// 	double tt=timediff(q1->ts,q2->ts);
// 	return tt<-1E-3?-1:(tt>1E-3?1:0);
// }
/* read satellite fcb data -----------------------------------------------------
* read satellite fractional cycle bias (dcb) parameters
* args   : char   *file       I   fcb parameters file (wild-card * expanded)
*          nav_t  *nav        IO  navigation data
* return : status (1:ok,0:error)
* notes  : fcb data appended to navigation data
*-----------------------------------------------------------------------------*/
// int readfcb(string& file, nav_t *nav)
// {
// 	readfcbf(file.c_str(), nav);
// // 	nav->fcbList.sort(cmpfcb);
// 	return 1;
// }

/* polynomial interpolation by Neville's algorithm ---------------------------*/
double interppol(const double *x, double *y, int n)
{
	for (int j=1; j < n;		j++)
	for (int i=0; i < n - j;	i++)
	{
		y[i] = (x[i+j] * y[i] - x[i] * y[i+1]) / (x[i+j] - x[i]);
	}

	return y[0];
}

/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(
	GTime time,
	SatSys Sat,
	nav_t& nav,
	double *rs,
	double *dts,
	double *vare,
	double *varc)
{
	double t[NMAX+1],p[3][NMAX+1],c[2],s[3];

	char id[4];
	Sat.getId(id);
//     trace(4,"pephpos : time=%s sat=%s\n",time.to_string(3).c_str(),id);

	rs[0]	= 0;
	rs[1]	= 0;
	rs[2]	= 0;
	*dts	= 0;

	PephList pephList = nav.pephMap[Sat];

	if	( (pephList.size()							< NMAX + 1)
		||(timediff(time, pephList.begin()->first)	< -MAXDTE)
		||(timediff(time, pephList.rbegin()->first)	> +MAXDTE))
	{
//         trace(3,"no prec ephem %s sat=%s\n",time.to_string(0).c_str(),id);
		return 0;
	}

// 	//search for the ephemeris in the list

	auto peph_it = pephList.lower_bound(time);
	if (peph_it == pephList.end())
	{
		peph_it--;
	}

	auto middle0 = peph_it;

	//go forward a few steps to make sure we're far from the end of the list.
	for (int i = 0; i < NMAX/2; i++)
	{
		peph_it++;
		if (peph_it == pephList.end())
		{
			break;
		}
	}

	//go backward a few steps to make sure we're far from the beginning of the list
	for (int i = 0; i <= NMAX; i++)	//todo aaron, needs +1 to go back an extra step due to end()?
	{
		peph_it--;
		if (peph_it == pephList.begin())
		{
			break;
		}
	}

	auto begin = peph_it;

	//get interpolation parameters and check all ephemerides have values.
	peph_it = begin;
	for (int i = 0; i <= NMAX; i++, peph_it++)
	{
		Peph& peph = peph_it->second;
		if (peph.Pos.norm() <= 0)
		{
//             trace(3,"prec ephem outage %s sat=%s\n",time.to_string(0).c_str(), id);
			return 0;
		}

		t[i] = timediff(peph.time, time);
		auto& pos = peph.Pos;
#if 0
		p[0][i]=pos[0];
		p[1][i]=pos[1];
#else
		/* correciton for earh rotation ver.2.4.0 */
		double sinl = sin(OMGE * t[i]);
		double cosl = cos(OMGE * t[i]);
		p[0][i] = cosl * pos[0] - sinl * pos[1];
		p[1][i] = sinl * pos[0] + cosl * pos[1];
#endif
		p[2][i] = pos[2];
	}

	for (int i = 0; i < 3; i++)
	{
		rs[i] = interppol(t, p[i], NMAX + 1);
	}
	double std = 0;
	if (vare)
	{
		for (int i = 0; i < 3; i++)
			s[i] = middle0->second.PosStd[i];
		std = norm(s, 3);

		/* extrapolation error for orbit */
		if      (t[0   ] > 0) std += EXTERR_EPH * SQR(t[0   ]) / 2;		//todo aaron, needs straigtening as below?
		else if (t[NMAX] < 0) std += EXTERR_EPH * SQR(t[NMAX]) / 2;

		*vare = SQR(std);
	}

	/* linear interpolation for clock */
	auto middle1 = middle0;
	if (middle0 != pephList.begin())
	{
		middle0--;
	}
	t[0] = timediff(time, middle0->second.time);
	t[1] = timediff(time, middle1->second.time);
	c[0] = middle0->second.Clk;
	c[1] = middle1->second.Clk;

	if 		(t[0] <= 0)
	{
		*dts = c[0];

		if (*dts != 0)
			std = middle0->second.ClkStd * CLIGHT	+ EXTERR_CLK * fabs(t[0]);
	}
	else if (t[1] >= 0)
	{
		*dts = c[1];

		if (*dts != 0)
			std = middle1->second.ClkStd * CLIGHT	+ EXTERR_CLK * fabs(t[1]);
	}
	else if ( c[0] != 0
			&&c[1] != 0)
	{
		*dts = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);

		double inv0 = 1 / middle0->second.ClkStd * CLIGHT + EXTERR_CLK * fabs(t[0]);
		double inv1 = 1 / middle1->second.ClkStd * CLIGHT + EXTERR_CLK * fabs(t[1]);
		std			= 1 / (inv0 + inv1);
	}
	else
	{
		*dts = 0;
	}
	if (varc)
		*varc=SQR(std);
	return 1;
}

/* satellite clock by precise clock ------------------------------------------*/
int pephclk(GTime time, string id, nav_t& nav, double *dtSat, double *varc)
{
//     BOOST_LOG_TRIVIAL(debug)
// 	<< "pephclk : time=" << time.to_string(3)
// 	<< " id=" << id;

	PclkList pclkList = nav.pclkMap[id];

	if	( (pclkList.size()							< 2)
		||(timediff(time, pclkList.front().	time)	< -MAXDTE)
		||(timediff(time, pclkList.back().	time)	> +MAXDTE))
	{
		BOOST_LOG_TRIVIAL(debug)
		<< "no prec clock " << time.to_string(0)
		<< " id=" << id;

		return -1;	//non zero for pass, negative for no result
	}

	//search for the ephemeris in the list
	auto pclk_it = pclkList.begin();
	while (pclk_it->time < time)
	{
		pclk_it++;
		if (pclk_it == pclkList.end())
		{
			pclk_it--;
			break;
		}
	}
	auto middle1 = pclk_it;
	auto middle0 = middle1;
	if (middle0 != pclkList.begin())
	{
		middle0--;
	}

	/* linear interpolation for clock */
	double t[2];
	double c[2];
	t[0] = timediff(time, middle0->time);
	t[1] = timediff(time, middle1->time);
	c[0] = middle0->clk;
	c[1] = middle1->clk;

	double std = 0;

	if		(t[0] <= 0)
	{
		*dtSat = c[0];

		if (*dtSat == 0)
			return 0;

		std	= middle0->std * CLIGHT	+ EXTERR_CLK * fabs(t[0]);
	}
	else if (t[1] >= 0)
	{
		*dtSat = c[1];

		if (*dtSat == 0)
			return 0;

		std	= middle1->std * CLIGHT	+ EXTERR_CLK * fabs(t[1]);
	}
	else if	( c[0] != 0
			&&c[1] != 0)
	{
		*dtSat = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);

		double inv0 = 1 / middle0->std * CLIGHT + EXTERR_CLK * fabs(t[0]);
		double inv1 = 1 / middle1->std * CLIGHT + EXTERR_CLK * fabs(t[1]);
		std			= 1 / (inv0 + inv1);
	}
	else
	{
		BOOST_LOG_TRIVIAL(debug)
		<< "prec clock outage " << time.to_string(0)
		<< " sat=" << id;

		return 0;
	}

	if (varc)
		*varc = SQR(std);

	return 1;
}
/* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef
* args   : gtime_t time       I   time (gpst)
*          double *rs         I   satellite position and velocity (ecef)
*                                 {x,y,z,vx,vy,vz} (m|m/s)
*          int    sat         I   satellite number
*          nav_t  *nav        I   navigation data
*          double *dant       I   satellite antenna phase center offset (ecef)
*                                 {dx,dy,dz} (m) (iono-free LC value)
* return : none
*-----------------------------------------------------------------------------*/
void satantoff(
	Trace&		trace,
	GTime		time,
	Vector3d&	rs,
	SatSys& 	Sat,
	SatNav*		satNav_ptr,
	Vector3d&	dant,
	PcoMapType*	pcoMap_ptr)
{
	auto&	lam = satNav_ptr->lamMap;
	double gmst,erpv[5]={0};
	E_FType j = F1;
	E_FType k = F2;
	tracepde(4,trace, "satantoff: time=%s sat=%2d\n",time.to_string(3).c_str() ,Sat);

	/* sun position in ecef */
	Vector3d rsun;
	sunmoonpos(gpst2utc(time),erpv,rsun.data(),NULL,&gmst);

	/* unit vectors of satellite fixed coordinates */
	Vector3d r = -rs;
	Vector3d ez = r.normalized();
	r = rsun - rs;
	Vector3d es = r.normalized();
	r = ez.cross(es);
	Vector3d ey = r.normalized();
	Vector3d ex = ey.cross(ez);

	int sys = Sat.sys;
	if 	( sys == E_Sys::GAL
		||sys == E_Sys::SBS)
	{
		k = F5;
	}

	if 	( lam[j] == 0
		||lam[k] == 0)
	{
		return;
	}

	double gamma	= SQR(lam[k]) / SQR(lam[j]);		//todo aaron, can use obs2lc?
	double C1		= gamma	/ (gamma - 1);
	double C2		= -1	/ (gamma - 1);

	if (pcoMap_ptr == nullptr)
	{
		return;
	}
	auto& pcoMap = *pcoMap_ptr;

	/* iono-free LC */
	for (int i = 0; i < 3; i++)
	{
		/* ENU to NEU */
		Vector3d pcoJ;
		Vector3d pcoK;
		if (pcoMap.find(j) == pcoMap.end())		pcoJ = Vector3d::Zero();
		else									pcoJ = pcoMap[j];
		if (pcoMap.find(k) == pcoMap.end())		pcoK = Vector3d::Zero();
		else									pcoK = pcoMap[k];
		double dant1	= pcoJ[1] * ex(i)
						+ pcoJ[0] * ey(i)
						+ pcoJ[2] * ez(i);	//todo aaron, matrix
		double dant2	= pcoK[1] * ex(i)
						+ pcoK[0] * ey(i)
						+ pcoK[2] * ez(i);

		dant(i)	= C1 * dant1
				+ C2 * dant2;
	}
}

/* satellite position/clock by precise ephemeris/clock -------------------------
* compute satellite position/clock with precise ephemeris/clock
* args   : gtime_t time       I   time (gpst)
*          int    sat         I   satellite number
*          nav_t  *nav        I   navigation data
*          int    opt         I   sat postion option
*                                 (0: center of mass, 1: antenna phase center)
*          double *rs         O   sat position and velocity (ecef)
*                                 {x,y,z,vx,vy,vz} (m|m/s)
*          double *dts        O   sat clock {bias,drift} (s|s/s)
*          double *var        IO  sat position and clock error variance (m)
*                                 (NULL: no output)
* return : status (1:ok,0:error or data outage)
* notes  : clock includes relativistic correction but does not contain code bias
*          before calling the function, nav->peph, nav->ne, nav->pclk and
*          nav->nc must be set by calling readsp3(), readrnx() or readrnxt()
*          if precise clocks are not set, clocks in sp3 are used instead
*-----------------------------------------------------------------------------*/
int peph2pos(
	Trace&		trace,
	GTime		time,
	SatSys&		Sat,
	nav_t& 		nav,
	int			opt,
	Obs&		obs,
	PcoMapType*	pcoMap_ptr)
{
	Vector3d dAnt	= Vector3d::Zero();
	Vector3d rst	= Vector3d::Zero();

	double dtss1 = 0,dtss2 = 0,dtst,vare=0,varc=0,tt=1E-3;

	tracepde(4,trace, "peph2pos: time=%s sat=%2d opt=%d\n", time.to_string(3).c_str(), Sat, opt);

	/* satellite position and clock bias */
	if 	( !pephpos(time, Sat, nav, obs.rSat.data(),	&dtss1,	&vare,	&varc)
		||!pephclk(time, Sat, nav, 					&dtss2,			&varc))
	{
		return 0;
	}

	if (dtss2 != 0)
	{
		double delta = dtss1 - dtss2;
		dtss1 = dtss2;
	}

	time = timeadd(time,tt);

	if 	( !pephpos(time, Sat, nav, rst.data(),		&dtst,	NULL,	NULL)
		||!pephclk(time, Sat, nav, 					&dtst,			NULL))
		return 0;

	/* satellite antenna offset correction */
	if (opt)
	{
		satantoff(trace, time, obs.rSat, Sat, &nav.satNavMap[Sat], dAnt, pcoMap_ptr);
	}

	obs.satVel = (rst - obs.rSat) / tt;
	obs.rSat += dAnt;

	/* relativistic effect correction */
	if (dtss1 != 0)
	{
		double deltaDt = dtst-dtss1;
		double relativisticAdj = 2 * obs.rSat.dot(obs.satVel) / CLIGHT / CLIGHT;
		obs.dtSat[0] = dtss1 - relativisticAdj;
		obs.dtSat[1] = deltaDt / tt;
	}
	else
	{
		/* no precise clock */
		obs.dtSat[0] = 0;
		obs.dtSat[1] = 0;
	}

	obs.var = vare + varc;

// 	obs.svh = 1;
	return 1;
}


#include "Assembler.hpp"

// std c++ headers
#include <algorithm>
#include <vector>

// MTL4 includes
#include <boost/numeric/mtl/mtl.hpp>

// AMDiS includes
#include "DOFVector.hpp"
#include "Element.hpp"
#include "ElInfo.hpp"
#include "Operator.hpp"
#include "QPsiPhi.hpp"

namespace AMDiS
{
  Assembler::Assembler(not_null<Operator*> op,
                       not_null<const FiniteElemSpace*> row,
				const FiniteElemSpace*  col)
    : operat(op),
      rowFeSpace(row),
      colFeSpace(col ? col : row.get()),
      nRow(rowFeSpace->getBasisFcts()->getNumber()),
      nCol(colFeSpace->getBasisFcts()->getNumber()),
      remember(true),
      rememberElMat(false),
      rememberElVec(false),
      elementMatrix(nRow, nCol),
      elementVector(nRow),
      tmpMat(nRow, nCol),
      lastMatEl(NULL),
      lastVecEl(NULL),
      lastTraverseId(-1)
  {}

  void Assembler::calculateElementMatrix(const ElInfo* elInfo,
                                         ElementMatrix& userMat,
                                         double factor)
  {
    if (remember && (factor != 1.0 || operat->getUhOld()))
      rememberElMat = true;

    Element* el = elInfo->getElement();

    if (el != lastMatEl || !operat->isOptimized())
    {
      initElement(elInfo);

      if (rememberElMat)
        set_to_zero(elementMatrix);

      lastMatEl = el;
    }
    else
    {
      // Only possible in single mesh case when one operator
      // is used more than twice?
      if (rememberElMat)
      {
        if (&userMat != &elementMatrix)
          userMat += factor * elementMatrix;
        return;
      }
    }

    ElementMatrix& mat = rememberElMat ? elementMatrix : userMat;

    if (secondOrderAssembler)
      secondOrderAssembler->calculateElementMatrix(elInfo, mat);
    if (firstOrderAssemblerGrdPsi)
      firstOrderAssemblerGrdPsi->calculateElementMatrix(elInfo, mat);
    if (firstOrderAssemblerGrdPhi)
      firstOrderAssemblerGrdPhi->calculateElementMatrix(elInfo, mat);
    if (zeroOrderAssembler)
      zeroOrderAssembler->calculateElementMatrix(elInfo, mat);

    if (rememberElMat && &userMat != &elementMatrix)
      userMat += factor * elementMatrix;
  }

  void Assembler::calculateElementVector(const ElInfo* elInfo,
                                         DenseVector<double>& userVec,
                                         double factor)
  {
    if (remember && factor != 1.0)
      rememberElVec = true;

    Element* el = elInfo->getElement();

    if ((el != lastMatEl && el != lastVecEl) || !operat->isOptimized())
      initElement(elInfo);

    if (el != lastVecEl || !operat->isOptimized())
    {
      if (rememberElVec)
        set_to_zero(elementVector);

      lastVecEl = el;
    }
    else
    {
      // Only possible in single mesh case when one operator
      // is used more than twice at dof vector?
      if (rememberElVec)
      {
        userVec += factor * elementVector;
        return;
      }
    }

    DenseVector<double>& vec = rememberElVec ? elementVector : userVec;

    if (operat->getUhOld() && remember)
    {
      matVecAssemble(elInfo, vec);
      if (rememberElVec)
        userVec += factor * elementVector;

      return;
    }

    if (firstOrderAssemblerGrdPsi)
      firstOrderAssemblerGrdPsi->calculateElementVector(elInfo, vec);
    if (zeroOrderAssembler)
      zeroOrderAssembler->calculateElementVector(elInfo, vec);

    if (rememberElVec)
      userVec += factor * elementVector;
  }

  void Assembler::matVecAssemble(const ElInfo* elInfo, DenseVector<double>& vec)
  {

    Element* el = elInfo->getElement();
    DenseVector<double> uhOldLoc(operat->getUhOld()->getFeSpace() == rowFeSpace ?
                                 nRow : nCol);
    operat->getUhOld()->getLocalVector(el, uhOldLoc);

    if (el != lastMatEl)
    {
      set_to_zero(elementMatrix);
      calculateElementMatrix(elInfo, elementMatrix);
    }

    vec += elementMatrix * uhOldLoc;
  }

  void Assembler::initElement(const ElInfo* smallElInfo,
                              const ElInfo* largeElInfo,
                              Quadrature* quad)
  {
    if (secondOrderAssembler)
      secondOrderAssembler->initElement(smallElInfo, largeElInfo, quad);
    if (firstOrderAssemblerGrdPsi)
      firstOrderAssemblerGrdPsi->initElement(smallElInfo, largeElInfo, quad);
    if (firstOrderAssemblerGrdPhi)
      firstOrderAssemblerGrdPhi->initElement(smallElInfo, largeElInfo, quad);
    if (zeroOrderAssembler)
      zeroOrderAssembler->initElement(smallElInfo, largeElInfo, quad);
  }

  void Assembler::checkQuadratures()
  {
    if (secondOrderAssembler)
    {
      // create quadrature
      if (!secondOrderAssembler->getQuadrature())
      {
        int dim = rowFeSpace->getMesh()->getDim();
        int degree = operat->getQuadratureDegree(2);
        Quadrature* quadrature = Quadrature::provideQuadrature(dim, degree);
        secondOrderAssembler->setQuadrature(quadrature);
      }
    }
    if (firstOrderAssemblerGrdPsi)
    {
      // create quadrature
      if (!firstOrderAssemblerGrdPsi->getQuadrature())
      {
        int dim = rowFeSpace->getMesh()->getDim();
        int degree = operat->getQuadratureDegree(1, GRD_PSI);
        Quadrature* quadrature = Quadrature::provideQuadrature(dim, degree);
        firstOrderAssemblerGrdPsi->setQuadrature(quadrature);
      }
    }
    if (firstOrderAssemblerGrdPhi)
    {
      // create quadrature
      if (!firstOrderAssemblerGrdPhi->getQuadrature())
      {
        int dim = rowFeSpace->getMesh()->getDim();
        int degree = operat->getQuadratureDegree(1, GRD_PHI);
        Quadrature* quadrature = Quadrature::provideQuadrature(dim, degree);
        firstOrderAssemblerGrdPhi->setQuadrature(quadrature);
      }
    }
    if (zeroOrderAssembler)
    {
      // create quadrature
      if (!zeroOrderAssembler->getQuadrature())
      {
        int dim = rowFeSpace->getMesh()->getDim();
        int degree = operat->getQuadratureDegree(0);
        Quadrature* quadrature = Quadrature::provideQuadrature(dim, degree);
        zeroOrderAssembler->setQuadrature(quadrature);
      }
    }
  }

  void Assembler::finishAssembling()
  {
    lastVecEl = NULL;
    lastMatEl = NULL;
  }

  OptimizedAssembler::OptimizedAssembler(Operator*  op,
                                         Quadrature* quad2,
                                         Quadrature* quad1GrdPsi,
                                         Quadrature* quad1GrdPhi,
                                         Quadrature* quad0,
                                         const FiniteElemSpace* rowFeSpace,
                                         const FiniteElemSpace* colFeSpace)
    : Assembler(op, rowFeSpace, colFeSpace)
  {
    bool opt = (rowFeSpace->getBasisFcts() == colFeSpace->getBasisFcts());

    // create sub assemblers
    secondOrderAssembler =
      SecondOrderAssembler::getSubAssembler(op, this, quad2, opt);
    firstOrderAssemblerGrdPsi =
      FirstOrderAssembler::getSubAssembler(op, this, quad1GrdPsi, GRD_PSI, opt);
    firstOrderAssemblerGrdPhi =
      FirstOrderAssembler::getSubAssembler(op, this, quad1GrdPhi, GRD_PHI, opt);
    zeroOrderAssembler =
      ZeroOrderAssembler::getSubAssembler(op, this, quad0, opt);

    checkQuadratures();
  }

  StandardAssembler::StandardAssembler(Operator* op,
                                       Quadrature* quad2,
                                       Quadrature* quad1GrdPsi,
                                       Quadrature* quad1GrdPhi,
                                       Quadrature* quad0,
                                       const FiniteElemSpace* rowFeSpace,
                                       const FiniteElemSpace* colFeSpace)
    : Assembler(op, rowFeSpace, colFeSpace)
  {
    remember = false;

    // create sub assemblers
    secondOrderAssembler =
      SecondOrderAssembler::getSubAssembler(op, this, quad2, false);
    firstOrderAssemblerGrdPsi =
      FirstOrderAssembler::getSubAssembler(op, this, quad1GrdPsi, GRD_PSI, false);
    firstOrderAssemblerGrdPhi =
      FirstOrderAssembler::getSubAssembler(op, this, quad1GrdPhi, GRD_PHI, false);
    zeroOrderAssembler =
      ZeroOrderAssembler::getSubAssembler(op, this, quad0, false);

    checkQuadratures();
  }

} // end namespace AMDiS


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//

#include "pxr/pxr.h"
#include "pxr/base/gf/ray.h"
#include "pxr/base/gf/line.h"
#include "pxr/base/gf/lineSeg.h"
#include "pxr/base/gf/plane.h"
#include "pxr/base/gf/range3d.h"

#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/wrapTypeHelpers.h"

#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/operators.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/python/tuple.hpp>

#include <string>

using namespace boost::python;

using std::string;

PXR_NAMESPACE_USING_DIRECTIVE

namespace {

static void
SetStartPointHelper( GfRay &self, const GfVec3d &startPoint ) {
    self.SetPointAndDirection( startPoint, self.GetDirection() );
}

static void
SetDirectionHelper( GfRay &self, const GfVec3d &direction ) {
    self.SetPointAndDirection( self.GetStartPoint(), direction );
}

static tuple
FindClosestPointHelper( const GfRay &self, const GfVec3d &point ) {
    double rayDist;
    GfVec3d result = self.FindClosestPoint( point, &rayDist );
    return make_tuple( result, rayDist );
}

static tuple
FindClosestPointsHelper1( const GfRay &l1, const GfLine &l2 )
{
    GfVec3d p1(0), p2(0);
    double t1 = 0.0, t2 = 0.0;
    bool result = GfFindClosestPoints( l1, l2, &p1, &p2, &t1, &t2 );
    return make_tuple( result, p1, p2, t1, t2 );
}

static tuple
FindClosestPointsHelper2( const GfRay &l1, const GfLineSeg &l2 )
{
    GfVec3d p1(0), p2(0);
    double t1 = 0.0, t2 = 0.0;
    bool result = GfFindClosestPoints( l1, l2, &p1, &p2, &t1, &t2 );
    return make_tuple( result, p1, p2, t1, t2 );
}

static tuple
IntersectHelper1( const GfRay &self, const GfVec3d &p0,
                  const GfVec3d &p1, const GfVec3d &p2 )
{
    double dist = 0;
    GfVec3d barycentricCoords(0);
    bool frontFacing = false;
    bool result =
        self.Intersect( p0, p1, p2, &dist, &barycentricCoords, &frontFacing );
    return make_tuple( result, dist, barycentricCoords, frontFacing );
}

static tuple
IntersectHelper2( const GfRay &self, const GfPlane &plane )
{
    double dist = 0;
    bool frontFacing = false;
    bool result = self.Intersect( plane, &dist, &frontFacing );
    return make_tuple( result, dist, frontFacing );
}

static tuple
IntersectHelper3( const GfRay &self, const GfRange3d &box )
{
    double enterDist = 0, exitDist = 0;
    bool result = self.Intersect( box, &enterDist, &exitDist );
    return make_tuple( result, enterDist, exitDist );
}

static tuple
IntersectHelper4( const GfRay &self, const GfVec3d& center, double radius )
{
    double enterDist = 0, exitDist = 0;
    bool result = self.Intersect( center, radius, &enterDist, &exitDist );
    return make_tuple( result, enterDist, exitDist );
}

static tuple
IntersectHelper5(const GfRay &self,
                 const GfVec3d &origin,
                 const GfVec3d &axis,
                 double radius)
{
    double enter = 0, exit = 0;
    bool result = self.Intersect(origin, axis, radius, &enter, &exit);
    return make_tuple(result, enter, exit);
}

static tuple
IntersectHelper6(const GfRay &self,
                 const GfVec3d &origin,
                 const GfVec3d &axis,
                 double radius,
                 double height)
{
    double enter = 0, exit = 0;
    bool result = self.Intersect(origin, axis, radius, height, &enter, &exit);
    return make_tuple(result, enter, exit);
}

static string _Repr(GfRay const &self) {
    return TF_PY_REPR_PREFIX + "Ray(" + TfPyRepr(self.GetStartPoint()) + ", " +
        TfPyRepr(self.GetDirection()) + ")";
}

} // anonymous namespace

void wrapRay()
{
    typedef GfRay This;

    def("FindClosestPoints", FindClosestPointsHelper1,
        "FindClosestPoints( r1, l2 ) -> tuple<intersects=bool, "
        "p1 = GfVec3d, p2 = GfVec3d, t1 = double, t2 = double>\n"
        "\n"
        "r1 : GfRay\n"
        "l2 : GfLine\n"
        "\n"
        "Computes the closest points between a ray and a line,\n"
        "returning a tuple. The first item in the tuple is true if "
        "they intersect. The two points are returned in p1 and p2.\n"
        "The parametric distance of each point on the ray and line is\n"
        "returned in t1 and t2.\n"
        "----------------------------------------------------------------------"
        );
    def("FindClosestPoints", FindClosestPointsHelper2,
        "FindClosestPoints( r1, s2 ) -> tuple<intersects = bool, "
        "p1 = GfVec3d, p2 = GfVec3d, t1 = double, t2 = double>\n"
        "\n"
        "r1 : GfRay\n"
        "s2 : GfLineSeg\n"
        "\n"
        "Computes the closest points between a ray and a line segment,\n"
        "returning a tuple. The first item in the tuple is true if "
        "they intersect. The two points are returned in p1 and p2.\n"
        "The parametric distance of each point on the ray and line\n"
        "segment is returned in t1 and t2.\n"
        "----------------------------------------------------------------------"
        );

    class_<This>("Ray", "", init<>())
        .def(init< const GfVec3d &, const GfVec3d & >())

        .def(TfTypePythonClass())

        .def("SetPointAndDirection",
             &This::SetPointAndDirection, return_self<>())
        .def("SetEnds", &This::SetEnds, return_self<>())

        .add_property( "startPoint", make_function
                       (&This::GetStartPoint,
                        return_value_policy<copy_const_reference>()),
                       SetStartPointHelper )
        .add_property( "direction", make_function
                       (&This::GetDirection,
                        return_value_policy<copy_const_reference>()),
                       SetDirectionHelper )

        .def("GetPoint", &This::GetPoint )

        .def("FindClosestPoint", FindClosestPointHelper)

        .def("Transform", &This::Transform, return_self<>())

        .def("Intersect", IntersectHelper1,
             "Intersect( p0, p1, p2 ) -> tuple<intersects = bool, dist =\n"
             "float, barycentric = GfVec3d, frontFacing = bool>\n"
             "\n"
             "Intersects the ray with the triangle formed by points p0,\n"
             "p1, and p2.  The first item in the tuple is true if the ray\n"
             "intersects the triangle. dist is the the parametric\n"
             "distance to the intersection point, the barycentric\n"
             "coordinates of the intersection point, and the front-facing\n"
             "flag. The barycentric coordinates are defined with respect\n"
             "to the three vertices taken in order.  The front-facing\n"
             "flag is True if the intersection hit the side of the\n"
             "triangle that is formed when the vertices are ordered\n"
             "counter-clockwise (right-hand rule).\n"
             "\n"
             "Barycentric coordinates are defined to sum to 1 and satisfy\n"
             "this relationsip:\n"
             "\n"
             "    intersectionPoint = (barycentricCoords[0] * p0 +\n"
             "                         barycentricCoords[1] * p1 +\n"
             "                         barycentricCoords[2] * p2);\n"
             "----------------------------------------------------------------------"
            )
        .def( "Intersect", IntersectHelper2,
              "Intersect( plane ) -> tuple<intersects = bool, dist = float,\n"
              "frontFacing = bool>\n"
              "\n"
              "Intersects the ray with the Gf.Plane.  The first item in\n"
              "the returned tuple is true if the ray intersects the plane.\n"
              "dist is the parametric distance to the intersection point\n"
              "and frontfacing is true if the intersection is on the side\n"
              "of the plane toward which the plane's normal points.\n"
             "----------------------------------------------------------------------"
            )
        .def( "Intersect", IntersectHelper3,
              "Intersect( range3d ) -> tuple<intersects = bool, enterDist\n"
              "= float, exitDist = float>\n"
              //\n"
              "Intersects the plane with an axis-aligned box in a\n"
              "Gf.Range3d.  intersects is true if the ray intersects it at\n"
              "all within bounds. If there is an intersection then enterDist\n"
              "and exitDist will be the parametric distances to the two\n"
              "intersection points.\n"
              "----------------------------------------------------------------------"
            )
        .def( "Intersect", IntersectHelper4,
              "Intersect( center, radius ) -> tuple<intersects = bool,\n"
              "enterDist = float, exitDist = float>\n"
              "\n"
              "Intersects the plane with an sphere. intersects is true if\n"
              "the ray intersects it at all within the sphere. If there is\n"
              "an intersection then enterDist and exitDist will be the\n"
              "parametric distances to the two intersection points.\n"
              "----------------------------------------------------------------------"
            )
        .def( "Intersect", IntersectHelper5,
              "Intersect( origin, axis, radius ) -> tuple<intersects = bool,\n"
              "enterDist = float, exitDist = float>\n"
              "\n"
              "Intersects the plane with an infinite cylinder. intersects\n"
              "is true if the ray intersects it at all within the\n"
              "sphere. If there is an intersection then enterDist and\n"
              "exitDist will be the parametric distances to the two\n"
              "intersection points.\n"
              "----------------------------------------------------------------------"
            )
        .def( "Intersect", IntersectHelper6,
              "Intersect( origin, axis, radius, height ) -> \n"
              "tuple<intersects = bool, enterDist = float, exitDist = float>\n"
              "\n"
              "Intersects the plane with an cylinder. intersects\n"
              "is true if the ray intersects it at all within the\n"
              "sphere. If there is an intersection then enterDist and\n"
              "exitDist will be the parametric distances to the two\n"
              "intersection points.\n"
              "----------------------------------------------------------------------"
            )

        .def( str(self) )
        .def( self == self )
        .def( self != self )

        .def("__repr__", _Repr)

        ;

}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver 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 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef IGREENSFUNCTION_HPP
#define IGREENSFUNCTION_HPP

#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

class Element;

#include "dielectric_profile/ProfileTypes.hpp"

/*! \file IGreensFunction.hpp
 *  \class IGreensFunction
 *  \brief Interface for Green's function classes
 *  \author Luca Frediani and Roberto Di Remigio
 *  \date 2012-2015
 *
 *  The Non-Virtual Interface (NVI) idiom is used. Notice also that some of the return
 *  types are in some cases "auto", meaning that we will let the compiler deduce
 *  them.
 */

/*! \typedef KernelS
 *  \brief functor handle to the kernelS method
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelS;

/*! \typedef KernelD
 *  \brief functor handle to the kernelD method
 */
typedef pcm::function<double(const Eigen::Vector3d &, const Eigen::Vector3d &, const Eigen::Vector3d &)> KernelD;

class IGreensFunction
{
  public:
    virtual ~IGreensFunction() {}
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    double kernelS(const Eigen::Vector3d & p1, const Eigen::Vector3d &p2) const {
      return kernelS_impl(p1, p2);
    }
    /*! Returns value of the kernel of the \f$\mathcal{D}\f$ integral operator for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    double kernelD(const Eigen::Vector3d & direction, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const {
      return kernelD_impl(direction, p1, p2);
    }

    KernelS exportKernelS() const {
      return exportKernelS_impl();
    }
    KernelD exportKernelD() const {
      return exportKernelD_impl();
    }

    /*! Whether the Green's function describes a uniform environment */
    virtual bool uniform() const = 0;
    /*! Returns a dielectric permittivity profile */
    virtual Permittivity permittivity() const = 0;

    /*! Calculates the matrix representation of the S operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd singleLayer(const std::vector<Element> & e) const = 0;
    /*! Calculates the matrix representation of the D operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd doubleLayer(const std::vector<Element> & e) const = 0;

    friend std::ostream & operator<<(std::ostream & os, IGreensFunction & gf) {
      return gf.printObject(os);
    }
  protected:
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    virtual double kernelS_impl(const Eigen::Vector3d & p1, const Eigen::Vector3d &p2) const = 0;
    /*! Returns value of the kernel of the \f$\mathcal{D}\f$ integral operator for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double kernelD_impl(const Eigen::Vector3d & direction,
        const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const = 0;
    virtual KernelS exportKernelS_impl() const = 0;
    virtual KernelD exportKernelD_impl() const = 0;
    virtual std::ostream & printObject(std::ostream & os) = 0;
};

#endif // IGREENSFUNCTION_HPP


#ifndef FLOW_H_
#define FLOW_H_
#include <Eigen/Core>
#include <list>
#include <map>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

using namespace boost;
using namespace Eigen;

typedef int EdgeWeightType;

// clang-format off
typedef adjacency_list_traits < vecS, vecS, directedS > Traits;
typedef adjacency_list < vecS, vecS, directedS,
    property < vertex_name_t, std::string,
    property < vertex_index_t, long,
    property < vertex_color_t, boost::default_color_type,
    property < vertex_distance_t, long,
    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,

    property < edge_capacity_t, EdgeWeightType,
    property < edge_residual_capacity_t, EdgeWeightType,
    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;
// clang-format on

void AddEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
             property_map<Graph, edge_reverse_t>::type& rev, const double capacity, Graph& g);

void AddDirectEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                   property_map<Graph, edge_reverse_t>::type& rev, const int capacity,
                   const int inv_capacity, Graph& g, Traits::edge_descriptor& e1,
                   Traits::edge_descriptor& e2);

class MaxFlowHelper {
   public:
    MaxFlowHelper() {}
    virtual ~MaxFlowHelper(){};
    virtual void resize(int n, int m) = 0;
    virtual int compute() = 0;
    virtual void AddEdge(int x, int y, int c, int rc, int v) = 0;
    virtual void Apply(std::vector<Vector2i>& edge_diff) = 0;
};

class BoykovMaxFlowHelper : public MaxFlowHelper {
   public:
    BoykovMaxFlowHelper() {
        rev = get(edge_reverse, g);
        num = 0;
    }
    int num;
    void resize(int n, int m) {
        vertex_descriptors.resize(n);
        for (int i = 0; i < n; ++i) {
            vertex_descriptors[i] = add_vertex(g);
        }
        num = n;
    }
    int compute() {
        EdgeWeightType flow =
            boykov_kolmogorov_max_flow(g, vertex_descriptors.front(), vertex_descriptors.back());
        return flow;
    }
    void AddEdge(int x, int y, int c, int rc, int v) {
        Traits::edge_descriptor e1, e2;
        AddDirectEdge(vertex_descriptors[x], vertex_descriptors[y], rev, c, rc, g, e1, e2);
        if (v != -1) {
            edge_to_variables[e1] = std::make_pair(v, -1);
            edge_to_variables[e2] = std::make_pair(v, 1);
        }
    }
    void Apply(std::vector<Vector2i>& edge_diff) {
        property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
        property_map<Graph, edge_residual_capacity_t>::type residual_capacity =
            get(edge_residual_capacity, g);

        graph_traits<Graph>::vertex_iterator u_iter, u_end;
        graph_traits<Graph>::out_edge_iterator ei, e_end;
        for (tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
            for (tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
                if (capacity[*ei] > 0) {
                    int flow = (capacity[*ei] - residual_capacity[*ei]);
                    if (flow > 0) {
                        auto it = edge_to_variables.find(*ei);
                        if (it != edge_to_variables.end()) {
                            edge_diff[it->second.first / 2][it->second.first % 2] +=
                                it->second.second * flow;
                        }
                        /*
                        int64_t key = (int64_t)*u_iter * num + target(*ei, g);
                        auto q = edge_to_variable[key];
                        edge_diff[q.first / 2][q.first % 2] += q.second * flow;
                        if (abs(edge_diff[q.first/2][q.first%2]) > 2) {
                            printf("Edge %d %d\n", *u_iter, target(*ei, g));
                            printf("Apply error 1: %d %d %d\n",
                                   edge_diff[q.first / 2][q.first % 2],
                                   q.second, flow);
                            printf("capacity %d\n", capacity[*ei]);
                            exit(0);
                        }
                         */
                    }
                }
    }
    Graph g;
    property_map<Graph, edge_reverse_t>::type rev;
    std::vector<Traits::vertex_descriptor> vertex_descriptors;
    std::map<Traits::edge_descriptor, std::pair<int, int>> edge_to_variables;
};

int flow(std::vector<std::map<int, std::pair<int, int>>>& graph);

inline void AddEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                    property_map<Graph, edge_reverse_t>::type& rev, const double capacity,
                    Graph& g) {
    Traits::edge_descriptor e1 = add_edge(v1, v2, g).first;
    Traits::edge_descriptor e2 = add_edge(v2, v1, g).first;
    put(edge_capacity, g, e1, capacity);
    put(edge_capacity, g, e2, capacity);

    rev[e1] = e2;
    rev[e2] = e1;
}

inline void AddDirectEdge(Traits::vertex_descriptor& v1, Traits::vertex_descriptor& v2,
                          property_map<Graph, edge_reverse_t>::type& rev, const int capacity,
                          const int inv_capacity, Graph& g, Traits::edge_descriptor& e1,
                          Traits::edge_descriptor& e2) {
    e1 = add_edge(v1, v2, g).first;
    e2 = add_edge(v2, v1, g).first;
    put(edge_capacity, g, e1, capacity);
    put(edge_capacity, g, e2, inv_capacity);

    rev[e1] = e2;
    rev[e2] = e1;
}

class ECMaxFlowHelper : public MaxFlowHelper {
   public:
    struct FlowInfo {
        int id;
        int capacity, flow;
        int v, d;
        FlowInfo* rev;
    };
    struct SearchInfo {
        SearchInfo(int _id, int _prev_id, FlowInfo* _info)
            : id(_id), prev_id(_prev_id), info(_info) {}
        int id;
        int prev_id;
        FlowInfo* info;
    };
    ECMaxFlowHelper() { num = 0; }
    int num;
    std::vector<FlowInfo*> variable_to_edge;
    void resize(int n, int m) {
        graph.resize(n);
        variable_to_edge.resize(m, 0);
        num = n;
    }
    void AddEdge(int x, int y, int c, int rc, int v) {
        FlowInfo flow;
        flow.id = y;
        flow.capacity = c;
        flow.flow = 0;
        flow.v = v;
        flow.d = -1;
        graph[x].push_back(flow);
        auto& f1 = graph[x].back();
        flow.id = x;
        flow.capacity = rc;
        flow.flow = 0;
        flow.v = v;
        flow.d = 1;
        graph[y].push_back(flow);
        auto& f2 = graph[y].back();
        f2.rev = &f1;
        f1.rev = &f2;
    }

    void ApplyFlow(int v1, int v2, int flow) {
        for (auto& it : graph[v1]) {
            if (it.id == v2) {
                it.flow += flow;
                break;
            }
        }
    }
    int compute() {
        int total_flow = 0;
        int count = 0;
        while (true) {
            count += 1;
            std::vector<int> vhash(num, 0);
            std::vector<SearchInfo> q;
            q.push_back(SearchInfo(0, -1, 0));
            vhash[0] = 1;
            int q_front = 0;
            bool found = false;
            while (q_front < q.size()) {
                int vert = q[q_front].id;
                for (auto& l : graph[vert]) {
                    if (vhash[l.id] || l.capacity <= l.flow) continue;
                    q.push_back(SearchInfo(l.id, q_front, &l));
                    vhash[l.id] = 1;
                    if (l.id == num - 1) {
                        found = true;
                        break;
                    }
                }
                if (found) break;
                q_front += 1;
            }
            if (q_front == q.size()) break;
            int loc = q.size() - 1;
            while (q[loc].prev_id != -1) {
                q[loc].info->flow += 1;
                q[loc].info->rev->flow -= 1;
                loc = q[loc].prev_id;
                //                int prev_v = q[loc].id;
                //                ApplyFlow(prev_v, current_v, 1);
                //                ApplyFlow(current_v, prev_v, -1);
            }
            total_flow += 1;
        }
        return total_flow;
    }
    void Apply(std::vector<Vector2i>& edge_diff) {
        for (int i = 0; i < graph.size(); ++i) {
            for (auto& flow : graph[i]) {
                if (flow.flow > 0 && flow.v != -1) {
                    if (flow.flow > 0) {
                        edge_diff[flow.v / 2][flow.v % 2] += flow.d * flow.flow;
                        if (abs(edge_diff[flow.v / 2][flow.v % 2]) > 2) {
                        }
                    }
                }
            }
        }
    }
    std::vector<std::list<FlowInfo>> graph;
};
#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * 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 "Lagrangian2d1DR.hpp"
#include <boost/math/quaternion.hpp>
#include "NewtonEulerDS.hpp"
#include "Interaction.hpp"
#include "BlockVector.hpp"
#include <boost/math/quaternion.hpp>

// #define DEBUG_NOCOLOR
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "siconos_debug.h"

void Lagrangian2d1DR::initialize(Interaction& inter)
{
  LagrangianR::initialize(inter);
  //proj_with_q  _jachqProj.reset(new SimpleMatrix(_jachq->size(0),_jachq->size(1)));

  if((inter.getSizeOfDS() !=3) and (inter.getSizeOfDS() !=6))
  {
    THROW_EXCEPTION("Lagrangian2d1DR::initialize(Interaction& inter). The size of ds must of size 3");
  }
  unsigned int qSize = 3 * (inter.getSizeOfDS() / 3);
  _jachq.reset(new SimpleMatrix(1, qSize));
}

void Lagrangian2d1DR::computeJachq(const BlockVector& q, BlockVector& z)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0 \n");

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double G1x = q.getValue(0);
  double G1y = q.getValue(1);

  _jachq->setValue(0,0,Nx);
  _jachq->setValue(0,1,Ny);
  _jachq->setValue(0,2,(G1y-Py)*Nx - (G1x-Px)*Ny);

  if(q.size() ==6)
  {
    DEBUG_PRINT("take into account second ds\n");
    double G2x = q.getValue(3);
    double G2y = q.getValue(4);

    _jachq->setValue(0,3,-Nx);
    _jachq->setValue(0,4,-Ny);
    _jachq->setValue(0,5,- ((G2y-Py)*Nx - (G2x-Px)*Ny));
  }
  DEBUG_EXPR(_jachq->display(););
  DEBUG_END("Lagrangian2d1DR::computeJachq(Interaction& inter, SP::BlockVector q0) \n");

}

double Lagrangian2d1DR::distance() const
{
  DEBUG_BEGIN("Lagrangian2d1DR::distance(...)\n")
  SiconosVector dpc(*_Pc2 - *_Pc1);
  DEBUG_END("Lagrangian2d1DR::distance(...)\n")
  return dpc.norm2() * (inner_prod(*_Nc, dpc) >= 0 ? -1 : 1);

}

void Lagrangian2d1DR::computeh(const BlockVector& q, BlockVector& z, SiconosVector& y)
{
  DEBUG_BEGIN("Lagrangian2d1DR::computeh(...)\n");
  DEBUG_EXPR(q.display());

  LagrangianScleronomousR::computeh(q, z, y);
  y.setValue(0, distance());
  DEBUG_EXPR(y.display(););
  DEBUG_EXPR(display(););
  DEBUG_END("Lagrangian2d1DR::computeh(...)\n")
}
void Lagrangian2d1DR::display() const
{
  LagrangianR::display();

  std::cout << " _Pc1 :" << std::endl;
  if(_Pc1)
    _Pc1->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Pc2 :" << std::endl;
  if(_Pc2)
    _Pc2->display();
  else
    std::cout << " nullptr :" << std::endl;

  std::cout << " _Nc :" << std::endl;
  if(_Nc)
    _Nc->display();
  else
    std::cout << " nullptr :" << std::endl;
  std::cout << " _relNc :" << std::endl;
  if(_relNc)
    _relNc->display();
  else
    std::cout << " nullptr :" << std::endl;

}


// 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 <bits/stdc++.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#define EIGEN_MPL2_ONLY
#include "multi_object_tracker/tracker/model/unknown_tracker.hpp"
#include "multi_object_tracker/utils/utils.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <autoware_utils/autoware_utils.hpp>

UnknownTracker::UnknownTracker(
  const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object)
: Tracker(time, object.classification),
  logger_(rclcpp::get_logger("UnknownTracker")),
  last_update_time_(time),
  z_(object.kinematics.pose_with_covariance.pose.position.z)
{
  object_ = object;

  // initialize params
  ekf_params_.use_measurement_covariance = false;
  float q_stddev_x = 0.0;                              // [m/s]
  float q_stddev_y = 0.0;                              // [m/s]
  float q_stddev_vx = autoware_utils::kmph2mps(0.1);   // [m/(s*s)]
  float q_stddev_vy = autoware_utils::kmph2mps(0.1);   // [m/(s*s)]
  float r_stddev_x = 0.4;                              // [m]
  float r_stddev_y = 0.4;                              // [m]
  float p0_stddev_x = 1.0;                             // [m/s]
  float p0_stddev_y = 1.0;                             // [m/s]
  float p0_stddev_vx = autoware_utils::kmph2mps(0.1);  // [m/(s*s)]
  float p0_stddev_vy = autoware_utils::kmph2mps(0.1);  // [m/(s*s)]
  ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
  ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
  ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
  ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0);
  ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
  ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
  ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
  ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
  ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
  ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0);
  max_vx_ = autoware_utils::kmph2mps(5);  // [m/s]
  max_vy_ = autoware_utils::kmph2mps(5);  // [m/s]

  // initialize X matrix
  Eigen::MatrixXd X(ekf_params_.dim_x, 1);
  X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
  X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
  if (object.kinematics.has_twist) {
    X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x;
    X(IDX::VY) = object.kinematics.twist_with_covariance.twist.linear.y;
  } else {
    X(IDX::VX) = 0.0;
    X(IDX::VY) = 0.0;
  }

  // initialize P matrix
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  if (
    !ekf_params_.use_measurement_covariance ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0) {
    // Rotate the covariance matrix according to the vehicle yaw
    // because p0_cov_x and y are in the vehicle coordinate system.
    P(IDX::X, IDX::X) = ekf_params_.p0_cov_x;
    P(IDX::X, IDX::Y) = 0.0;
    P(IDX::Y, IDX::Y) = ekf_params_.p0_cov_y;
    P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y);
    P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
    P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy;
  } else {
    P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
    P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
    P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
    P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X];
    if (object.kinematics.has_twist_covariance) {
      P(IDX::VX, IDX::VX) =
        object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
      P(IDX::VY, IDX::VY) =
        object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
    } else {
      P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
      P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy;
    }
  }

  ekf_.init(X, P);
}

bool UnknownTracker::predict(const rclcpp::Time & time)
{
  const double dt = (time - last_update_time_).seconds();
  bool ret = predict(dt, ekf_);
  if (ret) {
    last_update_time_ = time;
  }
  return ret;
}

bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const
{
  /*  == Nonlinear model ==
   *
   * x_{k+1}   = x_k + vx_k * dt
   * y_{k+1}   = y_k + vx_k * dt
   * vx_{k+1}  = vx_k
   * vy_{k+1}  = vy_k
   *
   */

  /*  == Linearized model ==
   *
   * A = [ 1, 0, dt,  0]
   *     [ 0, 1,  0, dt]
   *     [ 0, 0,  1,  0]
   *     [ 0, 0,  0,  1]
   */

  // X t
  Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);  // predicted state
  ekf.getX(X_t);

  // X t+1
  Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1);  // predicted state
  X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt;
  X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt;
  X_next_t(IDX::VX) = X_t(IDX::VX);
  X_next_t(IDX::VY) = X_t(IDX::VY);

  // A
  Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
  A(IDX::X, IDX::VX) = dt;
  A(IDX::Y, IDX::VY) = dt;

  // Q
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  // Rotate the covariance matrix according to the vehicle yaw
  // because q_cov_x and y are in the vehicle coordinate system.
  Q(IDX::X, IDX::X) = ekf_params_.q_cov_x * dt * dt;
  Q(IDX::X, IDX::Y) = 0.0;
  Q(IDX::Y, IDX::Y) = ekf_params_.q_cov_y * dt * dt;
  Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y);
  Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt;
  Q(IDX::VY, IDX::VY) = ekf_params_.q_cov_vy * dt * dt;
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);

  if (!ekf.predict(X_next_t, A, Q)) {
    RCLCPP_WARN(logger_, "Pedestrian : Cannot predict");
  }

  return true;
}

bool UnknownTracker::measureWithPose(
  const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
  constexpr int dim_y = 2;  // pos x, pos y depending on Pose output

  /* Set measurement matrix */
  Eigen::MatrixXd Y(dim_y, 1);
  Y << object.kinematics.pose_with_covariance.pose.position.x,
    object.kinematics.pose_with_covariance.pose.position.y;

  /* Set measurement matrix */
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
  C(0, IDX::X) = 1.0;  // for pos x
  C(1, IDX::Y) = 1.0;  // for pos y

  /* Set measurement noise covariance */
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
  if (
    !ekf_params_.use_measurement_covariance ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0) {
    R(0, 0) = ekf_params_.r_cov_x;  // x - x
    R(0, 1) = 0.0;                  // x - y
    R(1, 1) = ekf_params_.r_cov_y;  // y - y
    R(1, 0) = R(0, 1);              // y - x
  } else {
    R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
    R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
    R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X];
    R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
  }
  if (!ekf_.update(Y, C, R)) {
    RCLCPP_WARN(logger_, "Pedestrian : Cannot update");
  }

  // limit vx, vy
  {
    Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);
    Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x);
    ekf_.getX(X_t);
    ekf_.getP(P_t);
    if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) {
      X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_;
    }
    if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) {
      X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_;
    }
    ekf_.init(X_t, P_t);
  }

  // position z
  constexpr float gain = 0.9;
  z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z;

  return true;
}

bool UnknownTracker::measure(
  const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time)
{
  object_ = object;

  if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) {
    RCLCPP_WARN(
      logger_,
      "Pedestrian : There is a large gap between predicted time and measurement time. (%f)",
      (time - last_update_time_).seconds());
  }

  measureWithPose(object);

  return true;
}

bool UnknownTracker::getTrackedObject(
  const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
{
  object = utils::toTrackedObject(object_);
  object.object_id = getUUID();
  object.classification = getClassification();

  // predict kinematics
  KalmanFilter tmp_ekf_for_no_update = ekf_;
  const double dt = (time - last_update_time_).seconds();
  if (0.001 /*1msec*/ < dt) {
    predict(dt, tmp_ekf_for_no_update);
  }
  Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);                // predicted state
  Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x);  // predicted state
  tmp_ekf_for_no_update.getX(X_t);
  tmp_ekf_for_no_update.getP(P);

  // position
  object.kinematics.pose_with_covariance.pose.position.x = X_t(IDX::X);
  object.kinematics.pose_with_covariance.pose.position.y = X_t(IDX::Y);
  object.kinematics.pose_with_covariance.pose.position.z = z_;
  // position covariance
  constexpr double z_cov = 0.1 * 0.1;    // TODO(yukkysaito) Currently tentative
  constexpr double r_cov = 0.1 * 0.1;    // TODO(yukkysaito) Currently tentative
  constexpr double p_cov = 0.1 * 0.1;    // TODO(yukkysaito) Currently tentative
  constexpr double yaw_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov;

  // twist
  object.kinematics.twist_with_covariance.twist.linear.x = X_t(IDX::VX);
  object.kinematics.twist_with_covariance.twist.linear.y = X_t(IDX::VY);
  // twist covariance
  constexpr double vz_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double wx_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double wy_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double wz_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::VY, IDX::VY);
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov;

  return true;
}


/*
*   Copyright 2019 Simon Raschke
*
*   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.
*/

#pragma once

#include "common/definitions.hpp"
#include "common/component.hpp"

#if __has_include(<eigen3/Eigen/Geometry>)
    #include <eigen3/Eigen/Geometry>
#elif __has_include(<Eigen/Geometry>)
    #include <Eigen/Geometry>
#else
    #pragma error "no eigen include"
#endif
#include <cmath>
#include <memory>
#include <exception>

namespace ves
{
    struct Bounding;
    struct CoordinatesBounding;
    struct OrientationBounding;
}

struct ves::Bounding
{
protected:
    Bounding() = default;

public:
    std::unique_ptr<Eigen::Matrix<REAL,3,1>> origin {nullptr};

    virtual ~Bounding() = default;
    virtual bool isAllowed(const decltype(origin)::element_type&) = 0;
    virtual void forcefullyShift(const Eigen::Matrix<REAL,3,1>&) = 0;
};

struct ves::OrientationBounding
    : public Bounding
{
    REAL value = TWOPI;

    inline virtual bool isAllowed(const decltype(origin)::element_type& compare) override
    {
        if(!origin)
        {
            origin = std::make_unique<decltype(origin)::element_type>(compare);
            return true;
        }
        else
        {
            return (std::acos(compare.normalized().dot(origin->normalized())) < value);
            const bool allowed = (std::acos(compare.normalized().dot(origin->normalized())) < value);
            if(allowed)
            {
                origin = std::make_unique<decltype(origin)::element_type>(compare);
            }
            return allowed;
        }
    };

    virtual void forcefullyShift(const Eigen::Matrix<REAL,3,1>& s) override
    {
        if(origin)
            origin->operator+=(s);
    }
};

struct ves::CoordinatesBounding
    : public Bounding
{
    inline virtual bool isAllowed(const decltype(origin)::element_type& compare) override
    {
        if(!origin)
        {
            bool allowed = false;
            if(bounding_box)
            {
                allowed = bounding_box->contains(compare);
            }
            else if(sphere_bounds)
            {
                const auto offset = (compare - (*origin)).norm();
                allowed = offset >= sphere_bounds->first && offset <= sphere_bounds->second;
            }
            else if(!bounding_box && !sphere_bounds)
            {
                allowed = true;
            }
            else
            {
                vesCRITICAL(__PRETTY_FUNCTION__ << " bad decision");
                allowed = false;
            }

            if(allowed)
                origin = std::make_unique<decltype(origin)::element_type>(compare);

            return allowed;
        }
        else
        {
            if(bounding_box)
            {
                return bounding_box->contains(compare);
            }
            else if(sphere_bounds)
            {
                const auto offset = (compare - (*origin)).norm();
                return offset >= sphere_bounds->first && offset <= sphere_bounds->second;
            }
            else if(!bounding_box && !sphere_bounds)
            {
                return true;
            }
            else
            {
                vesCRITICAL(__PRETTY_FUNCTION__ << " bad decision");
                return false;
            }
        }
    };

    inline void setBoundingSphere(const REAL from, const REAL to)
    {
        sphere_bounds = std::make_unique<std::pair<REAL,REAL>>(std::make_pair(from,to));
        bounding_box.reset(nullptr);
    };

    inline void setBoundingBox(Eigen::AlignedBox<REAL,3>&& _box)
    {
        bounding_box = std::make_unique<Eigen::AlignedBox<REAL,3>>(std::move(_box));
        sphere_bounds.reset(nullptr);
    };

    virtual void forcefullyShift(const Eigen::Matrix<REAL,3,1>& s) override
    {
        if(origin)
            origin->operator+=(s);
        if(bounding_box)
            bounding_box->translate(s);
    }

    inline bool isBoxBound() const { return static_cast<bool>(bounding_box); };
    inline bool isSphereBound() const { return static_cast<bool>(sphere_bounds); };

    inline auto getBoundingBox() const { return *bounding_box; };
    inline auto getSphereBounds() const { return *sphere_bounds; };

protected:
    std::unique_ptr<Eigen::AlignedBox<REAL,3>> bounding_box = {nullptr};
    std::unique_ptr<std::pair<REAL,REAL>> sphere_bounds = {nullptr};
};