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

#define BOOST_TEST_MAIN

#include <limits>

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

#include "Tudat/Basics/testMacros.h"

#include "Tudat/SimulationSetup/tudatSimulationHeader.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_aerodynamic_coefficients_from_file )

BOOST_AUTO_TEST_CASE( testAerodynamicCoefficientsFromFile )
{
    using namespace tudat;
    using namespace ephemerides;
    using namespace interpolators;
    using namespace numerical_integrators;
    using namespace spice_interface;
    using namespace simulation_setup;
    using namespace basic_astrodynamics;
    using namespace orbital_element_conversions;
    using namespace propagators;
    using namespace aerodynamics;
    using namespace basic_mathematics;
    using namespace input_output;
    using namespace unit_conversions;

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ENVIRONMENT            //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    for( unsigned int i = 0; i < 3; i++ )
    {
        // Set simulation start epoch.
        const double simulationStartEpoch = 0.0;

        // Set simulation end epoch.
        const double simulationEndEpoch = 300.0;

        // Set numerical integration fixed step size.
        const double fixedStepSize = 1.0;

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////            CREATE ENVIRONMENT            //////////////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Define simulation body settings.
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( { "Earth" }, simulationStartEpoch - 10.0 * fixedStepSize,
                                        simulationEndEpoch + 10.0 * fixedStepSize );
        bodySettings[ "Earth" ]->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                    Eigen::Vector6d::Zero( ), "SSB", "J2000" );
        bodySettings[ "Earth" ]->rotationModelSettings->resetOriginalFrame( "J2000" );

        // Create Earth object
        simulation_setup::NamedBodyMap bodyMap = simulation_setup::createBodies( bodySettings );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             CREATE VEHICLE            /////////////////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Create vehicle objects.
        bodyMap[ "SpacePlane" ] = std::make_shared< simulation_setup::Body >( );
        bodyMap[ "SpacePlane" ]->setVehicleSystems( std::make_shared< system_models::VehicleSystems >( ) );
        bodyMap[ "SpacePlane" ]->getVehicleSystems( )->setCurrentControlSurfaceDeflection( "TestSurface", 0.1 );

        // Create vehicle  coefficients
        std::map< int, std::string > forceCoefficientFiles;
        forceCoefficientFiles[ 0 ] = tudat::input_output::getTudatRootPath( )
                + "/Astrodynamics/Aerodynamics/UnitTests/aurora_CD.txt";
        forceCoefficientFiles[ 2 ] = tudat::input_output::getTudatRootPath( )
                + "/Astrodynamics/Aerodynamics/UnitTests/aurora_CL.txt";
        std::map< int, std::string > momentCoefficientFiles;
        momentCoefficientFiles[ 1 ] = tudat::input_output::getTudatRootPath( )
                + "/Astrodynamics/Aerodynamics/UnitTests/aurora_Cm.txt";
        std::map< int, std::string > controlSurfaceForceCoefficientFiles;
        controlSurfaceForceCoefficientFiles[ 0 ] = tudat::input_output::getTudatRootPath( )
                + "/Astrodynamics/Aerodynamics/UnitTests/dCDwTest.txt";
        std::map< int, std::string > controlSurfaceMomentCoefficientFiles;
        controlSurfaceMomentCoefficientFiles[ 1 ] = tudat::input_output::getTudatRootPath( )
                + "/Astrodynamics/Aerodynamics/UnitTests/dCDwTest.txt";

        std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings =
                simulation_setup::readTabulatedAerodynamicCoefficientsFromFiles(
                    forceCoefficientFiles, momentCoefficientFiles, 60.734, 600.0, 60.734, Eigen::Vector3d::Zero( ),
        { aerodynamics::mach_number_dependent, aerodynamics::angle_of_attack_dependent },
                    true, true );
        if( i == 1 )
        {
            aerodynamicCoefficientSettings->setControlSurfaceSettings(
                        simulation_setup::readTabulatedControlIncrementAerodynamicCoefficientsFromFiles(
                            controlSurfaceForceCoefficientFiles, controlSurfaceMomentCoefficientFiles,
            { aerodynamics::mach_number_dependent, aerodynamics::angle_of_attack_dependent, aerodynamics::control_surface_deflection_dependent } ),
                        "TestSurface" );
        }
        else if( i == 2 )
        {
            aerodynamicCoefficientSettings->setControlSurfaceSettings(
                        simulation_setup::readTabulatedControlIncrementAerodynamicCoefficientsFromFiles(
                            controlSurfaceForceCoefficientFiles,
            { aerodynamics::mach_number_dependent, aerodynamics::angle_of_attack_dependent, aerodynamics::control_surface_deflection_dependent } ),
                        "TestSurface" );
        }

        bodyMap[ "SpacePlane" ]->setAerodynamicCoefficientInterface(
                    createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "SpacePlane" ) );

        bodyMap[ "SpacePlane" ]->setConstantBodyMass( 50.0E3 );

        // Finalize body creation.
        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             CREATE ACCELERATIONS            ///////////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        // Define propagator settings variables.
        SelectedAccelerationMap accelerationMap;
        std::vector< std::string > bodiesToPropagate;
        std::vector< std::string > centralBodies;

        ////////////////////////////////////// Variable thrust calculations /////////////////////////////////////////////
        // Define thrustdependent variables
        std::vector< propulsion::ThrustIndependentVariables > thrustDependencies;
        thrustDependencies.push_back( propulsion::mach_number_dependent_thrust );
        thrustDependencies.push_back( propulsion::dynamic_pressure_dependent_thrust );

        // Read tables with thrust and specific impulse as function of dependent variables
        std::pair< boost::multi_array< double, 2 >, std::vector< std::vector< double > > > thrustValues =
                MultiArrayFileReader< 2 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Propulsion/UnitTests/Tmax_test.txt" );
        std::pair< boost::multi_array< double, 2 >, std::vector< std::vector< double > > > specificImpulseValues =
                MultiArrayFileReader< 2 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Propulsion/UnitTests/Isp_test.txt" );

        // Use multilinear interpolation to find Thrust and specific impulse from tables at current values
        // of thrustdpendent variables
        std::shared_ptr< interpolators::Interpolator< double, double > > thrustMagnitudeInterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 2 > >(
                    thrustValues.second, thrustValues.first );
        std::shared_ptr< interpolators::Interpolator< double, double > > specificImpulseInterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 2 > >(
                    specificImpulseValues.second, specificImpulseValues.first );

        //////////////////////////////////////// End Variable thrust calculations /////////////////////////////////////////////

        // Define acceleration model settings.
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSpacePlane;
        accelerationsOfSpacePlane[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 2, 0 ) );
        accelerationsOfSpacePlane[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) );

        accelerationsOfSpacePlane[ "SpacePlane" ].push_back( std::make_shared< ThrustAccelerationSettings >(
                                                                 std::make_shared< ThrustDirectionGuidanceSettings >(
                                                                     thrust_direction_from_existing_body_orientation, "Earth" ),
                                                                 std::make_shared< ParameterizedThrustMagnitudeSettings >(
                                                                     thrustMagnitudeInterpolator, thrustDependencies,
                                                                     specificImpulseInterpolator, thrustDependencies ) ) );

        accelerationMap[ "SpacePlane" ] = accelerationsOfSpacePlane;

        bodiesToPropagate.push_back( "SpacePlane" );
        centralBodies.push_back( "Earth" );

        // Create acceleration models
        basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToPropagate, centralBodies );
        setTrimmedConditions( bodyMap.at( "SpacePlane" ) );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Set spherical elements for SpacePlane.
        Eigen::Vector6d SpacePlaneInitialState;
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::radiusIndex ) = spice_interface::getAverageRadius( "Earth" ) + 2.0E3;
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::latitudeIndex ) = convertDegreesToRadians( 0.0 );
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::longitudeIndex ) = convertDegreesToRadians( 0.0 );
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::speedIndex ) = 170;
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::flightPathIndex ) = convertDegreesToRadians( 20.0 );
        SpacePlaneInitialState( SphericalOrbitalStateElementIndices::headingAngleIndex ) = convertDegreesToRadians( 90.0 );

        // Convert SpacePlane state from spherical elements to Cartesian elements.
        const Eigen::Vector6d systemInitialState = convertSphericalOrbitalToCartesianState(
                    SpacePlaneInitialState );

        // Define list of dependent variables to save.
        std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables;
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >( mach_number_dependent_variable, "SpacePlane" ) );
        dependentVariables.push_back(
                    std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                        "SpacePlane", reference_frames::angle_of_attack ) );
        dependentVariables.push_back(
                    std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                        "SpacePlane", reference_frames::angle_of_sideslip ) );
        dependentVariables.push_back(
                    std::make_shared< BodyAerodynamicAngleVariableSaveSettings >(
                        "SpacePlane", reference_frames::bank_angle ) );
        if( i > 0 )
        {
            dependentVariables.push_back(
                        std::make_shared< SingleDependentVariableSaveSettings >(
                            control_surface_deflection_dependent_variable, "SpacePlane", "TestSurface" ) );
        }
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        aerodynamic_moment_coefficients_dependent_variable, "SpacePlane" ) );
        dependentVariables.push_back(
                    std::make_shared< SingleDependentVariableSaveSettings >(
                        aerodynamic_force_coefficients_dependent_variable, "SpacePlane" ) );

        // Create total propagatortermination settings.
        std::shared_ptr< PropagationTerminationSettings > propagationTerminationSettings;

        std::vector< std::shared_ptr< PropagationTerminationSettings > >  propagationTerminationSettingsList;
        propagationTerminationSettingsList.push_back(
                    std::make_shared< PropagationDependentVariableTerminationSettings >(
                        std::make_shared< SingleDependentVariableSaveSettings >(
                            altitude_dependent_variable, "SpacePlane", "Earth" ), 120.0E3, false ) );
        propagationTerminationSettingsList.push_back(
                    std::make_shared< PropagationDependentVariableTerminationSettings >(
                        std::make_shared< SingleDependentVariableSaveSettings >(
                            altitude_dependent_variable, "SpacePlane", "Earth" ), 1.0E3, true ) );
        propagationTerminationSettingsList.push_back(
                    std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

        \
        propagationTerminationSettings = std::make_shared< PropagationHybridTerminationSettings >(
                    propagationTerminationSettingsList, true );

        // Create propagation settings.
        std::shared_ptr< TranslationalStatePropagatorSettings < double > > propagatorSettings =
                std::make_shared< TranslationalStatePropagatorSettings< double > >
                ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, propagationTerminationSettings,
                  cowell, std::make_shared< DependentVariableSaveSettings >( dependentVariables ) );

        std::shared_ptr< IntegratorSettings< > > integratorSettings =
                std::make_shared< IntegratorSettings< > >
                ( rungeKutta4, simulationStartEpoch, fixedStepSize );

        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
        ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

        // Create simulation object and propagate dynamics.
        SingleArcDynamicsSimulator< > dynamicsSimulator(
                    bodyMap, integratorSettings, propagatorSettings, true, false, false );

        // Retrieve numerical solutions for state and dependent variables
        std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > numericalSolution =
                dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
        std::map< double, Eigen::VectorXd > dependentVariableSolution =
                dynamicsSimulator.getDependentVariableHistory( );

        // Iterate over results for dependent variables, and check against manually retrieved values.
        std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > apolloCoefficientInterface =
                bodyMap.at( "SpacePlane" )->getAerodynamicCoefficientInterface( );

        std::pair< boost::multi_array< double, 2 >, std::vector< std::vector< double > > > cdMuliArray =
                tudat::input_output::MultiArrayFileReader< 2 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Aerodynamics/UnitTests/aurora_CD.txt" );
        std::shared_ptr< interpolators::Interpolator< double, double > > cdInterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 2 > >(
                    cdMuliArray.second, cdMuliArray.first );

        std::pair< boost::multi_array< double, 2 >, std::vector< std::vector< double > > > clMuliArray =
                tudat::input_output::MultiArrayFileReader< 2 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Aerodynamics/UnitTests/aurora_CL.txt" );
        std::shared_ptr< interpolators::Interpolator< double, double > > clInterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 2 > >(
                    clMuliArray.second, clMuliArray.first );

        std::pair< boost::multi_array< double, 2 >, std::vector< std::vector< double > > >  cmMuliArray =
                tudat::input_output::MultiArrayFileReader< 2 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Aerodynamics/UnitTests/aurora_Cm.txt" );
        std::shared_ptr< interpolators::Interpolator< double, double > > cmInterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 2 > >(
                    cmMuliArray.second, cmMuliArray.first );

        std::pair< boost::multi_array< double, 3 >, std::vector< std::vector< double > > >  controlSurfaceIncrementMuliArray =
                tudat::input_output::MultiArrayFileReader< 3 >::readMultiArrayAndIndependentVariables(
                    tudat::input_output::getTudatRootPath( ) + "/Astrodynamics/Aerodynamics/UnitTests/dCDwTest.txt" );
        std::shared_ptr< interpolators::Interpolator< double, double > > controlSurfaceIncrementinterpolator =
                std::make_shared< interpolators::MultiLinearInterpolator< double, double, 3 > >(
                    controlSurfaceIncrementMuliArray.second, controlSurfaceIncrementMuliArray.first );

        int parameterAddition = 0;
        if( i > 0 )
        {
            parameterAddition++;
        }
        for( std::map< double, Eigen::VectorXd >::iterator variableIterator = dependentVariableSolution.begin( );
             variableIterator != dependentVariableSolution.end( ); variableIterator++ )
        {
            double machNumber = variableIterator->second( 0 );
            double angleOfAttack = variableIterator->second( 1 );
            double sideslipAngle = variableIterator->second( 2 );
            double bankAngle = variableIterator->second( 3 );

            double controlSurfaceDeflection  = 0.0;
            if( i > 0 )
            {
                controlSurfaceDeflection = variableIterator->second( 4 );
            }

            Eigen::Vector3d momentCoefficients = variableIterator->second.segment( 4 + parameterAddition, 3 );
            Eigen::Vector3d forceCoefficients = variableIterator->second.segment( 7 + parameterAddition, 3 );

            std::vector< double > aerodynamicCoefficientInput;
            aerodynamicCoefficientInput.push_back( machNumber );
            aerodynamicCoefficientInput.push_back( angleOfAttack );

            std::map< std::string, std::vector< double > > controlSurfaceCoefficientInput;
            if( i > 0 )
            {
                controlSurfaceCoefficientInput[ "TestSurface" ] = aerodynamicCoefficientInput;
                controlSurfaceCoefficientInput[ "TestSurface" ].push_back( controlSurfaceDeflection );
            }

            apolloCoefficientInterface->updateFullCurrentCoefficients(
                        aerodynamicCoefficientInput, controlSurfaceCoefficientInput );
            Eigen::Vector3d computedForceCoefficients = apolloCoefficientInterface->getCurrentForceCoefficients( );
            Eigen::Vector3d computedMomentCoefficients = apolloCoefficientInterface->getCurrentMomentCoefficients( );

            BOOST_CHECK_SMALL( std::fabs( computedForceCoefficients( 1 ) ), 1.0E-14 );

            // Check force and moment coefficients from output to direct computation from interface
            for( unsigned int j = 0; j < 3; j ++ )
            {
                BOOST_CHECK_SMALL(
                            std::fabs( computedForceCoefficients( j ) - forceCoefficients( j ) ),
                            6.0 * std::numeric_limits< double >::epsilon( ) );
                BOOST_CHECK_SMALL(
                            std::fabs( computedMomentCoefficients( j ) - momentCoefficients( j ) ),
                            6.0 * std::numeric_limits< double >::epsilon( ) );
            }

            // Check trimmed condition (y-term)/symmetric vehicle shape (x- and z-term).
            BOOST_CHECK_EQUAL( momentCoefficients( 0 ), 0.0 );
            BOOST_CHECK_SMALL( std::fabs( momentCoefficients( 1 ) ), 1.0E-16 );
            BOOST_CHECK_EQUAL( momentCoefficients( 2 ), 0.0 );

            // Check if coefficients are correctly parsed from files
            double directCd = cdInterpolator->interpolate( aerodynamicCoefficientInput );
            if( i > 0 )
            {
                directCd += controlSurfaceIncrementinterpolator->interpolate( controlSurfaceCoefficientInput[ "TestSurface" ] );
            }
            double directCl = clInterpolator->interpolate( aerodynamicCoefficientInput );
            double directCm = cmInterpolator->interpolate( aerodynamicCoefficientInput );
            if( i == 1 )
            {
                directCm += controlSurfaceIncrementinterpolator->interpolate( controlSurfaceCoefficientInput[ "TestSurface" ] );
            }
            BOOST_CHECK_SMALL( std::fabs( directCd - forceCoefficients( 0 ) ) , 1.0E-16 );
            BOOST_CHECK_SMALL( std::fabs( directCm - momentCoefficients( 1 ) ) , 1.0E-16 );
            BOOST_CHECK_SMALL( std::fabs( directCl - forceCoefficients( 2 ) ) , 1.0E-16 );
            BOOST_CHECK_EQUAL( std::fabs( forceCoefficients( 1 ) ), 0.0 );

            if( i == 1 )
            {
                controlSurfaceCoefficientInput[ "TestSurface" ][ 2 ] = 0.0;
                apolloCoefficientInterface->updateFullCurrentCoefficients(
                            aerodynamicCoefficientInput, controlSurfaceCoefficientInput );
                Eigen::Vector3d computedPerturbedMomentCoefficients =
                        apolloCoefficientInterface->getCurrentMomentCoefficients( );
                BOOST_CHECK_EQUAL( ( std::fabs( computedPerturbedMomentCoefficients( 1 ) ) > 1.0E-8 ), true );
                controlSurfaceCoefficientInput[ "TestSurface" ][ 2 ] = controlSurfaceDeflection;

            }

            // Check zero angles.
            BOOST_CHECK_EQUAL( sideslipAngle, 0.0 );
            BOOST_CHECK_EQUAL( bankAngle, 0.0 );

            // Check if trim is not trivial
            std::vector< double > perturbedAerodynamicCoefficientInput;
            perturbedAerodynamicCoefficientInput.push_back( machNumber + 1.0 );
            perturbedAerodynamicCoefficientInput.push_back( angleOfAttack - 0.1 );
            apolloCoefficientInterface->updateFullCurrentCoefficients(
                        perturbedAerodynamicCoefficientInput, controlSurfaceCoefficientInput );
            Eigen::Vector3d computedPerturbedMomentCoefficients = apolloCoefficientInterface->getCurrentMomentCoefficients( );
            BOOST_CHECK_EQUAL( ( std::fabs( computedPerturbedMomentCoefficients( 1 ) ) > 1.0E-8 ), true );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>


#pragma once

#include "TaskPlanner.hpp"
#include <boost/thread.hpp>

namespace onlinecentralizedtaskplanner
{
struct st_location
{
  unsigned int vertex;
  unsigned int time;
  unsigned int waypoint;

  st_location(unsigned int vertex = 0, unsigned int time = 0, unsigned int waypoint = 0)
    : vertex(vertex), time(time), waypoint(waypoint)
  {
  }
  st_location(const st_location &ref) : vertex(ref.vertex), time(ref.time), waypoint(ref.waypoint)
  {
  }
  bool operator<(const st_location &loc) const
  {
    if (time < loc.time)
    {
      return true;
    }
    else if (time == loc.time)
    {
      if (waypoint > loc.waypoint)
      {
        return true;
      }
    }
    return false;
  }
};

bool astar_cmp_function(const std::vector<std::vector<unsigned int>> &min_hops_matrix,
                        const std::vector<unsigned int> &waypoints, const st_location &lhs, const st_location &rhs);

const std::string PS_path = ros::package::getPath("logistic_sim");

class OnlineCentralizedTaskPlanner : public taskplanner::TaskPlanner
{
public:
  OnlineCentralizedTaskPlanner(ros::NodeHandle &nh_, const std::string &name = "OnlineCentralizedTaskPlanner");
  ~OnlineCentralizedTaskPlanner()
  {
    for (unsigned int i = 0; i < dimension; i++)
    {
      for (unsigned int j = 0; j < MAX_TIME; j++)
      {
        for (unsigned int k = 0; k < MAX_TIME; k++)
        {
          delete[] prev_paths[i][k][j];
        }
        delete[] prev_paths[i][j];
        delete[] path_sizes[i][j];
        delete[] visited[i][j];
      }

      delete[] prev_paths[i];
      delete[] path_sizes[i];
      delete[] visited[i];
    }
    delete[] prev_paths;
    delete[] path_sizes;
    delete[] visited;
    delete[] queue;
  }

  void token_callback(const logistic_sim::TokenConstPtr &msg) override;
  virtual std::vector<logistic_sim::Path> path_partition(logistic_sim::Token &token, std::vector<logistic_sim::Mission> &missions) = 0;

  void build_map_graph() override;

  bool check_paths_conflicts(const std::vector<logistic_sim::Path> &paths, bool print = true);

  // template versions of the dijkstra functions
  // T is any callable type that returns a bool and takes two st_location objects
  template <class T = bool(const st_location &, const st_location &)>
  std::vector<unsigned int> spacetime_dijkstra(const std::vector<logistic_sim::Path> &other_paths,
                                               const std::vector<std::vector<unsigned int>> &graph,
                                               const std::vector<unsigned int> &waypoints, int ID_ROBOT,
                                               int start_time = 0, std::vector<unsigned int> *last_leg = nullptr,
                                               std::vector<unsigned int> *first_leg = nullptr,
                                               T *cmp_function = nullptr);

  template <class T = bool(const st_location &, const st_location &)>
  unsigned int insertion_sort(st_location *queue, unsigned int size, st_location loc, T *cmp_function = nullptr);

protected:
  // adjacency list of the graph, contains only neighbour of vertices, without edge lengths
  std::vector<std::vector<unsigned int>> map_graph, min_hops_matrix;
  std::vector<std::vector<unsigned int>> calculate_min_hops_matrix();
  std::vector<logistic_sim::Path> trails, home_trails;

  // astar fields
  unsigned int ****prev_paths;
  unsigned int ***path_sizes;
  unsigned int ***visited;
  st_location *queue;
  const unsigned int WHITE = 0, GRAY = 1, BLACK = 2, MAX_TIME = 150U, MAX_WAYPOINTS = 64U;
  void allocate_memory()
  {
    std::cout << "allocating memory..." << std::endl;
    std::cout << dimension << " " << MAX_TIME << std::endl;
    prev_paths = new unsigned int ***[dimension];
    path_sizes = new unsigned int **[dimension];
    for (unsigned int i = 0; i < dimension; i++)
    {
      prev_paths[i] = new unsigned int **[MAX_TIME];
      path_sizes[i] = new unsigned int *[MAX_TIME];
      for (unsigned int j = 0; j < MAX_TIME; j++)
      {
        prev_paths[i][j] = new unsigned int *[MAX_WAYPOINTS];
        path_sizes[i][j] = new unsigned int[MAX_WAYPOINTS];
        for (unsigned int k = 0; k < MAX_WAYPOINTS; k++)
        {
          prev_paths[i][j][k] = new unsigned int[MAX_TIME];
        }
      }
    }

    visited = new unsigned int **[dimension];
    for (unsigned int i = 0; i < dimension; i++)
    {
      visited[i] = new unsigned int *[MAX_TIME];
      for (unsigned int j = 0; j < MAX_TIME; j++)
      {
        visited[i][j] = new unsigned int[MAX_WAYPOINTS];
      }
    }

    queue = new st_location[MAX_TIME * MAX_TIME];
  }

  ros::Time last_goal_time;
  ros::Duration shutdown_timeout = ros::Duration(5 * 60.0), shutdown_warning = ros::Duration(4 * 60.0);
  bool warning_occured = false;
  logistic_sim::Token::_GOAL_STATUS_type last_goal_status;
};

}  // namespace onlinecentralizedtaskplanner

#include "impl/OnlineCentralizedTaskPlanner.i.hpp"


#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/option.hpp>
#include <boost/program_options/parsers.hpp>
#define BOOST_FILESYSTEM_VERSION 3
#include <boost/filesystem.hpp>

#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/lexical_cast.hpp>

#include <rw/kinematics/Kinematics.hpp>
#include <rw/kinematics/MovableFrame.hpp>
#include <rw/loaders/WorkCellFactory.hpp>
#include <rw/loaders/model3d/STLFile.hpp>
#include <rw/loaders/path/PathLoader.hpp>
#include <rw/models/Device.hpp>
#include <rw/models/Object.hpp>
#include <rw/geometry/IndexedTriMesh.hpp>
#include <rw/geometry/IntersectUtil.hpp>
#include <rw/geometry/TriangleUtil.hpp>
#include <rw/geometry/TriMesh.hpp>
#include <rw/proximity/ProximityData.hpp>
#include <rw/trajectory/Path.hpp>
#include <rwlibs/proximitystrategies/ProximityStrategyPQP.hpp>
#include <rw/proximity/ProximitySetupRule.hpp>

#include <sstream>
#include <fstream>
#include <string>

#include "CutActionParam.hpp"

using namespace boost::program_options;
using namespace std;
using namespace rw::core;
using namespace rw::geometry;
using namespace rw::kinematics;
using namespace rw::loaders;
using namespace rw::math;
using namespace rw::models;
using namespace rw::proximity;
using namespace rw::trajectory;
using rwlibs::proximitystrategies::ProximityStrategyPQP;

bool isDebugEnabled = false;

struct Knife {
	// the direction in which the knife is sharp, this is an interval
	// we can only cut when moving in this direction
	rw::math::Vector3D<> from,to;

	ProximityModel::Ptr blade, handle;
	Transform3D<> bladeOffset, handleOffset;

	// blade edge is key points on the sharp edge of the blade,
	// all points should be in the cutting plane
	// described relative to blade frame
	std::vector<Vector3D<> > bladeedge;
	// todo how do we handle double bladed stuff
};

struct CutResult {
	CutResult():success(false),failType(NONE){}
	bool success;

	typedef enum {NONE, NoContact, BadDirection, InitialCollision, HandleCollision, GripperCollision,SizeOfStatusArray} Failure;
	Failure failType;
	// the pose of the knife at each timestep
	std::vector<Transform3D<> > path;
	// the force at each timestep
	std::vector< std::pair<Vector3D<>,Vector3D<> > > forcepath;
	// the two resulting meshes (if successfull)
	std::vector<TriMesh::Ptr> meshes;
	//PlainTriMeshF::Ptr mesh1, mesh2;
};

Ptr<CutResult> simulateCut(Knife& knife, ProximityModel::Ptr obj, Object::Ptr objPtr, ProximityStrategyPQP& colstrat, CutActionParam& param);
std::vector<TriMesh::Ptr> knifeMesh(TriMesh::Ptr knife, const Transform3D<>& wTknife,
								 TriMesh::Ptr object, const Transform3D<>& wTobject);

Vector3D<> parseVector3D(const std::string& str){

	stringstream sstr(str);
	Vector3D<> val;
	sstr >> val[0] >> val[1] >> val[2];
	//std::cout << val << std::endl;
	return val;
}

std::vector<Vector3D<> > parseBladeEdge(const std::string& str){
	//std::cout << "parseBladeEdge" << std::endl;
	std::vector<Vector3D<> > result;
	stringstream sstr(str);
	while(!sstr.eof()){
		Vector3D<> val;
		sstr >> val[0] >> val[1] >> val[2];
		result.push_back(val);
		//std::cout << val << std::endl;
	}
	return result;
}

int main(int argc, char** argv){
    // we need
    options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("knife,k", value<string>(), "the knife name.")
        ("object,o", value<string>(), "the object name.")
        ("output-dir", value<string>()->default_value("out"), "The output directory.")
        ("output-geom", value<bool>()->default_value(false), "The geometries of cutted object will be saved.")
        ("output-statepath", value<bool>()->default_value(false), "State path for visually checking cutting trajectory.")
        ("debug", value<bool>()->default_value(false), "Debug stuff.")
        ("wc,w", value<string>(), "the workcell file.")
        ("input", value<string>(), "input of action parameters.")

        ("gripper", value<string>(), "the gripper.")
        ("gripper-tcp", value<string>()->default_value("GRIPPER_TCP"), "the gripper tcp.")
        ("gripper-base", value<string>()->default_value("GRIPPER_MOVER"), "the frame that moves the gripper.")

        //("batchsize", value<int>()->default_value(5000), "The max nr grasp of output, files will be split into multiple files.")
        //("useGraspTarget",value<bool>()->default_value(false),"Enable to copy ObjTTcpTarget to target.pose")
    ;
    positional_options_description optionDesc;
    optionDesc.add("input",-1);
    variables_map vm;
    store(command_line_parser(argc, argv).
              options(desc).positional(optionDesc).run(), vm);
    notify(vm);

    if (vm.count("help")) {
        cout << "Usage:\n\n"
                  << "\t" << argv[0] <<" [options] -w<workcellFile> -o<objectName> -k<knifeName> <actionInput> \n"
                  << "\n";
        cout << desc << "\n";
        return 1;
    }

    std::string wcFile = vm["wc"].as<string>();
    std::string knifeName = vm["knife"].as<string>();
    std::string objName = vm["object"].as<string>();
    std::string input = vm["input"].as<string>();

    string out_dir = vm["output-dir"].as<string>();
    if(!boost::filesystem::exists(out_dir)){
    	boost::filesystem::create_directory(out_dir);

    }
    bool output_statepath = vm["output-statepath"].as<bool>();
    bool output_geom = vm["output-geom"].as<bool>();
    bool debug = vm["debug"].as<bool>();
    isDebugEnabled = debug;
    std::vector<CutActionParam> params = CutActionParam::load(input);

    WorkCell::Ptr wc = WorkCellFactory::load(wcFile);

    MovableFrame* knife_frame = wc->findFrame<MovableFrame>(knifeName);
    std::string bladeName = knife_frame->getPropertyMap().get<std::string>("blade");
    std::string handleName = knife_frame->getPropertyMap().get<std::string>("handle");
    std::string bladeedge_desc = knife_frame->getPropertyMap().get<std::string>("bladeedge");
    std::string from_desc = knife_frame->getPropertyMap().get<std::string>("dir_from");
    std::string to_desc = knife_frame->getPropertyMap().get<std::string>("dir_to");

    Object::Ptr blade = wc->findObject(bladeName);
    Object::Ptr handle = wc->findObject(handleName);
    Object::Ptr obj = wc->findObject(objName);

    State state = wc->getDefaultState();

    // parse gripper stuff
    std::string gripper_name = vm["gripper"].as<string>();
    std::string gripper_tcp_name = vm["gripper-tcp"].as<string>();
    std::string gripper_base_name = vm["gripper-base"].as<string>();

    Device::Ptr gripper = wc->findDevice(gripper_name);
    MovableFrame* gripper_base = wc->findFrame<MovableFrame>(gripper_base_name);
    Frame* gripper_tcp = wc->findFrame(gripper_tcp_name);

    if(gripper==NULL)
        RW_THROW("Gripper \""<<gripper_name<<"\" could not be found in workcell!");
    if(gripper_base==NULL)
        RW_THROW("Gripper base \""<<gripper_base_name<<"\" could not be found in workcell!");
    if(gripper_tcp==NULL)
        RW_THROW("Gripper \""<<gripper_tcp_name<<"\" could not be found in workcell!");

    // describe everything relative to knife_frame
    Knife cutter;
    cutter.bladeedge = parseBladeEdge( bladeedge_desc );
    cutter.from = parseVector3D( from_desc );
    cutter.to = parseVector3D( to_desc );
    cutter.bladeOffset = Kinematics::frameTframe(knife_frame, blade->getBase(), state);
    cutter.handleOffset = Kinematics::frameTframe(knife_frame, handle->getBase(), state);

    ofstream resultFile;
    resultFile.open(std::string(out_dir+"/result.txt").c_str());
    ofstream resultCompactFile;
    resultCompactFile.open(std::string(out_dir+"/resultCompact.txt").c_str());
    // make proximity model
	TimedStatePath spath_global;
	int time_global = 0;

	// prepare the collision strategy
    ProximityStrategyPQP::Ptr strategy = ownedPtr(new ProximityStrategyPQP());
    strategy->addModel(blade);
    strategy->addModel(handle);
    strategy->addModel(obj);
    ProximityModel::Ptr objectModel = strategy->getModel(obj->getBase());

    // gripper collision detector - create a collision detector that only
    // test collision between gripper and object
    CollisionDetector detector(wc, ownedPtr(new ProximityStrategyPQP()) );
    ProximitySetupRule ruleExcludeAll("*", "*", ProximitySetupRule::EXCLUDE_RULE);
    ProximitySetupRule ruleIncludeObject(gripper_name+".*", objName, ProximitySetupRule::INCLUDE_RULE);

    detector.addRule( ruleExcludeAll );
    detector.addRule( ruleIncludeObject );
    rw::proximity::ProximityData pdata;
    pdata.setCollisionQueryType( CollisionDetector::FirstContactNoInfo );
    int successes=0;
    // int totalSims=params.size(); // Unused
    std::vector<int> failTypes( CutResult::SizeOfStatusArray , 0);
	for(size_t pi=0;pi<params.size(); pi++){

        cutter.blade = strategy->getModel(blade->getBase());
        cutter.handle = strategy->getModel(handle->getBase());

		// we simulate
		if(isDebugEnabled)
		    std::cout << "Simulate cute: " << std::endl;
		Ptr<CutResult> result = simulateCut(cutter, objectModel, obj, *strategy, params[pi]);

		// before saving result, test if gripper collides with object during motion
		if(result->success){
		    bool gripperCollideWithObj = false;
	        for(size_t i=0;i<result->path.size();i++){
	            // set configuration of object and knife and save state.
	            Transform3D<> objPose( params[0].posObj, params[0].rpyObj );
	            Transform3D<> knifet3d = result->path[i];
	            ((MovableFrame*)obj->getBase())->setTransform(objPose, state);
	            knife_frame->setTransform(knifet3d, state);

	            // calculate position of gripper base
	            Transform3D<> knifeTgripper(params[pi].posGripper,params[pi].rpyGripper);
                Transform3D<> knifeTgripperbase = knifeTgripper * Kinematics::frameTframe(gripper_tcp,gripper_base, state);

	            gripper->setQ( params[pi].gripperQ, state);
	            gripper_base->moveTo(knifeTgripperbase,knife_frame,state);
	            // test for collision
	            gripperCollideWithObj |= detector.inCollision(state, pdata);
	        }
	        if(gripperCollideWithObj){
	            //std::cout << "GRIPPER COLLIDES WITH OBJECT!" << std::endl;
	            result->success = false;
	            result->failType = CutResult::GripperCollision;
	        } else {
	            //std::cout << "GRIPPER DOES NOT COLLIDE WITH OBJECT!" << std::endl;
	        }
		}
		if(result->success){
		    successes++;
		} else {
		    failTypes[result->failType]++;
		}

		std::cout << (pi+1) << "\t"
                        << successes << "\t"
                        << (successes*100.0)/(pi+1) <<"%" << "\t";
		for(std::size_t errType=0;errType<failTypes.size();errType++)
		    std::cout << failTypes[errType] << "\t";
        std::cout << "        \r";

		resultCompactFile << pi << " "
		                  << result->success << " "
		                  << result->failType << " "
		                  << result->meshes.size() << " "
		                  << "\n";

		resultFile << "\nexp " << pi
				   << "\n success " << result->success
				   << "\n failtype " << result->failType
				   << "\n pathlen "  << result->path.size()
				   << "\n forcepathlen "  << result->forcepath.size()
				   << "\n geoms "  << result->meshes.size()
				   ;

		resultFile << "\n ftdata \n";
		for(std::size_t i=0;i<result->forcepath.size();i++){
			Vector3D<> f = result->forcepath[i].first;
			Vector3D<> t = result->forcepath[i].second;
			resultFile << f[0] << " " << f[1] << " " << f[2] << " "
			           << t[0] << " " << t[1] << " " << t[2] << "\n";
		}

		if(output_statepath)
			resultFile << "\n statepath "  << boost::lexical_cast<std::string>(pi) << "_statepath.rwplay";

		if(output_geom){
			TriMesh::Ptr mesh = obj->getGeometry()[0]->getGeometryData()->getTriMesh() ;
			STLFile::save( *mesh,
					out_dir+"/"+ boost::lexical_cast<std::string>(pi) + "_geo" +".stl");

			for(std::size_t i=0;i<result->meshes.size();i++){
				resultFile << "\n geo "  << pi<< "_geo" << i <<".stl";
				STLFile::save( *result->meshes[i] ,
						out_dir+"/"+ boost::lexical_cast<std::string>(pi) + "_geo" + boost::lexical_cast<std::string>(i) +".stl");
			}
		}

		// create state path
		if(output_statepath){
            TimedStatePath spath;
            for(size_t i=0;i<result->path.size();i++){
                // set configuration of object and knife and save state.
                Transform3D<> objPose( params[0].posObj, params[0].rpyObj );
                Transform3D<> knifet3d = result->path[i];
                knife_frame->setTransform(knifet3d, state);
                ((MovableFrame*)obj->getBase())->setTransform(objPose, state);

                // calculate position of gripper base
                Transform3D<> knifeTgripper(params[pi].posGripper,params[pi].rpyGripper);
                gripper->setQ( params[pi].gripperQ, state);
                Transform3D<> knifeTgripperbase = knifeTgripper * Kinematics::frameTframe(gripper_tcp,gripper_base, state);

                gripper_base->moveTo(knifeTgripperbase,knife_frame,state);

                spath.push_back( TimedState(i*0.01, state) );

                spath_global.push_back( TimedState(time_global*0.01, state) );
                time_global++;
            }
			PathLoader::storeTimedStatePath(*wc, spath, out_dir+"/"+ boost::lexical_cast<std::string>(pi) + "_statepath.rwplay");
		}
		/*
		if(result->success){
			RW_WARN("");
			// change objectModel
		    //strategy = new ProximityStrategyPQP();
		    //strategy->addModel(blade);
		    //strategy->addModel(handle);

		    //obj = ownedPtr( new Object(obj->getBase(), ownedPtr( new Geometry( result->meshes[1] ) )));

			Geometry::Ptr geom = obj->getGeometry()[0];

			geom->setGeometryData( result->meshes[1] );
			obj = ownedPtr( new Object(obj->getBase(), geom ));

			//objectModel->removeGeometry( objectModel->getGeometryIDs()[0] );
			//obj->removeGeometry( obj->getGeometry()[0] );
			std::cout << "nr meshes: " << result->meshes.size() << std::endl;
			//Geometry::Ptr geom = ownedPtr( new Geometry( result->meshes[1] ) );
			//geom->setFrame(obj->getBase());
			//obj->addGeometry( geom );
			RW_WARN("");
		    //strategy->addModel(obj);
		    //objectModel = strategy->getModel(obj->getBase());

			//objectModel->addGeometry( *(obj->getGeometry()[0]) );

		}
		*/

    }

	if(output_statepath)
		PathLoader::storeTimedStatePath(*wc, spath_global, out_dir+"/global_statepath.rwplay");

    // save summary

    return 1;
}

rw::core::Ptr<CutResult> simulateCut(Knife& knife, ProximityModel::Ptr obj, Object::Ptr objPtr, ProximityStrategyPQP& colstrat, CutActionParam& param){
	ProximityStrategyData data;

	Ptr<CutResult> res = ownedPtr( new CutResult() );

	Transform3D<> knife_pose_init(param.pos, param.rpy );
	// calculate the new move direction
	Vector3D<> knifeNormal = normalize( cross( knife_pose_init.R()*knife.from, knife_pose_init.R()*knife.to) );
	// project knife dir onto plane
	Vector3D<> ndir = normalize(param.dir - (dot(param.dir, knifeNormal) * knifeNormal) );
	// make ndir as long as param.dir
	ndir = ndir*param.dir.norm2();

	if(isDebugEnabled){
        std::cout << "KnifeDIR: " << ndir << std::endl;
        std::cout << "paramDIR: " << param.dir << std::endl;
	}

	// if ndir is not within [from;to] then the direction is bad
	double ang1 = angle(knife_pose_init.R()*knife.from, ndir, knifeNormal);
	double ang2 = angle(knife_pose_init.R()*knife.from, knife_pose_init.R()*knife.to, knifeNormal);
	if(ang2<0 && (ang1>0 || ang1<ang2) ){
		res->failType = CutResult::BadDirection;
		return res;
	}
	if(ang2>0 && (ang1<0 || ang1>ang2) ){
		res->failType = CutResult::BadDirection;
		return res;
	}

	int timesteps = 100;
	double len_dt = param.len/timesteps;
	Transform3D<> objPose( param.posObj, param.rpyObj );
	// the simulation runs in two steps:
	// first the position of the knife is translated in the cutting direction until
	// collision with object is detected
	int i = 0;
	data.setCollisionQueryType( CollisionStrategy::FirstContact);
	for(i=0;i<timesteps;i++){
		// calculate new position of knife
		Transform3D<> t3d_knife(param.pos + param.dir*(len_dt*i), param.rpy );
		res->path.push_back(t3d_knife);
		res->forcepath.push_back(std::make_pair(Vector3D<>(0,0,0),Vector3D<>(0,0,0)));

		// check if knife is in collision
		// if handle is in collision the we failed
		if( colstrat.inCollision(knife.handle, t3d_knife * knife.handleOffset, obj, objPose, data) ){
			res->success = false;
			res->failType = CutResult::HandleCollision;
			return res;
		}

		if( colstrat.inCollision(knife.blade, t3d_knife * knife.bladeOffset, obj, objPose, data) ){
			// TODO: make sure its the sharp end of the blade that hit the object
			res->success = true;
			break;
		}
	}

	// check if we hit the object
	if( i>=timesteps ){
		res->failType = CutResult::NoContact;
		return res;
	}

	const std::size_t cuttingStartIndex = res->path.size()-2;
	std::size_t cuttingEndIndex = res->path.size();

	data.setCollisionQueryType( CollisionStrategy::AllContacts );
	// the second step is to continue moving the knife in the direction of cutting
	// until it is through the object
	Vector3D<> cpos = res->path.back().P();
	bool lastCollide = true;
	int j = 1;
	for(;i<timesteps;i++){
		// calculate new position of knife
		Transform3D<> t3d_knife(cpos + ndir*(len_dt*j), param.rpy );
		j++;
		//Transform3D<> t3d_knife(param.pos + param.dir*(len_dt*i), param.rpy );

		res->path.push_back(t3d_knife);
		// translate contact force -ndir into object coordinates
		res->forcepath.push_back(std::make_pair(-ndir,Vector3D<>(0,0,0)));

		// check if knife is in collision
		// if handle is in collision the we failed
		if( colstrat.inCollision(knife.handle, t3d_knife * knife.handleOffset, obj, objPose, data) ){
			res->success = false;
			res->failType = CutResult::HandleCollision;
			return res;
		}

		if( colstrat.inCollision(knife.blade, t3d_knife * knife.bladeOffset, obj, objPose, data) ){
			// TODO: calculate force path from contact information
			// the force is scaled by the area which is in contact
			//data.getCollisionData()._collisionPairs
			lastCollide = true;
		} else {
			// knife released hold of object so we stop...
			if(lastCollide)
				cuttingEndIndex = i+1;
			lastCollide = false;
		}
	}

	// now create the cutting mesh
	PlainTriMeshF cuttingMesh;
	if(isDebugEnabled)
	    std::cout << "Cutting start: " << cuttingStartIndex << std::endl;
	for(std::size_t j = cuttingStartIndex+1; j < cuttingEndIndex; j++) {
		Transform3D<> knifeTrans_a = res->path[j-1];
		Transform3D<> knifeTrans_b = res->path[j] ;
		for(std::size_t x=1;x<knife.bladeedge.size();x++){
			Vector3D<float> a1 = cast<float>( knifeTrans_a * knife.bladeedge[x-1] );
			Vector3D<float> a2 = cast<float>( knifeTrans_a * knife.bladeedge[x] );

			Vector3D<float> b1 = cast<float>( knifeTrans_b * knife.bladeedge[x-1] );
			Vector3D<float> b2 = cast<float>( knifeTrans_b * knife.bladeedge[x] );

			cuttingMesh.add( Triangle<float>(a1,b1,b2) );
			cuttingMesh.add( Triangle<float>(a1,b2,a2) );
		}
	}
    if(isDebugEnabled){
    	STLFile::save(cuttingMesh, "tests_mesh.stl");
    }
	// perform actual cutting

	TriMesh::Ptr objTriMesh = objPtr->getGeometry()[0]->getGeometryData()->getTriMesh();

	std::vector<TriMesh::Ptr> cutobjs =
			knifeMesh(Ptr<TriMesh>(&cuttingMesh), Transform3D<>::identity(), objTriMesh, objPose);

	res->meshes = cutobjs;
	return res;
}

std::vector<TriMesh::Ptr> knifeMesh(TriMesh::Ptr knife, const Transform3D<>& wTknife,
		TriMesh::Ptr object, const Transform3D<>& wTobject){
    if(isDebugEnabled){
        std::cout << "Knifing mesh" << std::endl;

        std::cout << "to indexed mesh" << std::endl;
    }
	// convert object to indexed trimesh
	IndexedTriMeshN0<float>::Ptr imesh_object =
			TriangleUtil::toIndexedTriMesh<IndexedTriMeshN0<float> >( *object );

	// now create a mesh representation of edges
    ProximityStrategyPQP::Ptr strategy = ownedPtr( new ProximityStrategyPQP() );
    Geometry knifeGeom(knife);
    Geometry objectGeom(imesh_object);
    ProximityStrategyData data;
    data.setCollisionQueryType( CollisionStrategy::AllContacts);
    ProximityModel::Ptr knifeModel = strategy->createModel();
    knifeModel->addGeometry(knifeGeom);
    ProximityModel::Ptr objectModel = strategy->createModel();
    objectModel->addGeometry(objectGeom);
    if( ! strategy->inCollision(knifeModel, wTknife, objectModel, wTobject, data) )
    	return std::vector<TriMesh::Ptr>();
    // now comes the fun part
    // find all edges/triangles that are to be changed
    PlainTriMeshD::Ptr knifeMeshResult_left = ownedPtr( new PlainTriMeshD() );
    PlainTriMeshD::Ptr knifeMeshResult_right = ownedPtr( new PlainTriMeshD() );
    PlainTriMeshD knifeMeshCols;

    /*
    std::map<int, int> knifeMap, objectMap;
    typedef std::pair<int, int> IDs;
    for( IDs prims : data.getCollisionData()._geomPrimIds ) {
    	int knife_tri = prims.first;
    	int object_tri = prims.second;
    	knifeMap[knife_tri]++;
    	objectMap[object_tri]++;
    }

    std::cout << "KNIFE MAP" << std::endl;
    for( IDs data : knifeMap ) {
    	std::cout << data.first << " = " << data.second << "\n";
    }

    std::cout << "OBJECT MAP" << std::endl;
    for( IDs data : objectMap ) {
    	std::cout << data.first << " = " << data.second << "\n";
    }
*/

    std::vector<int> collidingTrisObject;
    std::stack<int> vertsLeft, vertsRight;

    typedef std::pair<int, int> IDs;
    for( IDs prims : data.getCollisionData()._geomPrimIds ) {
    	int knife_tri = prims.first;
    	int object_tri = prims.second;
    	collidingTrisObject.push_back(object_tri);
    	//
    	// now create a cutted triangle on the knife mesh which is on the
    	// negative side of the object surface (inside the object)
    	Triangle<> ktri, otri;
    	knife->getTriangle(knife_tri, ktri);
    	imesh_object->getTriangle(object_tri, otri);

    	IndexedTriangle<uint32_t> otri_idx = imesh_object->getIndexedTriangle( object_tri );

    	ktri = ktri.transform(wTknife);
    	otri = otri.transform(wTobject);
    	knifeMeshCols.add(otri);
    	// determine contact points
    	Vector3D<> p1,p2;
    	if( !IntersectUtil::intersetPtTriTri(ktri,otri,p1,p2) ){
    		//RW_WARN("COULD NOT FIND INTERSECTION");
    		continue;
    	}
    	if( std::isnan(p1[0]) || std::isnan(p2[0])){

    		std::cout << p1 <<"\n"<<  p2 << "\n\n";
    		std::cout << ktri << std::endl;
    		std::cout << otri << std::endl;
    	}
    	// create new triangles
    	// 1. determine on which side of the line (p1,p2) the vertices of otri.
    	Vector3D<> normal = otri.calcFaceNormal();
    	Vector3D<> knormal = ktri.calcFaceNormal();
    	bool side0 = dot(cross((p2-p1),(otri[0]-p1)), normal ) <0;
    	bool side1 = dot(cross((p2-p1),(otri[1]-p1)), normal ) <0;
    	bool side2 = dot(cross((p2-p1),(otri[2]-p1)), normal ) <0;

    	if(dot(otri[0]-ktri[0], knormal ) <0) vertsLeft.push( otri_idx[0] );
    	else vertsRight.push( otri_idx[0] );
    	if(dot(otri[1]-ktri[0], knormal ) <0 ) vertsLeft.push( otri_idx[1] );
    	else vertsRight.push( otri_idx[1] );
    	if(dot(otri[2]-ktri[0], knormal ) <0 ) vertsLeft.push( otri_idx[2] );
    	else vertsRight.push( otri_idx[2] );

    	// we rotate triangle such that s1 and s2 are allways on the same side
    	if(side0==side2){
    		//side = side0;
    		std::swap( otri[0],otri[1] );
    		std::swap( otri[2],otri[0] );
    	} else if(side1==side2 ){
    		//side = side1;
    		std::swap( otri[0],otri[1] );
    		std::swap( otri[1],otri[2] );
    	}
    	// check which side of knife the tri0 is on

    	// project  tri0 onto p1,p2
    	Vector3D<> p = IntersectUtil::closestPtPointRay(otri[0],p1,p2);
    	bool knife_side = !(dot( otri[0]-p , knormal )>=0);

    	/*
    	Vector3D<> p_tri1 = IntersectUtil::closestPtPointLine(otri[1],p1,p2);
    	if(MetricUtil::dist2(otri[0], p )<0.0001)
    		continue;
    	if(MetricUtil::dist2(otri[0], p )<MetricUtil::dist2(otri[1], p_tri1 )){
        	knife_side = !(dot( otri[1]-p_tri1 , knormal )>=0);

    	}
    	*/
    	///std::cout << MetricUtil::dist2(otri[0], p ) << std::endl;

    	if(side0==side1 && side0==side2){
        	//std::cout << side0 << " " << side1 << " " << side2 << std::endl;

    		//std::cout << p1 <<"\n"<<  p2 << "\n\n";
    		//std::cout << ktri << std::endl;
    		//std::cout << otri << std::endl;

			if(!knife_side)
			{
				//knifeMeshResult.add(otri);
			}

    		continue;
    	}

    	Transform3D<> objTw = inverse( wTobject );
		if(dot( cross(p2-otri[0],p1-otri[0]) , normal )>0){
			// then p2 comes before p1
			if(knife_side){
				knifeMeshResult_left->add(Triangle<>(otri[0],otri[1],p2).transform(objTw) );
				knifeMeshResult_left->add(Triangle<>(otri[0],p2,p1).transform(objTw) );

				knifeMeshResult_right->add(Triangle<>(otri[2],p1,p2).transform(objTw) );
			} else {
				knifeMeshResult_right->add(Triangle<>(otri[0],otri[1],p2).transform(objTw));
				knifeMeshResult_right->add(Triangle<>(otri[0],p2,p1).transform(objTw) );

				knifeMeshResult_left->add(Triangle<>(otri[2],p1,p2).transform(objTw) );
			}

			//} else {
			//	knifeMeshResult_right.add(Triangle<>(otri[2],p1,p2) );
			//}
		} else {
			// then p1 comes before p2
			if(knife_side){
				knifeMeshResult_left->add(Triangle<>(otri[0],otri[1],p1).transform(objTw));
				knifeMeshResult_left->add(Triangle<>(otri[0],p1,p2).transform(objTw) );
			//} else {
				knifeMeshResult_right->add(Triangle<>(otri[2],p2,p1).transform(objTw) );
			} else {
				knifeMeshResult_right->add(Triangle<>(otri[0],otri[1],p1).transform(objTw));
				knifeMeshResult_right->add(Triangle<>(otri[0],p1,p2).transform(objTw) );
			//} else {
				knifeMeshResult_left->add(Triangle<>(otri[2],p2,p1).transform(objTw) );
			}
		}

    }

    if(isDebugEnabled){
		STLFile::save(*knifeMeshResult_left,"knifemesh_result_left.stl");
		STLFile::save(*knifeMeshResult_right,"knifemesh_result_right.stl");
    }

    // now come the last part, determine the new meshes by computing connectivity between
    // the cutted mesh and the remaining mesh.

    // create vertice-nighbour map
    std::vector<int> triColor(imesh_object->size(), 0);
    std::vector<int> vertColor(imesh_object->size(), 0);
    std::vector<std::vector<int> > verticeToTris(imesh_object->getVertices().size());

    for(std::size_t i=0; i<imesh_object->size(); i++ ){
    	IndexedTriangle<uint32_t> itri = imesh_object->getIndexedTriangle(i);
		const int intI = boost::numeric_cast<int>(i);
    	verticeToTris[itri.getVertexIdx(0)].push_back(intI);
    	verticeToTris[itri.getVertexIdx(1)].push_back(intI);
    	verticeToTris[itri.getVertexIdx(2)].push_back(intI);
    }
    enum{NONE=0, COLLIDING_TRI=1, LEFT_TRI, RIGHT_TRI};
    // now color all triangles on either side of the knife
    for(int colTri : collidingTrisObject) {
    	triColor[colTri] = COLLIDING_TRI;
    }
    // next do floodfill and color all left and right triangles
    // std::stack<int> verticeFrontLeft, verticeFrontRight;
    while(!vertsLeft.empty()){
    	int vert = vertsLeft.top();
    	vertsLeft.pop();
    	// get triangles that are not colored
    	for(int tri : verticeToTris[vert]) {
    		if(triColor[tri]!=NONE)
    			continue;
    		triColor[tri] = LEFT_TRI;
    		// add vertices to vertsLeft
    		IndexedTriangle<uint32_t> itri = imesh_object->getIndexedTriangle(tri);
    		vertsLeft.push( itri[0] );
    		vertsLeft.push( itri[1] );
    		vertsLeft.push( itri[2] );
    	}
    	//
    }

    while(!vertsRight.empty()){
    	int vert = vertsRight.top();
    	vertsRight.pop();
    	// get triangles that are not colored
    	for(int tri : verticeToTris[vert]) {
    		if(triColor[tri]!=NONE)
    			continue;
    		triColor[tri] = RIGHT_TRI;
    		// add vertices to vertsLeft
    		IndexedTriangle<uint32_t> itri = imesh_object->getIndexedTriangle(tri);
    		vertsRight.push( itri[0] );
    		vertsRight.push( itri[1] );
    		vertsRight.push( itri[2] );
    	}
    	//
    }

    // now add all triangles that are colored left to left trimesh
    for(std::size_t tri=0; tri<triColor.size(); tri++){

    	if(triColor[tri]==LEFT_TRI){
        	Triangle<> otri;
        	imesh_object->getTriangle(tri, otri);
        	//otri = otri.transform(wTobject);

    		knifeMeshResult_left->add( otri );
    	}

    	if(triColor[tri]==RIGHT_TRI){
        	Triangle<> otri;
        	imesh_object->getTriangle(tri, otri);
        	//otri = otri.transform(wTobject);

    		knifeMeshResult_right->add( otri );
    	}
    }

    if(isDebugEnabled){
		STLFile::save(*knifeMeshResult_left,"knifemesh_result_left_comp.stl");
		STLFile::save(*knifeMeshResult_right,"knifemesh_result_right_comp.stl");
		STLFile::save(knifeMeshCols,"knifemesh_cols.stl");
    }
    // the inefficient approach here is to
    // 1. color all colliding triangles

    std::vector<TriMesh::Ptr> result;
    result.push_back(knifeMeshResult_left);
    result.push_back(knifeMeshResult_right);
    return result;
}


#include "framework/builder/cfem_framework_builder.h"

#include <fstream>
#include <streambuf>
#include <deal.II/base/mpi.h>
#include <formulation/angular/self_adjoint_angular_flux.h>

#include "calculator/cell/integrated_fission_source.h"
#include "calculator/cell/total_aggregated_fission_source.h"
#include "convergence/moments/convergence_checker_l1_norm.hpp"
#include "convergence/reporter/mpi_noisy.h"
#include "domain/domain.hpp"
#include "domain/finite_element/finite_element_gaussian.h"
#include "domain/mesh/mesh_cartesian.hpp"
#include "eigenvalue/k_effective/updater_via_fission_source.h"
#include "formulation/cfem_diffusion_stamper.h"
#include "formulation/scalar/diffusion.hpp"
#include "formulation/cfem_saaf_stamper.h"
#include "framework/framework.h"
#include "iteration/updater/source_updater_gauss_seidel.h"
#include "iteration/updater/angular_source_updater_gauss_seidel.h"
#include "iteration/updater/fixed_updater.h"
#include "iteration/updater/angular_fixed_updater.h"
#include "iteration/initializer/initialize_fixed_terms_once.h"
#include "iteration/initializer/set_fixed_terms_once.h" // to be removed
#include "iteration/group/group_source_iteration.hpp"
#include "iteration/outer/outer_power_iteration.h"
#include "material/material_protobuf.h"
#include "problem/parameter_types.h"
#include "convergence/parameters/single_parameter_checker.h"
#include "convergence/final_checker_or_n.h"
#include "quadrature/utility/quadrature_utilities.h"
#include "quadrature/factory/quadrature_factories.h"
#include "results/output_dealii_vtu.h"
#include "solver/group/single_group_solver.h"
#include "solver/gmres.h"
#include "system/system.h"
#include "system/solution/mpi_group_angular_solution.h"
#include "system/terms/term.h"
#include "system/terms/term_types.h"
#include "system/moments/spherical_harmonic.h"
#include "system/system_functions.h"

namespace bart {

namespace framework {

namespace builder {

template <int dim>
std::unique_ptr<FrameworkI> CFEM_FrameworkBuilder<dim>::BuildFramework(
    problem::ParametersI &prm,
    dealii::ParameterHandler &d2_prm) {

  std::cout << "Setting up materials" << std::endl;

  std::ifstream mapping_file(prm.MaterialMapFilename());
  std::string material_mapping(
      (std::istreambuf_iterator<char>(mapping_file)),
      std::istreambuf_iterator<char>());

  const int n_groups = prm.NEnergyGroups();
  int n_angles;

  MaterialProtobuf materials(d2_prm);
  auto cross_sections_ptr = std::make_shared<bart::data::CrossSections>(materials);

  std::cout << "Building Finite Element"<< std::endl;
  std::shared_ptr<FiniteElement>finite_element_ptr(std::move(BuildFiniteElement(
      &prm)));

  std::cout << "Building Domain" << std::endl;
  std::shared_ptr<Domain> domain_ptr(std::move(BuildDomain(
      &prm,finite_element_ptr, material_mapping)));

  std::cout << "Setting up domain" << std::endl;
  domain_ptr->SetUpMesh().SetUpDOF();

  std::shared_ptr<SourceUpdater> source_updater_ptr;
  std::unique_ptr<Initializer> initializer_ptr;

  std::shared_ptr<AngularQuadratureSet> quadrature_ptr = nullptr;

  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    printf("Building Quadrature Set\n");
    quadrature_ptr = BuildAngularQuadratureSet(&prm);
    n_angles = quadrature_ptr->size();
  } else {
    printf("Scalar solve\n");
    n_angles = 1;
  }

  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    printf("Building Stamper\n");
    std::shared_ptr<CFEMAngularStamper> stamper_ptr(std::move(BuildAngularStamper(
        &prm, domain_ptr, finite_element_ptr, cross_sections_ptr, quadrature_ptr)));
    printf("Building Source Updater\n");
    source_updater_ptr = std::move(BuildSourceUpdater(&prm, stamper_ptr, quadrature_ptr));
    printf("Building Initializer\n");
    initializer_ptr = BuildInitializer(&prm, stamper_ptr, quadrature_ptr);
  } else {
    printf("Building Scalar Stamper\n");
    std::shared_ptr<CFEMStamper> stamper_ptr(std::move(BuildStamper(
        &prm, domain_ptr, finite_element_ptr, cross_sections_ptr)));
    printf("Building Source Updater\n");
    source_updater_ptr = std::move(BuildSourceUpdater(&prm, stamper_ptr));
    printf("Building Initializer\n");
    initializer_ptr = BuildInitializer(&prm, stamper_ptr);
  }

  std::cout << "Building single group solver" << std::endl;
  auto single_group_solver_ptr = BuildSingleGroupSolver();

  std::cout << "Building inner iteration objects" << std::endl;

  auto in_group_final_checker = BuildMomentConvergenceChecker(1e-10, 100);

  // Build reporter
  std::shared_ptr<ConvergenceReporter> reporter(std::move(BuildConvergenceReporter()));

  auto moment_calculator_type = quadrature::MomentCalculatorImpl::kScalarMoment;

  // Moment calculator
  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    moment_calculator_type = quadrature::MomentCalculatorImpl::kZerothMomentOnly;
  }

  auto moment_calculator_ptr = quadrature::factory::MakeMomentCalculator<dim>(
      moment_calculator_type,
      quadrature_ptr);

  std::cout << "Building and initializing system object" << std::endl;

  // Solution group
  auto solution_ptr =
      std::make_shared<system::solution::MPIGroupAngularSolution>(n_angles);

  system::SetUpMPIAngularSolution(*solution_ptr, *domain_ptr);

  std::cout << "Building in-group iteration" << std::endl;

  auto in_group_iteration =
      std::make_unique<iteration::group::GroupSourceIteration<dim>>(
          std::move(single_group_solver_ptr),
          std::move(in_group_final_checker),
          std::move(moment_calculator_ptr),
          solution_ptr,
          source_updater_ptr,
          reporter);

  std::cout << "Building K_Effective updater" << std::endl;

  // KEffectiveUpdater
  using FissionSourceCalulator = calculator::cell::TotalAggregatedFissionSource<dim>;
  using IntegratedFissionSourceCalc = calculator::cell::IntegratedFissionSource<dim>;
  auto int_fission_ptr = std::make_unique<IntegratedFissionSourceCalc>(
      finite_element_ptr, cross_sections_ptr);
  auto fission_source_calc_ptr = std::make_unique<FissionSourceCalulator>(
      std::move(int_fission_ptr), domain_ptr);
  using KEffectiveUpdater = eigenvalue::k_effective::UpdaterViaFissionSource;

  // KEffective convergence checker
  using KEffConvChecker = bart::convergence::parameters::SingleParameterChecker;
  auto k_eff_conv_checker = std::make_unique<KEffConvChecker>();
  using KEffectiveFinalCovergence = bart::convergence::FinalCheckerOrN<double,
      bart::convergence::parameters::SingleParameterChecker>;
  auto k_eff_final_checker = std::make_unique<KEffectiveFinalCovergence>(
      std::move(k_eff_conv_checker)
      );

  // KEffective updater
  auto k_effective_updater = std::make_unique<KEffectiveUpdater>(
      std::move(fission_source_calc_ptr), 2.0, 10);

  std::cout << "Building outer-iteration" << std::endl;

  using PowerIteration = iteration::outer::OuterPowerIteration;
  auto power_iteration_ptr = std::make_unique<PowerIteration>(
      std::move(in_group_iteration),
      std::move(k_eff_final_checker),
      std::move(k_effective_updater),
      source_updater_ptr,
      reporter
  );

  std::cout << "Building System" << std::endl;

  auto system = std::make_unique<system::System>();

  system->total_groups = n_groups;
  std::unordered_set<bart::system::terms::VariableLinearTerms>
      source_terms{bart::system::terms::VariableLinearTerms::kScatteringSource,
                   bart::system::terms::VariableLinearTerms::kFissionSource};
  system->right_hand_side_ptr_ =
      std::make_unique<system::terms::MPILinearTerm>(source_terms);
  system->left_hand_side_ptr_ =
      std::make_unique<system::terms::MPIBilinearTerm>();

  std::cout << "Filling system" << std::endl;

  // Fill system with objects
  for (int group = 0; group < n_groups; ++group) {
    for (int angle = 0; angle < n_angles; ++angle) {
      system::Index index{group, angle};
      // LHS
      auto fixed_matrix_ptr = domain_ptr->MakeSystemMatrix();
      system->left_hand_side_ptr_->SetFixedTermPtr(index, fixed_matrix_ptr);

      // RHS
      auto fixed_vector_ptr =
          std::make_shared<bart::system::MPIVector>(domain_ptr->locally_owned_dofs(),
                                                    MPI_COMM_WORLD);
      system->right_hand_side_ptr_->SetFixedTermPtr(index, fixed_vector_ptr);

      for (auto term : source_terms) {
        auto variable_vector_ptr =
            std::make_shared<bart::system::MPIVector>(domain_ptr->locally_owned_dofs(),
                                                      MPI_COMM_WORLD);
        system->right_hand_side_ptr_->SetVariableTermPtr(
            index, term, variable_vector_ptr);
      }
    }
  }

  std::cout << "Fill system moments" << std::endl;

  // Moments
  system->current_moments =
      std::make_unique<system::moments::SphericalHarmonic>(n_groups, 0);
  system->previous_moments =
      std::make_unique<system::moments::SphericalHarmonic>(n_groups, 0);

  for (auto& moment_pair : system->current_moments->moments()) {
    auto index = moment_pair.first;
    auto& current_moment = system->current_moments->operator[](index);
    current_moment.reinit(solution_ptr->operator[](0).size());
    auto& previous_moment = system->previous_moments->operator[](index);
    previous_moment.reinit(solution_ptr->operator[](0).size());
    current_moment = 1;
    previous_moment = 1;
  }

  // Initialize System
  system->k_effective = 1.0;
  system->total_groups = n_groups;
  system->total_angles = n_angles;

  std::cout << "Build Results Output" << std::endl;
  auto results_output_ptr =
      std::make_unique<results::OutputDealiiVtu<dim>>(domain_ptr);

  return std::make_unique<framework::Framework>(
      std::move(system),
      std::move(initializer_ptr),
      std::move(power_iteration_ptr),
      std::move(results_output_ptr));
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildAngularQuadratureSet(
        problem::ParametersI* problem_parameters)
-> std::shared_ptr<AngularQuadratureSet> {

  std::shared_ptr<AngularQuadratureSet> return_ptr = nullptr;

  std::shared_ptr<quadrature::QuadratureGeneratorI<dim>>
      quadrature_generator_ptr = nullptr;

  const int order_value = problem_parameters->AngularQuadOrder();
  switch (problem_parameters->AngularQuad()) {
    default: {
      if (dim == 3) {
        quadrature_generator_ptr =
            quadrature::factory::MakeAngularQuadratureGeneratorPtr<dim>(
                quadrature::Order(order_value),
                quadrature::AngularQuadratureSetType::kLevelSymmetricGaussian);
      } else {
        AssertThrow(false,
            dealii::ExcMessage("No supported quadratures for this dimension "
                               "and transport model"))
      }
    }
  }

  return_ptr = quadrature::factory::MakeQuadratureSetPtr<dim>();

  auto quadrature_points = quadrature::utility::GenerateAllPositiveX<dim>(
      quadrature_generator_ptr->GenerateSet());

  quadrature::factory::FillQuadratureSet<dim>(return_ptr.get(),
                                              quadrature_points);

  return std::move(return_ptr);
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildConvergenceReporter()
-> std::unique_ptr<ConvergenceReporter> {
  std::unique_ptr<ConvergenceReporter> return_ptr = nullptr;

  using Reporter = bart::convergence::reporter::MpiNoisy;
  int this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  auto pout_ptr = std::make_unique<dealii::ConditionalOStream>(std::cout, this_process == 0);
  return_ptr = std::make_unique<Reporter>(std::move(pout_ptr));

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildDomain(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    std::string material_mapping)-> std::unique_ptr<Domain> {

  // Build mesh
  auto mesh_ptr = std::make_unique<domain::mesh::MeshCartesian<dim>>(
      problem_parameters->SpatialMax(),
      problem_parameters->NCells(),
      material_mapping);

  return std::make_unique<domain::Definition<dim>>(
      std::move(mesh_ptr), finite_element_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildFiniteElement(
    problem::ParametersI *problem_parameters)-> std::unique_ptr<FiniteElement> {
  return std::make_unique<domain::finite_element::FiniteElementGaussian<dim>>(
      problem::DiscretizationType::kContinuousFEM,
      problem_parameters->FEPolynomialDegree());
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildInitializer(
    const problem::ParametersI *problem_parameters,
    const std::shared_ptr<CFEMStamper> &stamper_ptr)
-> std::unique_ptr<Initializer> {

  std::unique_ptr<Initializer> return_ptr = nullptr;

  using FixedUpdaterType = iteration::updater::FixedUpdater<CFEMStamper>;
  auto fixed_updater_ptr = std::make_unique<FixedUpdaterType>(stamper_ptr);

  if (problem_parameters->TransportModel() == problem::EquationType::kDiffusion) {
    return_ptr = std::make_unique<iteration::initializer::SetFixedTermsOnce>(
        std::move(fixed_updater_ptr), problem_parameters->NEnergyGroups(), 1);
  }

  return std::move(return_ptr);
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildInitializer(
    const problem::ParametersI *problem_parameters,
    const std::shared_ptr<CFEMAngularStamper> &stamper_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<Initializer> {

  std::unique_ptr<Initializer> return_ptr = nullptr;

  using FixedUpdaterType = iteration::updater::AngularFixedUpdater<CFEMAngularStamper>;
  auto fixed_updater_ptr = std::make_unique<FixedUpdaterType>(
      stamper_ptr, quadrature_set_ptr);

  if (problem_parameters->TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    return_ptr = std::make_unique<iteration::initializer::SetFixedTermsOnce>(
        std::move(fixed_updater_ptr),
        problem_parameters->NEnergyGroups(),
        quadrature_set_ptr->size());
  }

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildMomentConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<MomentConvergenceChecker>{
  //TODO(Josh): Add option for using other than L1Norm

  using CheckerType = convergence::moments::SingleMomentCheckerL1Norm;
  using FinalCheckerType = convergence::FinalCheckerOrN<
      system::moments::MomentVector,
      convergence::moments::SingleMomentCheckerI>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));

  return_ptr->SetMaxIterations(max_iterations);

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildParameterConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<ParameterConvergenceChecker>{

  using CheckerType = convergence::parameters::SingleParameterChecker;
  using FinalCheckerType = convergence::FinalCheckerOrN<double, CheckerType>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));

  return_ptr->SetMaxIterations(max_iterations);

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSingleGroupSolver(
    const int max_iterations,
    const double convergence_tolerance) -> std::unique_ptr<SingleGroupSolver> {
  std::unique_ptr<SingleGroupSolver> return_ptr = nullptr;

  auto linear_solver_ptr = std::make_unique<solver::GMRES>(max_iterations,
                                                           convergence_tolerance);

  return_ptr = std::move(
      std::make_unique<solver::group::SingleGroupSolver>(
          std::move(linear_solver_ptr)));

  return return_ptr;
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSourceUpdater(
    problem::ParametersI *,
    const std::shared_ptr<CFEMStamper> stamper_ptr)
    -> std::unique_ptr<SourceUpdater> {
  // TODO(Josh): Add option for non-gauss-seidel updating
  using SourceUpdater = iteration::updater::SourceUpdaterGaussSeidel<CFEMStamper>;
  return std::make_unique<SourceUpdater>(stamper_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSourceUpdater(
    problem::ParametersI *,
    const std::shared_ptr<CFEMAngularStamper> stamper_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<SourceUpdater> {
  // TODO(Josh): Add option for non-gauss-seidel updating
  using SourceUpdater = iteration::updater::AngularSourceUpdaterGaussSeidel<CFEMAngularStamper>;
  return std::make_unique<SourceUpdater>(stamper_ptr, quadrature_set_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildStamper(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<Domain> &domain_ptr,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    const std::shared_ptr<CrossSections> &cross_sections_ptr)
-> std::unique_ptr<CFEMStamper> {

  std::unique_ptr<CFEMStamper> return_ptr = nullptr;

  // Diffusion Stamper
  if (problem_parameters->TransportModel() == problem::EquationType::kDiffusion) {

    auto diffusion_ptr = std::make_unique<formulation::scalar::Diffusion<dim>>(
        finite_element_ptr, cross_sections_ptr);

    return_ptr = std::move(
        std::make_unique<formulation::CFEM_DiffusionStamper<dim>>(
            std::move(diffusion_ptr),
            domain_ptr,
            problem_parameters->ReflectiveBoundary()));

  } else {
    AssertThrow(false, dealii::ExcMessage("Unsuppored equation type passed"
                                          " to BuildScalarFormulation"));
  }

  return return_ptr;
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildAngularStamper(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<Domain> &domain_ptr,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    const std::shared_ptr<CrossSections> &cross_sections_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<CFEMAngularStamper> {

  std::unique_ptr<CFEMAngularStamper> return_ptr = nullptr;

  if (problem_parameters->TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {

    auto saaf_formulation_ptr =
        std::make_unique<formulation::angular::SelfAdjointAngularFlux<dim>>(
        finite_element_ptr, cross_sections_ptr, quadrature_set_ptr);

    return_ptr = std::move(
        std::make_unique<formulation::CFEM_SAAF_Stamper<dim>>(
            std::move(saaf_formulation_ptr),
            domain_ptr));
  } else {
    AssertThrow(false, dealii::ExcMessage("Unsuppored equation type passed"
                                          "to BuildScalarFormulation"));
  }

  return return_ptr;
}

template class CFEM_FrameworkBuilder<1>;
template class CFEM_FrameworkBuilder<2>;
template class CFEM_FrameworkBuilder<3>;

} // namespace builder

} // namespace framework

} // namespace bart


#ifndef ASLAM_BACKEND_GENERIC_MATRIX_EXPRESSION_HPP
#define ASLAM_BACKEND_GENERIC_MATRIX_EXPRESSION_HPP

#include <set>
#include <type_traits>
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>
#include <aslam/backend/JacobianContainer.hpp>
#include "ScalarExpression.hpp"
#include "GenericMatrixExpressionNode.hpp"
#include "Differential.hpp"

namespace aslam {
namespace backend {

namespace internal {
template<typename TNode>
struct GenericMatrixNodeTraits {
  typedef TNode node_t;
  typedef typename boost::shared_ptr<node_t> node_ptr_t;
  typedef typename node_t::matrix_t matrix_t;
  typedef typename node_t::value_t value_t;
  typedef typename node_t::tangent_vector_t tangent_vector_t;
  typedef typename node_t::differential_t differential_t;
  typedef typename node_t::constant_t constant_t;
};
}

template<int IRows, int ICols, typename TScalar = double, typename TNode = GenericMatrixExpressionNode<IRows, ICols, TScalar> >
class GenericMatrixExpression {
 public:
  typedef GenericMatrixExpression self_t;
  typedef GenericMatrixExpressionNode<IRows, ICols, TScalar> default_node_t;
  typedef TNode node_t;
  typedef GenericMatrixExpression<IRows, ICols, TScalar, default_node_t> default_self_t;

  typedef internal::GenericMatrixNodeTraits<node_t> node_traits_t;
  typedef typename node_traits_t::node_ptr_t node_ptr_t;
  typedef typename node_traits_t::matrix_t matrix_t;
  typedef typename node_traits_t::value_t value_t;
  typedef typename node_traits_t::tangent_vector_t tangent_vector_t;
  typedef typename node_traits_t::differential_t differential_t;
  typedef typename node_traits_t::constant_t constant_t;

  /// \brief initialize from an existing node.
  GenericMatrixExpression(node_ptr_t root);

  /// \brief Initialize from an existing node. The node will not be deleted.
  GenericMatrixExpression(node_t * root);

  /// \brief Initialize a constant expression from an matrix. The matrix will be copied.
  template<typename DERIVED>
  GenericMatrixExpression(const Eigen::MatrixBase<DERIVED> & mat);

  virtual ~GenericMatrixExpression() {
  }

  /// \brief Evaluate the full Eigen matrix.
  matrix_t toFullMatrix() const;

  /// \brief Evaluate the full Eigen matrix.
  matrix_t evaluate() const {
    return toFullMatrix();
  }

  /// \brief return the expression representing the Moore-Penrose-Inverse of this expression.
  GenericMatrixExpression<ICols, IRows, TScalar> inverse() const;

  /// \brief return the transposed matrix expression.
  GenericMatrixExpression<ICols, IRows, TScalar> transpose() const;

  template<int IColsOther, typename TOtherNode>
  GenericMatrixExpression<IRows, IColsOther, TScalar> operator*(const GenericMatrixExpression<ICols, IColsOther, TScalar, TOtherNode> & other) const;

  inline operator GenericMatrixExpression<IRows, ICols, TScalar>() const;

  default_self_t operator*(const ScalarExpression & scalarExpression) const;
  default_self_t operator*(TScalar scalar) const;
  default_self_t operator-() const;
  template<typename TOtherNode>
  default_self_t operator+(const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode> & other) const;
  template<typename TOtherNode>
  default_self_t operator-(const GenericMatrixExpression<IRows, ICols, TScalar, TOtherNode> & other) const;

  template<int IColsOther, typename TOtherNode, typename std::enable_if<IRows == 3 && (ICols == 1 || IColsOther == 1), int>::type = 0>
  GenericMatrixExpression<3, (ICols == 1 ? IColsOther : ICols), TScalar> cross(const GenericMatrixExpression<3, IColsOther, TScalar, TOtherNode> & other) const;

  /// \brief Evaluate the Jacobians
  void evaluateJacobians(JacobianContainer & outJacobians) const;
  void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const;
  void evaluateJacobians(JacobianContainer & outJacobians, const differential_t & diff) const;

  void getDesignVariables(DesignVariable::set_t & designVariables) const;

  inline node_ptr_t root() const {
    return _root;
  }

  inline static self_t constant(int rows = IRows, int cols = ICols) {
    return GenericMatrixExpression(new constant_t(rows, cols));
  }
 protected:
  GenericMatrixExpression() {
  }
 private:
  node_ptr_t _root;

 public:
  template<typename TDerived, typename TOperand>
  class UnaryOperationResult : public node_t {
   public:
    typedef TOperand operand_t;
    typedef typename TOperand::node_t operand_node_t;
    typedef typename node_t::differential_t::domain_t apply_diff_return_t;

    virtual ~UnaryOperationResult() {
    }

    inline apply_diff_return_t applyDiff(const typename operand_t::tangent_vector_t & /* tangent_vector */) const {
      throw std::runtime_error("This method must be shadowed or not used!");
    }

    static inline self_t create(const TOperand & operand) {
      TDerived * p = new TDerived();
      p->_operand = operand.root();
      return self_t(node_ptr_t(p));
    }
    template<typename TData>
    static inline self_t create(const TOperand & operand, TData data) {
      TDerived * p = new TDerived(data);
      p->_operand = operand.root();
      return self_t(node_ptr_t(p));
    }

   protected:
    inline UnaryOperationResult() {
    }
    const operand_node_t& getOperandNode() const {
      return *_operand;
    }
    virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const {
      _operand->getDesignVariables(designVariables);
    }
    virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const typename node_t::differential_t & chainRuleDifferentail) const {
      this->getOperandNode().evaluateJacobians(outJacobians, Diff(*static_cast<const TDerived *>(this), chainRuleDifferentail));
    }
   private:
    typename TOperand::node_ptr_t _operand;

    class Diff : public ComposedDifferential<typename operand_t::tangent_vector_t, apply_diff_return_t, TScalar, Diff> {
      const TDerived & _derivedNode;
     public:
      typedef ComposedDifferential<typename operand_t::tangent_vector_t, apply_diff_return_t, TScalar, Diff> base_t;
      typedef typename base_t::domain_t domain_t;

      inline Diff(const TDerived & derivedNode, const typename TDerived::differential_t & diff)
          : base_t(diff),
            _derivedNode(derivedNode) {
      }
      ~Diff() {
      }

      inline apply_diff_return_t apply(const typename operand_t::tangent_vector_t & tangent_vector) const {
        return _derivedNode.applyDiff(tangent_vector);
      }
    };
  };

  template<typename TDerived, typename TLhs, typename TRhs>
  class BinaryOperationResult : public node_t {
   public:
    typedef typename node_t::differential_t::domain_t apply_diff_return_t;

    virtual ~BinaryOperationResult() {
    }

    typedef TLhs lhs_t;
    typedef TRhs rhs_t;
    typedef typename TLhs::node_t lhs_node_t;
    typedef typename TRhs::node_t rhs_node_t;

    inline apply_diff_return_t applyLhsDiff(const typename lhs_t::tangent_vector_t & tangent_vector) const {
      throw std::runtime_error("This method must be shadowed or not used!");
    }
    inline apply_diff_return_t applyRhsDiff(const typename rhs_t::tangent_vector_t & tangent_vector) const {
      throw std::runtime_error("This method must be shadowed or not used!");
    }

    static inline self_t create(const TLhs & l, const TRhs & r) {
      TDerived * p = new TDerived();
      p->_lhs = l.root();
      p->_rhs = r.root();
      return self_t(node_ptr_t(p));
    }
   protected:
    inline BinaryOperationResult() {
    }
    const lhs_node_t & getLhsNode() const {
      return *_lhs;
    }
    const rhs_node_t & getRhsNode() const {
      return *_rhs;
    }
    virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const {
      _lhs->getDesignVariables(designVariables);
      _rhs->getDesignVariables(designVariables);
    }
    virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const typename node_t::differential_t & diff) const {
      this->getLhsNode().evaluateJacobians(outJacobians, Diff<lhs_t, &TDerived::applyLhsDiff>(*static_cast<const TDerived *>(this), diff));
      this->getRhsNode().evaluateJacobians(outJacobians, Diff<rhs_t, &TDerived::applyRhsDiff>(*static_cast<const TDerived *>(this), diff));
    }
   private:
    typename TLhs::node_ptr_t _lhs;
    typename TRhs::node_ptr_t _rhs;

    template<typename TSide, apply_diff_return_t (TDerived::*FApply)(const typename TSide::tangent_vector_t &) const>
    class Diff : public ComposedDifferential<typename TSide::tangent_vector_t, apply_diff_return_t, TScalar, Diff<TSide, FApply> > {
      const TDerived & _derivedNode;
     public:
      typedef ComposedDifferential<typename TSide::tangent_vector_t, apply_diff_return_t, TScalar, Diff> base_t;
      typedef typename base_t::domain_t domain_t;

      inline Diff(const TDerived & derivedNode, const typename TDerived::differential_t & diff)
          : base_t(diff),
            _derivedNode(derivedNode) {
      }
      ~Diff() {
      }

      inline apply_diff_return_t apply(const typename TSide::tangent_vector_t & tangent_vector) const {
        return (_derivedNode.*FApply)(tangent_vector);
      }
    };
  };
};

}  // namespace backend
}  // namespace aslam

#include "implementation/GenericMatrixExpression.hpp"

#endif /* ASLAM_BACKEND_MATRIX_EXPRESSION_HPP */


#include "satimages.h"

#include <OpenImageIO/imagebuf.h>
#include <OpenImageIO/imagebufalgo.h>

using namespace OpenImageIO_v2_3;

// Range TODO: replace with C++20 Range
#include <boost/range/counting_range.hpp>

float_t mean(std::vector<float_t>& equalities)
{
    float_t mean = 0;
    std::for_each(equalities.begin(), equalities.end(), [&mean](float_t& val) {
        mean += val;
        });

    mean /= equalities.size();
    return mean;
}

float_t compareColors(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2) {
    return (1 - (std::sqrt(std::pow(r2 - r1, 2) + std::pow(g2 - g1, 2) + std::pow(b2 - b1, 2)) / GRAD_MEH_MAX_COLOR_DIF));
}

float_t compareRows(std::vector<uint8_t>::iterator mipmap1It, std::vector<uint8_t>::iterator mipmap2It, uint32_t width) {
    std::vector<float_t> equalities = {};
    for (size_t i = 0; i < width; i++) {
        equalities.push_back(compareColors(*mipmap1It, *(mipmap1It + 1), *(mipmap1It + 2), *mipmap2It, *(mipmap2It + 1), *(mipmap2It + 2)));
        mipmap1It += 4;
        mipmap2It += 4;
    }

    return mean(equalities);
}

uint32_t calcOverLap(MipMap& mipmap1, MipMap& mipmap2) {
    std::vector<std::pair<float_t, size_t>> bestMatchingOverlaps = {};
    auto filter = mipmap1.height > 512 ? 6 : 3;

    for (uint32_t overlap = 1; overlap < (mipmap1.height / filter); overlap++) {
        std::vector<float_t> equalities = {};

        for (size_t i = 0; i < overlap; i++) {
            auto beginRowUpper = mipmap1.data.begin() + (mipmap2.height - 1 - (overlap - i)) * (size_t)mipmap1.width * 4;
            auto beginRowLower = mipmap2.data.begin() + i * (size_t)mipmap2.width * 4;

            equalities.push_back(compareRows(beginRowUpper,
                beginRowLower,
                (uint32_t)mipmap1.width));
        }

        auto meanVal = mean(equalities);
        if (meanVal > GRAD_MEH_OVERLAP_THRESHOLD) {
            bestMatchingOverlaps.push_back({ meanVal, overlap });
        }
    }

    uint32_t calculatedOverlap = 0;
    float_t bestOverlap = 0;

    for (auto& kvp : bestMatchingOverlaps) {
        if (kvp.first > bestOverlap) {
            bestOverlap = kvp.first;
            calculatedOverlap = kvp.second;
        }
    }

    return calculatedOverlap + 1;
}

void writeSatImages(grad_aff::Wrp& wrp, const int32_t& worldSize, std::filesystem::path& basePathSat, const std::string& worldName)
{
    std::vector<std::string> rvmats = {};
    for (auto& rv : wrp.rvmats) {
        if (rv != "") {
            rvmats.push_back(rv);
        }
    }

    std::sort(rvmats.begin(), rvmats.end());

    if (rvmats.size() > 1) {
        std::vector<std::string> lcoPaths = {};
        lcoPaths.reserve(rvmats.size());

        auto rvmatPbo = grad_aff::Pbo::Pbo(findPboPath(rvmats[0]).string());
        rvmatPbo.readPbo(false);

        // Has to be not empty
        std::string lastValidRvMat = "1337";
        std::string fillerTile = "";
        std::string prefix = "s_";

        uint32_t maxX2 = 0;
        uint32_t maxY2 = 0;

        // TODO: refactor this when implementing the new rap api
        for (auto& rvmatPath : rvmats) {

            std::vector<std::string> parts;
            boost::split(parts, ((fs::path)rvmatPath).filename().string(), boost::is_any_of("_"));

            if (parts.size() > 1) {
                std::vector<std::string> numbers;
                boost::split(numbers, parts[1], boost::is_any_of("-"));

                if (numbers.size() > 1) {
                    auto x = std::stoi(numbers[0]);
                    auto y = std::stoi(numbers[1]);

                    if (x > maxX2) {
                        maxX2 = x;
                    }

                    if (y > maxY2) {
                        maxY2 = y;
                    }
                }
            }

            if (!boost::istarts_with(((fs::path)rvmatPath).filename().string(), lastValidRvMat)) {
                auto rap = grad_aff::Rap::Rap(rvmatPbo.getEntryData(rvmatPath));
                rap.readRap();

                for (auto& entry : rap.classEntries) {
                    if (boost::iequals(entry->name, "Stage0")) {
                        auto rapClassPtr = std::static_pointer_cast<RapClass>(entry);

                        for (auto& subEntry : rapClassPtr->classEntries) {
                            if (boost::iequals(subEntry->name, "texture")) {
                                auto textureStr = std::get<std::string>(std::static_pointer_cast<RapValue>(subEntry)->value);
                                auto rvmatFilename = ((fs::path)textureStr).filename().string();
                                if (!boost::istarts_with(rvmatFilename, prefix)) {
                                    if (fillerTile == "") {
                                        fillerTile = textureStr;
                                    }
                                }
                                else {
                                    lcoPaths.push_back(textureStr);
                                    lastValidRvMat = rvmatFilename.substr(0, 9);
                                }
                                break;
                            }
                        }
                    }
                }
            }
        }

        // Remove Duplicates
        std::sort(lcoPaths.begin(), lcoPaths.end());
        auto last = std::unique(lcoPaths.begin(), lcoPaths.end());
        lcoPaths.erase(last, lcoPaths.end());

        // Fix wrong file extensions (cup summer has png exten
        for (auto& lcoPath : lcoPaths) {
            fs::path lcoPathAsPath = (fs::path)lcoPath;
            if (!boost::iequals(lcoPathAsPath.extension().string(), ".paa")) {
                lcoPath = lcoPathAsPath.replace_extension(".paa").string();
            }
        }

        // "strechted border" overlap is og overlap/2

        // find highest x/y
        std::vector<std::string> splitResult = {};
        boost::split(splitResult, ((fs::path)lcoPaths[lcoPaths.size() - 1]).filename().string(), boost::is_any_of("_"));
        uint32_t maxX = maxX2; //std::stoi(splitResult[1]);
        uint32_t maxY = maxY2; //std::stoi(splitResult[2]);

        // find two tiles "in the middle"
        uint32_t x = maxX / 2;
        uint32_t y = maxY / 2;

        prefix = "00";

        if (y > 9 && y < 100) {
            prefix = "0";
        }
        else if (y >= 100) {
            prefix = "";
        }

        std::stringstream upperLcoPathStream;
        upperLcoPathStream << x << "_" << prefix << y << "_lco.paa";
        auto upperPaaPath = upperLcoPathStream.str();

        std::stringstream lowerLcoPathStream;
        lowerLcoPathStream << x << "_" << prefix << (y + 1) << "_lco.paa";
        auto lowerPaaPath = lowerLcoPathStream.str();

        MipMap upperMipmap;
        MipMap lowerMipmap;

        for (auto& lcoPath : lcoPaths) {
            if (boost::iends_with(lcoPath, upperPaaPath)) {
                auto upperPbo = grad_aff::Pbo::Pbo(findPboPath(lcoPath).string());
                auto upperData = upperPbo.getEntryData(lcoPath);
                auto upperPaa = grad_aff::Paa::Paa();
                upperPaa.readPaa(upperData);
                upperMipmap = upperPaa.mipMaps[0];
            }
            if (boost::iends_with(lcoPath, lowerPaaPath)) {
                auto lowerPbo = grad_aff::Pbo::Pbo(findPboPath(lcoPath).string());
                auto lowerData = lowerPbo.getEntryData(lcoPath);
                auto lowerPaa = grad_aff::Paa::Paa();
                lowerPaa.readPaa(lowerData);
                lowerMipmap = lowerPaa.mipMaps[0];
            }
        }

        auto overlap = calcOverLap(upperMipmap, lowerMipmap);

        ImageBuf dst(ImageSpec(upperMipmap.width + maxX * (upperMipmap.width - overlap), upperMipmap.height + maxY * (upperMipmap.height - overlap), 4, TypeDesc::UINT8));

        if (fillerTile != "") {
            auto fillerPbo = grad_aff::Pbo::Pbo(findPboPath(fillerTile).string());
            auto fillerData = fillerPbo.getEntryData(fillerTile);
            auto fillerPaa = grad_aff::Paa::Paa();
            fillerPaa.readPaa(fillerData);

            ImageBuf src(ImageSpec(fillerPaa.mipMaps[0].height, fillerPaa.mipMaps[0].height, 4, TypeDesc::UINT8), fillerPaa.mipMaps[0].data.data());

            size_t noOfTiles = (worldSize / fillerPaa.mipMaps[0].height);

            auto cr = boost::counting_range(size_t(0), noOfTiles);
            std::for_each(std::execution::par_unseq, cr.begin(), cr.end(), [noOfTiles, &dst, fillerPaa, src](size_t i) {
                for (size_t j = 0; j < noOfTiles; j++) {
                    ImageBufAlgo::paste(dst, (i * fillerPaa.mipMaps[0].height), (j * fillerPaa.mipMaps[0].height), 0, 0, src);
                }
                });
        }

        auto pbo = grad_aff::Pbo::Pbo(findPboPath(lcoPaths[0]).string());

        for (auto& lcoPath : lcoPaths) {
            auto data = pbo.getEntryData(lcoPath);

            if (data.empty()) {
                pbo = grad_aff::Pbo::Pbo(findPboPath(lcoPath).string());
                data = pbo.getEntryData(lcoPath);
            }

            auto paa = grad_aff::Paa::Paa();
            paa.readPaa(data);

            std::vector<std::string> splitResult = {};
            boost::split(splitResult, ((fs::path)lcoPath).filename().string(), boost::is_any_of("_"));

            ImageBuf src(ImageSpec(paa.mipMaps[0].height, paa.mipMaps[0].height, 4, TypeDesc::UINT8), paa.mipMaps[0].data.data());

            uint32_t x = std::stoi(splitResult[1]);
            uint32_t y = std::stoi(splitResult[2]);

            ImageBufAlgo::paste(dst, (x * paa.mipMaps[0].height - x * overlap), (y * paa.mipMaps[0].height - y * overlap), 0, 0, src);
        }

        // https://github.com/pennyworth12345/A3_MMSI/wiki/Mapframe-Information#stretching-at-edge-tiles
        maxX += 1;

        auto hasEqualStrechting = (worldSize == (upperMipmap.width * maxX)) || (worldSize == ((upperMipmap.width - overlap) * maxX));
        auto strechedBorderSizeStart = overlap / 2;
        auto strechedBorderSizeEnd = strechedBorderSizeStart;

        if (!hasEqualStrechting) {
            auto tileSizeAfterOverlap = upperMipmap.width - overlap;
            auto temp = (tileSizeAfterOverlap * maxX) - worldSize;
            temp -= (overlap / 2);
            auto lastTileRealWidth = tileSizeAfterOverlap - temp;
            strechedBorderSizeEnd = upperMipmap.width - lastTileRealWidth;
        }

        dst = ImageBufAlgo::cut(dst, ROI(strechedBorderSizeStart, dst.spec().width - strechedBorderSizeEnd, strechedBorderSizeStart, dst.spec().height - strechedBorderSizeEnd));
        size_t tileSize = dst.spec().width / 4;

        auto cr = boost::counting_range(0, 4);
        std::for_each(std::execution::par_unseq, cr.begin(), cr.end(), [basePathSat, dst, tileSize](int i) {
            auto curWritePath = basePathSat / std::to_string(i);
            if (!fs::exists(curWritePath)) {
                fs::create_directories(curWritePath);
            }

            for (int32_t j = 0; j < 4; j++) {
                ImageBuf out = ImageBufAlgo::cut(dst, ROI(i * tileSize, (i + 1) * tileSize, j * tileSize, (j + 1) * tileSize));
                out.write((curWritePath / std::to_string(j).append(".png")).string());
            }
            });
    }
}