/* Copyright 2019 Anis KHLIF. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include <sstream>
#include <unordered_set>
#include <unordered_map>
#include <iterator>
#include <vector>
#include <cassert>

#include <iostream>

#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/lib/random/philox_random.h"
#include "tensorflow/core/lib/random/simple_philox.h"
#include "tensorflow/core/util/guarded_philox_random.h"

#include <boost/graph/graphml.hpp>
#include <boost/graph/adjacency_list.hpp>

#include "sampling.h"
#include "graph_kernel_base.h"
#include "graphseq_kernels.h"

using namespace tensorflow;

namespace gseq{

Status Node2VecSeqOp::MakeGraphTypeAndInit(Env* env, const string& filename){
    bool has_weight = HasWeights();
    if(directed_){
        typedef graph_types<true>::Graph Graph;
        Graph graph;
        return init_with_graph<Node2VecSeqOp, Graph>(this, env, filename, graph);
    }
    else{
        typedef graph_types<false>::Graph Graph;
        Graph graph;
        return init_with_graph<Node2VecSeqOp, Graph>(this, env, filename, graph);
    }
}

void Node2VecSeqOp::PrecomputeWalk(int walk_idx, int start_node, random::SimplePhilox& gen){
    // First sample start node
    Alias a = node_alias_[start_node];
    int from_node;
    if(HasWeights()){
        from_node = sample_alias(a, gen);
    }
    else{
        auto i = gen.Uniform(a.idx.size());
        from_node = a.idx[i];
    }

    // Now sample using w2v distribution
    int prev_node = start_node;
    auto w = precomputed_walks.matrix<int32>();
    w(walk_idx, 0) = start_node;
    w(walk_idx, 1) = from_node;
    //w[1] = from_node;
    for(int k=2; k < seq_size_; k++){
        assert(edge_alias_[from_node].find(prev_node) != edge_alias_[from_node].end());
        Alias* a = &(edge_alias_[from_node][prev_node]);
        int next_node = sample_alias(*a, gen);
        w(walk_idx, k) = (int32) next_node;
        prev_node = from_node; from_node = next_node;
    }
}

Status Node2VecSeqOp::Init(Env* env, const string& filename) {
  // std::cout << "Init" << std::endl;
    if (p_ == 0. || q_ == 0.) {
        return errors::InvalidArgument("The parameters p and q can't be 0.");
    }
    if (seq_size_ < 2) {
        return errors::InvalidArgument("The sequence size must be greater than two");
    }
    write_walk_idx = 0;
    cur_walk_idx = 0;

    precomputed_walks = Tensor(DT_INT32, TensorShape({PRECOMPUTE, seq_size_}));
    return MakeGraphTypeAndInit(env, filename);
}

std::vector<std::unordered_map<int, Alias>>* Node2VecSeqOp::getEdgeAlias(){
    return &edge_alias_;
}

Status RandWalkSeq::MakeGraphTypeAndInit(Env* env, const string& filename){
    bool has_weight = HasWeights();
    if(directed_){
      typedef graph_types<true>::Graph Graph;
      Graph graph;
      return init_with_graph<RandWalkSeq, Graph>(this, env, filename, graph);
    }
    else{
      typedef graph_types<false>::Graph Graph;
      Graph graph;
      return init_with_graph<RandWalkSeq, Graph>(this, env, filename, graph);
    }
}

void RandWalkSeq::PrecomputeWalk(int walk_idx, int start_node, random::SimplePhilox& gen){
  int node = start_node;
  auto w = precomputed_walks.matrix<int32>();
  w(walk_idx, 0) = start_node;
  //w[1] = from_node;
  for(int k=1; k < seq_size_; k++){
    Alias a = node_alias_[node];
    if(HasWeights()){
      node = sample_alias(a, gen);
    }
    else{
      auto i = gen.Uniform(a.idx.size());
      node = a.idx[i];
    }
    w(walk_idx, k) = (int32) node;
  }
}

Status RandWalkSeq::Init(Env* env, const string& filename) {
  if (seq_size_ < 2) {
    return errors::InvalidArgument("The sequence's size must be greater than two");
  }
  write_walk_idx = 0;
  cur_walk_idx = 0;
  precomputed_walks = Tensor(DT_INT32, TensorShape({PRECOMPUTE, seq_size_}));
  return MakeGraphTypeAndInit(env, filename);
}

// template<> Status init_with_graph<RandWalkSeq, typename graph_types<true>::Graph>;
// template<> Status init_with_graph<RandWalkSeq, typename graph_types<false>::Graph>;

REGISTER_KERNEL_BUILDER(Name("RandWalkSeq").Device(DEVICE_CPU), RandWalkSeq);

REGISTER_KERNEL_BUILDER(Name("Node2VecSeq").Device(DEVICE_CPU), Node2VecSeqOp);

} // Namespace


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <vector>

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

#include <Eigen/Core>

#include <tudat/astro/basic_astro/physicalConstants.h>
#include <tudat/basics/testMacros.h>
#include <tudat/math/basic/mathematicalConstants.h>
#include "tudat/astro/basic_astro/unitConversions.h"
#include <tudat/astro/basic_astro/orbitalElementConversions.h>

#include <tudat/io/basicInputOutput.h>

#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/gravitation/gravityFieldModel.h"
#include "tudat/astro/mission_segments/createTransferTrajectory.h"
#include "tudat/simulation/environment_setup/body.h"
#include "tudat/simulation/environment_setup/createBodies.h"

namespace tudat
{
namespace unit_tests
{

// Additional namespaces to be used.
using namespace tudat::input_output;
using namespace tudat::simulation_setup;
using namespace mission_segments;

//! Test implementation of trajectory class
BOOST_AUTO_TEST_SUITE( test_trajectory )

BOOST_AUTO_TEST_CASE( testMGATrajectory_New )
{
    // Expected test result based on the ideal Cassini 1 trajectory as modelled by GTOP software
    // distributed and downloadable from the ESA website, or within the PaGMO Astrotoolbox.
    const double expectedDeltaV = 4930.72686847243;

    // Create environment
    tudat::simulation_setup::SystemOfBodies bodies = createSimplifiedSystemOfBodies( );

    // Set transfer order
    std::vector< std::string > bodyOrder = {
        "Earth", "Venus", "Venus", "Earth", "Jupiter", "Saturn" };
    int numberOfNodes = bodyOrder.size( );

    std::shared_ptr< TransferTrajectory > transferTrajectory;
    double nominalDeltaV = TUDAT_NAN;
    double nominalCaptureDeltaV = TUDAT_NAN;

    for( unsigned int creationType = 0; creationType < 3; creationType++ )
    {
        // Create leg settings (all unpowered)
        std::vector< std::shared_ptr< TransferLegSettings > > transferLegSettings;
        std::vector< std::shared_ptr< TransferNodeSettings > > transferNodeSettings;

        // Define minimum periapsis altitudes for flybys;
        std::map< std::string, double > minimumPeriapses;
        minimumPeriapses[ "Venus" ] = 6351800.0;
        minimumPeriapses[ "Earth" ] = 6678000.0;
        minimumPeriapses[ "Jupiter" ] =  600000000.0;
        minimumPeriapses[ "Saturn" ] = 65000000.0;

        if( creationType == 0 )
        {
            transferLegSettings.resize( numberOfNodes - 1 );
            transferLegSettings[ 0 ] = unpoweredLeg( );
            transferLegSettings[ 1 ] = unpoweredLeg( );
            transferLegSettings[ 2 ] = unpoweredLeg( );
            transferLegSettings[ 3 ] = unpoweredLeg( );
            transferLegSettings[ 4 ] = unpoweredLeg( );

            transferNodeSettings.resize( numberOfNodes );
            transferNodeSettings[ 0 ] = escapeAndDepartureNode( std::numeric_limits< double >::infinity( ), 0.0 );
            transferNodeSettings[ 1 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 1 ) ) );
            transferNodeSettings[ 2 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 2 ) ) );
            transferNodeSettings[ 3 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 3 ) ) );
            transferNodeSettings[ 4 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 4 ) ) );
            transferNodeSettings[ 5 ] = captureAndInsertionNode( 1.0895e8 / 0.02, 0.98 );
        }
        else if( creationType == 1 )
        {
            getMgaTransferTrajectorySettingsWithoutDsm(
                        transferLegSettings, transferNodeSettings, bodyOrder,
                        std::make_pair( std::numeric_limits< double >::infinity( ), 0.0 ),
                        std::make_pair( 1.0895e8 / 0.02, 0.98 ),
                        minimumPeriapses );
        }
        else if( creationType == 2 )
        {
            getMgaTransferTrajectorySettingsWithoutDsm(
                        transferLegSettings, transferNodeSettings, bodyOrder,
                        std::make_pair( std::numeric_limits< double >::infinity( ), 0.0 ),
                        std::make_pair( TUDAT_NAN, TUDAT_NAN ),
                        minimumPeriapses );

        }
        transferTrajectory = createTransferTrajectory(
                    bodies, transferLegSettings, transferNodeSettings, bodyOrder, "Sun" );

        // Create list of node times
        double JD = physical_constants::JULIAN_DAY;
        std::vector< double > nodeTimes;
        // Subtract 0.5 to go from MJD2000 to J2000
        nodeTimes.push_back( ( -789.8117 - 0.5 ) * JD );
        nodeTimes.push_back( nodeTimes.at( 0 ) + 158.302027105278 * JD );
        nodeTimes.push_back( nodeTimes.at( 1 ) + 449.385873819743 * JD );
        nodeTimes.push_back( nodeTimes.at( 2 ) + 54.7489684339665 * JD );
        nodeTimes.push_back( nodeTimes.at( 3 ) + 1024.36205846918 * JD );
        nodeTimes.push_back( nodeTimes.at( 4 ) + 4552.30796805542 * JD );

        std::vector< Eigen::VectorXd > transferLegFreeParameters;
        for( int i = 0; i < numberOfNodes - 1; i++ )
        {
            transferLegFreeParameters.push_back( Eigen::VectorXd( 0 ) );
        }
        std::vector< Eigen::VectorXd > transferNodeFreeParameters;
        for( int i = 0; i < numberOfNodes; i++ )
        {
            transferNodeFreeParameters.push_back( Eigen::VectorXd( 0 ) );
        }
        if( creationType == 2 )
        {
            transferNodeFreeParameters[ numberOfNodes - 1 ] = ( Eigen::Vector3d( )<<65000000.0, 0.0, 0.0 ).finished( );
        }

        printTransferParameterDefinition( transferLegSettings, transferNodeSettings );

        transferTrajectory->evaluateTrajectory(
                    nodeTimes, transferLegFreeParameters, transferNodeFreeParameters );

        if( creationType < 2 )
        {
            BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, transferTrajectory->getTotalDeltaV( ), 1.0E-3 );

            if( creationType == 0 )
            {
                nominalDeltaV = transferTrajectory->getTotalDeltaV( );
                nominalCaptureDeltaV = transferTrajectory->getNodeDeltaV( 5 );
            }
            else
            {
                BOOST_CHECK_CLOSE_FRACTION( nominalDeltaV, transferTrajectory->getTotalDeltaV( ), 1.0E-12 );
            }
        }
        else if( creationType == 2 )
        {

            BOOST_CHECK_CLOSE_FRACTION( nominalCaptureDeltaV, nominalDeltaV - transferTrajectory->getTotalDeltaV( ), 1.0E-12 );
        }

        std::vector< std::map< double, Eigen::Vector6d > > statesAlongTrajectoryPerLeg;
        transferTrajectory-> getStatesAlongTrajectoryPerLeg(
                    statesAlongTrajectoryPerLeg, 10 );

        double sunGravitationalParameter = bodies.at( "Sun" )->getGravitationalParameter( );
        for( unsigned int i = 0; i < statesAlongTrajectoryPerLeg.size( ); i++ )
        {
            double legStartTime = nodeTimes.at( i );
            double legEndTime = nodeTimes.at( i + 1 );

            std::map< double, Eigen::Vector6d > statesAlongSingleLeg = statesAlongTrajectoryPerLeg.at( i );

            // Check initial and final time on output list
            BOOST_CHECK_CLOSE_FRACTION( statesAlongSingleLeg.begin( )->first, legStartTime, 1.0E-14 );
            BOOST_CHECK_CLOSE_FRACTION( statesAlongSingleLeg.rbegin( )->first, legEndTime, 1.0E-14 );

            // Check if Keplerian state (slow elements) is the same for each output point
            Eigen::Vector6d previousKeplerianState = Eigen::Vector6d::Constant( TUDAT_NAN );
            for( auto it : statesAlongSingleLeg )
            {
                Eigen::Vector6d currentCartesianState = it.second;
                Eigen::Vector6d currentKeplerianState = tudat::orbital_element_conversions::convertCartesianToKeplerianElements(
                            currentCartesianState, sunGravitationalParameter );
                if( previousKeplerianState == previousKeplerianState )
                {
                    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                                ( previousKeplerianState.segment( 0, 5 ) ),
                                ( currentKeplerianState.segment( 0, 5 ) ),
                                1.0E-10 );

                }
                previousKeplerianState = currentKeplerianState;
            }

            // Check if output meets boundary conditions
            Eigen::Vector3d depatureBodyPosition = bodies.at( bodyOrder.at( i ) )->getStateInBaseFrameFromEphemeris(
                        legStartTime ).segment( 0, 3 );
            Eigen::Vector3d arrivalBodyPosition = bodies.at( bodyOrder.at( i + 1 ) )->getStateInBaseFrameFromEphemeris(
                        legEndTime ).segment( 0, 3 );

            for( int j = 0; j < 3; j++ )
            {
                //TODO: Find out why tolerance needs to be so big for one of the legs
                BOOST_CHECK_SMALL( std::fabs( statesAlongSingleLeg.begin( )->second( j ) - depatureBodyPosition( j ) ), 20.0E3 );
                BOOST_CHECK_SMALL( std::fabs( statesAlongSingleLeg.rbegin( )->second( j ) - arrivalBodyPosition( j ) ), 20.0E3 );
            }
        }
    }
}

//! Test delta-V computation for MGA-1DSM Velocity Formulation trajectory model.
BOOST_AUTO_TEST_CASE( testMGA1DSMVFTrajectory1 )
{
    // Expected test result based on the ideal Messenger trajectory as modelled by GTOP software
    // distributed and downloadable from the ESA website, or within the PaGMO Astrotoolbox.
    const double expectedDeltaV = 8630.83256199051;

    // Create environment
    simulation_setup::SystemOfBodies bodies = createSimplifiedSystemOfBodies( );

    // Set transfer order
    std::vector< std::string > bodyOrder = {
        "Earth", "Earth", "Venus", "Venus", "Mercury" };
    int numberOfNodes = bodyOrder.size( );

    // Create leg settings
    std::vector< std::shared_ptr< TransferLegSettings > > transferLegSettings;
    transferLegSettings.resize( numberOfNodes - 1 );
    transferLegSettings[ 0 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 1 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 2 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 3 ] = dsmVelocityBasedLeg( );

    // Create node settings
    std::vector< std::shared_ptr< TransferNodeSettings > > transferNodeSettings;
    transferNodeSettings.resize( numberOfNodes );
    transferNodeSettings[ 0 ] = escapeAndDepartureNode( std::numeric_limits< double >::infinity( ), 0.0 );
    transferNodeSettings[ 1 ] = swingbyNode( );
    transferNodeSettings[ 2 ] = swingbyNode( );
    transferNodeSettings[ 3 ] = swingbyNode( );
    transferNodeSettings[ 4 ] = captureAndInsertionNode( std::numeric_limits< double >::infinity( ), 0.0 );

    std::shared_ptr< TransferTrajectory > transferTrajectory = createTransferTrajectory(
                bodies, transferLegSettings, transferNodeSettings, bodyOrder, "Sun" );

    // Add the time of flight and start epoch, which are in JD.
    double JD = physical_constants::JULIAN_DAY;
    std::vector< double > nodeTimes;
    nodeTimes.push_back( ( 1171.64503236 - 0.5 ) * JD );
    nodeTimes.push_back( nodeTimes.at( 0 ) + 399.999999715 * JD );
    nodeTimes.push_back( nodeTimes.at( 1 ) + 178.372255301 * JD );
    nodeTimes.push_back( nodeTimes.at( 2 ) + 299.223139512  * JD );
    nodeTimes.push_back( nodeTimes.at( 3 ) + 180.510754824 * JD );

    std::vector< Eigen::VectorXd > transferLegFreeParameters;
    transferLegFreeParameters.resize( numberOfNodes - 1 );
    transferLegFreeParameters[ 0 ] = ( Eigen::VectorXd( 1 ) << 0.234594654679 ).finished( );
    transferLegFreeParameters[ 1 ] = ( Eigen::VectorXd( 1 ) << 0.0964769387134 ).finished( );
    transferLegFreeParameters[ 2 ] = ( Eigen::VectorXd( 1 ) << 0.829948744508 ).finished( );
    transferLegFreeParameters[ 3 ] = ( Eigen::VectorXd( 1 ) << 0.317174785637 ).finished( );

    std::vector< Eigen::VectorXd > transferNodeFreeParameters;
    transferNodeFreeParameters.resize( numberOfNodes );
    transferNodeFreeParameters[ 0 ] = ( Eigen::VectorXd( 3 ) <<1408.99421278, 0.37992647165 * 2 * 3.14159265358979, std::acos(  2 * 0.498004040298 - 1. ) - 3.14159265358979 / 2 ).finished( );
    transferNodeFreeParameters[ 1 ] = ( Eigen::VectorXd( 3 ) <<1.80629232251 * 6.378e6, 1.35077257078, 0.0 ).finished( );
    transferNodeFreeParameters[ 2 ] = ( Eigen::VectorXd( 3 ) <<3.04129845698 * 6.052e6, 1.09554368115, 0.0 ).finished( );
    transferNodeFreeParameters[ 3 ] = ( Eigen::VectorXd( 3 ) <<1.10000000891 * 6.052e6, 1.34317576594, 0.0 ).finished( );

    transferTrajectory->evaluateTrajectory(
                nodeTimes, transferLegFreeParameters, transferNodeFreeParameters );
    printTransferParameterDefinition( transferLegSettings, transferNodeSettings );

    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, transferTrajectory->getTotalDeltaV( ), 1.0E-3 );
}

//! Test delta-V computation for another MGA-1DSM Velocity Formulation trajectory model.
BOOST_AUTO_TEST_CASE( testMGA1DSMVFTrajectory2 )
{
    // Expected test result based on the ideal Cassini 2 trajectory as modelled by GTOP software
    // distributed and downloadable from the ESA website, or within the PaGMO Astrotoolbox.
    const double expectedDeltaV = 8385.15784516116;

    // Create environment
    simulation_setup::SystemOfBodies bodies = createSimplifiedSystemOfBodies( );

    // Set transfer order
    std::vector< std::string > bodyOrder = { "Earth", "Venus", "Venus",  "Earth", "Jupiter", "Saturn" };
    int numberOfNodes = bodyOrder.size( );

    // Create leg settings
    std::vector< std::shared_ptr< TransferLegSettings > > transferLegSettings;
    transferLegSettings.resize( numberOfNodes - 1 );
    transferLegSettings[ 0 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 1 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 2 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 3 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 4 ] = dsmVelocityBasedLeg( );

    std::vector< std::shared_ptr< TransferNodeSettings > > transferNodeSettings;
    transferNodeSettings.resize( numberOfNodes );
    transferNodeSettings[ 0 ] = escapeAndDepartureNode( std::numeric_limits< double >::infinity( ), 0.0 );
    transferNodeSettings[ 1 ] = swingbyNode( );
    transferNodeSettings[ 2 ] = swingbyNode( );
    transferNodeSettings[ 3 ] = swingbyNode( );
    transferNodeSettings[ 4 ] = swingbyNode( );
    transferNodeSettings[ 5 ] = captureAndInsertionNode( std::numeric_limits< double >::infinity( ), 0.0 );

    std::shared_ptr< TransferTrajectory > transferTrajectory = createTransferTrajectory(
                bodies, transferLegSettings, transferNodeSettings, bodyOrder, "Sun" );

    // Add the time of flight and start epoch, which are in JD.
    double JD = physical_constants::JULIAN_DAY;
    std::vector< double > nodeTimes;
    nodeTimes.push_back( ( -779.046753814506 - 0.5 ) * JD );
    nodeTimes.push_back( nodeTimes.at( 0 ) + 167.378952534645 * JD );
    nodeTimes.push_back( nodeTimes.at( 1 ) + 424.028254165204 * JD );
    nodeTimes.push_back( nodeTimes.at( 2 ) + 53.2897409769205  * JD );
    nodeTimes.push_back( nodeTimes.at( 3 ) + 589.766954923325 * JD );
    nodeTimes.push_back( nodeTimes.at( 4 ) + 2200.00000000000 * JD );

    std::vector< Eigen::VectorXd > transferLegFreeParameters;
    transferLegFreeParameters.resize( numberOfNodes - 1 );
    transferLegFreeParameters[ 0 ] = ( Eigen::VectorXd( 1 ) << 0.769483451363201 ).finished( );
    transferLegFreeParameters[ 1 ] = ( Eigen::VectorXd( 1 ) << 0.513289529822621 ).finished( );
    transferLegFreeParameters[ 2 ] = ( Eigen::VectorXd( 1 ) << 0.0274175362264024 ).finished( );
    transferLegFreeParameters[ 3 ] = ( Eigen::VectorXd( 1 ) << 0.263985256705873 ).finished( );
    transferLegFreeParameters[ 4 ] = ( Eigen::VectorXd( 1 ) << 0.599984695281461 ).finished( );

    std::vector< Eigen::VectorXd > transferNodeFreeParameters;
    transferNodeFreeParameters.resize( numberOfNodes );
    transferNodeFreeParameters[ 0 ] = ( Eigen::VectorXd( 3 ) <<3259.11446832345, 0.525976214695235 * 2 * 3.14159265358979, std::acos(  2 * 0.38086496458657 - 1 ) - 3.14159265358979 / 2 ).finished( );; // 1st leg.
    transferNodeFreeParameters[ 1 ] = ( Eigen::VectorXd( 3 ) <<1.34877968657176 * 6.052e6, -1.5937371121191, 0.0  ).finished( ); // 2nd leg.
    transferNodeFreeParameters[ 2 ] = ( Eigen::VectorXd( 3 ) <<1.05 * 6.052e6, -1.95952512232447, 0.0  ).finished( );// 3rd leg.
    transferNodeFreeParameters[ 3 ] = ( Eigen::VectorXd( 3 ) <<1.30730278372017 * 6.378e6, -1.55498859283059, 0.0  ).finished( ); //4th leg.
    transferNodeFreeParameters[ 4 ] = ( Eigen::VectorXd( 3 ) <<69.8090142993495 * 7.1492e7, -1.5134625299674, 0.0  ).finished( );//4th leg.
    transferNodeFreeParameters[ 5 ] = Eigen::VectorXd( 0 );

    transferTrajectory->evaluateTrajectory(
                nodeTimes, transferLegFreeParameters, transferNodeFreeParameters );
    printTransferParameterDefinition( transferLegSettings, transferNodeSettings );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, transferTrajectory->getTotalDeltaV( ), 1.0E-3);
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <graphene/singularity/activity_index_calculator.hpp>
#include <graphene/singularity/ncd_aware_rank.hpp>
#include <ctime>
#include <boost/numeric/ublas/io.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <fstream>
#include <thread>

using namespace boost::numeric::ublas;
using namespace boost;
using namespace singularity;

activity_index_calculator::activity_index_calculator(parameters_t parameters) {
    this->parameters = parameters;
    p_weight_matrix = std::make_shared<matrix_t>(initial_size, initial_size);
}

void activity_index_calculator::collect_accounts(
    account_id_map_t& account_id_map,
    const std::vector<transaction_t>& transactions
) {
    std::lock_guard<std::mutex> lock(accounts_lock);
    unsigned int account_id = account_id_map.size();
    for (unsigned int i=0; i<transactions.size(); i++) {
        transaction_t transaction = transactions[i];
        account_id_map_t::iterator found_source = account_id_map.find(transaction.source_account);
        account_id_map_t::iterator found_target = account_id_map.find(transaction.target_account);
        if (found_source == account_id_map.end()) {
            account_id_map.insert(account_id_map_t::value_type(transaction.source_account, account_id++));
        }
        if (found_target == account_id_map.end()) {
            account_id_map.insert(account_id_map_t::value_type(transaction.target_account, account_id++));
        }
    }
}

void activity_index_calculator::add_block(const std::vector<transaction_t>& transactions) {
    std::vector<transaction_t> filtered_transactions = filter_block(transactions);
    std::lock_guard<std::mutex> lock(weight_matrix_lock);
    collect_accounts(account_map, filtered_transactions);

    total_handled_blocks_count++;
    handled_blocks_count++;
    if (handled_blocks_count >= parameters.decay_period) {
        handled_blocks_count -= parameters.decay_period;
        *p_weight_matrix *= parameters.decay_koefficient;
    }

    if (p_weight_matrix->size1() < account_map.size()) {
        matrix_t::size_type new_size = p_weight_matrix->size1();
        while (new_size < account_map.size()) {
            new_size *= 2;
        }
        p_weight_matrix = matrix_tools::resize(*p_weight_matrix, new_size, new_size);
    }

    update_weight_matrix(*p_weight_matrix, account_map, filtered_transactions);
}

std::vector<transaction_t> singularity::activity_index_calculator::filter_block(const std::vector<transaction_t>& block)
{
    std::vector<transaction_t> filtered_block;

    for (auto transaction: block) {
        if (check_transaction(transaction)) {
            filtered_block.push_back(transaction);
        }
    }

    return filtered_block;
}

void activity_index_calculator::skip_blocks(unsigned int blocks_count)
{
    std::lock_guard<std::mutex> lock(weight_matrix_lock);
    total_handled_blocks_count += blocks_count;
    handled_blocks_count += blocks_count;

    if (handled_blocks_count >= parameters.decay_period) {
        unsigned int decay_period_count = handled_blocks_count / parameters.decay_period;
        handled_blocks_count = handled_blocks_count - decay_period_count * parameters.decay_period;
        double decay_value = std::pow(parameters.decay_koefficient, decay_period_count);
        *p_weight_matrix *= decay_value;
    }
}

account_activity_index_map_t activity_index_calculator::calculate()
{
    if (account_map.size() == 0) {
        return account_activity_index_map_t();
    }
    ncd_aware_rank nar(parameters);
    matrix_t outlink_matrix(account_map.size(), account_map.size());
    calculate_outlink_matrix(outlink_matrix, *p_weight_matrix);
    std::shared_ptr<vector_t> rank = nar.process(outlink_matrix);

    return calculate_score(account_map, *rank);
}

bool activity_index_calculator::check_account( account_t account )
{
    if (account.amount < parameters.account_amount_threshold * parameters.token_usd_rate) {
        return false;
    }

    return true;
}

bool activity_index_calculator::check_transaction( transaction_t transaction)
{
    if (transaction.amount < parameters.transaction_amount_threshold * parameters.token_usd_rate) {
        return false;
    }

    if (transaction.source_account_balance < parameters.account_amount_threshold * parameters.token_usd_rate) {
        return false;
    }

    if (transaction.target_account_balance < parameters.account_amount_threshold * parameters.token_usd_rate) {
        return false;
    }

    return true;
}

void activity_index_calculator::calculate_outlink_matrix(
    matrix_t& o,
    matrix_t& weight_matrix
)
{
    matrix_t::size_type size = o.size1();
    {
        std::lock_guard<std::mutex> lock(weight_matrix_lock);

        for (matrix_t::iterator1 i = weight_matrix.begin1(); i != weight_matrix.end1(); i++)
        {
            if (i.index1() >= size) {
                break;
            }
            for (matrix_t::iterator2 j = i.begin(); j != i.end(); j++)
            {
                if (j.index2() >= size) {
                    break;
                }
                o(j.index1(), j.index2()) -= *j;
                o(j.index2(), j.index1()) += *j;
            }
        }
    }

    for (matrix_t::iterator1 i = o.begin1(); i != o.end1(); i++)
    {
        for (matrix_t::iterator2 j = i.begin(); j != i.end(); j++)
        {
            if (*j < 0) {
                *j = 0;
            }
        }
    }

    matrix_tools::normalize_columns(o);
}

void activity_index_calculator::update_weight_matrix(matrix_t& weight_matrix, account_id_map_t& account_id_map, const std::vector<transaction_t>& transactions) {
    for (unsigned int i=0; i<transactions.size(); i++) {
        transaction_t t = transactions[i];
        weight_matrix(account_id_map[t.source_account], account_id_map[t.target_account]) += t.amount;
    }
}

account_activity_index_map_t activity_index_calculator::calculate_score(
        const account_id_map_t& account_id_map,
        const vector_t& rank
)
{
    account_activity_index_map_t v;

    for (auto i: account_id_map) {
        v[i.first] = rank[i.second];
    }

    return v;
}

void activity_index_calculator::save_state_to_file(std::string filename)
{
    std::lock_guard<std::mutex> lock(weight_matrix_lock);
    try {
        std::ofstream ofs(filename, std::ofstream::out);

        boost::archive::binary_oarchive oarch(ofs);
        oarch << BOOST_SERIALIZATION_NVP(*this);
    } catch (std::ifstream::failure& e) {
        throw runtime_exception("Failed writing to a file " + filename);
    } catch ( boost::archive::archive_exception& e) {
        throw runtime_exception("Failed serialization to a file " + filename);
    }
}

void activity_index_calculator::load_state_from_file(std::string filename)
{
    std::lock_guard<std::mutex> lock(weight_matrix_lock);
    try {
        std::ifstream ifs(filename, std::istream::in);
        boost::archive::binary_iarchive iarch(ifs);

        iarch >> *this;
    } catch (std::ifstream::failure& e) {
        throw runtime_exception("Failed reading from a file " + filename);
    } catch ( boost::archive::archive_exception& e) {
        throw runtime_exception("Failed deserialization from a file " + filename);
    }
}

unsigned int activity_index_calculator::get_total_handled_block_count()
{
    return total_handled_blocks_count;
}

singularity::parameters_t singularity::activity_index_calculator::get_parameters()
{
    return parameters;
}

void singularity::activity_index_calculator::set_parameters(singularity::parameters_t params)
{
    parameters = params;
}


// Distributed under the BSD License, see accompanying LICENSE.txt
// (C) Copyright 2010 John-John Tedro et al.
#ifndef COLOR_HPP
#define COLOR_HPP

#include <stdint.h>
#include <assert.h>
#include <math.h>

#include <string>
#include <sstream>
#include <ostream>

#include <boost/lexical_cast.hpp>

extern float color_i_to_f[0x100];

struct color{
  float r;
  float g;
  float b;
  float a;
  int32_t z;

  color(color *c)
    : r(c->r),
      g(c->g),
      b(c->b),
      a(c->a),
      z(0)
  {
  }

  color()
    : r(1.0f),
      g(1.0f),
      b(1.0f),
      a(0.0f),
      z(0)
  {
  }

  color(uint8_t r, uint8_t g, uint8_t b, uint8_t a)
    : r(color_i_to_f[r]),
      g(color_i_to_f[g]),
      b(color_i_to_f[b]),
      a(color_i_to_f[a]),
      z(0)
  {
  }

  color(int r, int g, int b, int a)
    : r(color_i_to_f[uint8_t(r)]),
      g(color_i_to_f[uint8_t(g)]),
      b(color_i_to_f[uint8_t(b)]),
      a(color_i_to_f[uint8_t(a)]),
      z(0)
  {
  }

  color(float r, float g, float b, float a)
    : r(r),
      g(g),
      b(b),
      a(a),
      z(0)
  {
  }

  bool is_opaque() const {
    return a == 1.0f;
  }

  bool is_transparent() const {
    return a != 1.0f;
  }

  bool is_invisible() const {
    return a == 0.0f;
  }

  ~color(){
  }

  void darken(float c);
  void darken(int c);
  void lighten(float c);
  void lighten(int c);
  void blend(const color &other);

  inline void read(color *buf) {
    r = buf->r;
    g = buf->g;
    b = buf->b;
    a = buf->a;
  }

  inline void write(color *buf) {
    buf->r = r;
    buf->g = g;
    buf->b = b;
    buf->a = a;
  }

  friend std::ostream& operator<<(std::ostream& out, const color& c) // output
  {
    std::stringstream ss;
    ss << (int)roundf(c.r*255.0f) << "," << (int)roundf(c.g*255.0f) << "," << (int)roundf(c.b*255.0f) << "," << (int)roundf(c.a*255.0f);
    out << ss.str();
    return out;
  }

  bool operator!=(const color &c) const
  {
    return !(r == c.r && g == c.g && b == c.b && a == c.a);
  }

  bool operator==(const color &c) const
  {
    return r == c.r && g == c.g && b == c.b && a == c.a;
  }
};

#endif /* COLOR_HPP */


#include <benchmark/benchmark.h>

#include <thread>
#include <numeric>
#include <atomic>
#include <boost/format.hpp>
#include "ps/support/config.h"
#include "ps/base/frontend.h"
#include "ps/base/cards.h"
#include "ps/base/tree.h"
#include "ps/base/board_combination_iterator.h"
#include "ps/support/index_sequence.h"
#include "ps/eval/evaluator_5_card_map.h"
#include "ps/eval/evaluator_6_card_map.h"
#include "app/pretty_printer.h"
#include "ps/base/algorithm.h"
#include "ps/eval/instruction.h"
#include "ps/base/rank_hasher.h"
#include "ps/base/suit_hasher.h"
#include "ps/base/holdem_board_decl.h"
#include "ps/eval/class_cache.h"
#include "ps/eval/pass.h"
#include "ps/eval/pass_eval_hand_instr_vec.h"
#include "ps/base/rank_board_combination_iterator.h"

#include <boost/timer/timer.hpp>

#include <boost/log/trivial.hpp>

#include <Eigen/Dense>
#include <fstream>

#include "ps/support/command.h"
#include "ps/support/persistent.h"
#include "ps/eval/holdem_class_vector_cache.h"

using namespace ps;
using namespace ps::frontend;

/*
The point of this is to save the compution of mapping AA vs KK vs QQ to hard vectors,
then can re-apply the result to any matching pattern
*/
struct holdem_class_hard_from_proto
{
        using match_ty = std::tuple<holdem_class_type, rank_id, rank_id>;
        using rank_mapping_ty = std::array<int, rank_decl::max_id>;

        static std::string pattern_to_string(std::vector<match_ty> const& pattern)
        {
                std::stringstream ss;
                ss << "<";
                std::string sep = "";
                for(auto const& m : pattern)
                {
                        switch (std::get<0>(m))
                        {
                        case holdem_class_type::pocket_pair:
                                ss << "(pp," << rank_decl::get(std::get<1>(m)).to_string() << ")" << sep;
                                break;
                        case holdem_class_type::suited:
                                ss << "(suited," << rank_decl::get(std::get<1>(m)).to_string()
                                        << "," << rank_decl::get(std::get<2>(m)).to_string() << ")" << sep;
                                break;
                        case holdem_class_type::offsuit:
                                ss << "(offsuit," << rank_decl::get(std::get<1>(m)).to_string()
                                        << "," << rank_decl::get(std::get<2>(m)).to_string() << ")" << sep;
                        }
                        sep = ",";
                }
                ss << ">";
                return ss.str();
        }

        struct prototype
        {
                prototype(
                        std::vector< holdem_hand_vector > const& hv,
                        std::vector< matrix_t > const& transform)
                        : hv_(hv), transform_(transform)
                {}
                void emit(
                        holdem_class_vector const& cv,
                        rank_mapping_ty const& mapping,
                        std::vector< holdem_hand_vector >& out,
                        std::vector<matrix_t>& transform)const
                {
                        holdem_hand_vector working_hv;
                        for (size_t idx = 0; idx != hv_.size(); ++idx)
                        {
                                working_hv.clear();
                                for (holdem_id hid : hv_[idx])
                                {
                                        auto const& hand = holdem_hand_decl::get(hid);

                                        auto mapped_rank_first = mapping[hand.first().rank()];
                                        auto mapped_rank_second = mapping[hand.second().rank()];

                                        auto mapped_first = card_decl::make_id(
                                                hand.first().suit(),
                                                mapped_rank_first);

                                        auto mapped_second = card_decl::make_id(
                                                hand.second().suit(),
                                                mapped_rank_second);

                                        auto mapped_hid = holdem_hand_decl::make_id(mapped_first, mapped_second);

                                        working_hv.push_back(mapped_hid);

                                }

                                out.push_back(working_hv);
                                transform.push_back(transform_[idx]);
                        }
                }
                std::vector< holdem_hand_vector > hv_;
                std::vector< matrix_t > transform_;
        };
        void populate(holdem_class_vector const& cv, std::vector< holdem_hand_vector >& out, std::vector<matrix_t>& transform)
        {
                auto p = make_pattern(cv);

                auto const& pattern = std::get<0>(p);

                //std::cout << cv << " => " << pattern_to_string(pattern) << "\n";

                // mapping is cv rank to standard rank
                auto const& mapping = std::get<1>(p);
                auto const& inv_mapping = std::get<2>(p);

                auto iter = prototype_map_.find(pattern);

                if (iter == prototype_map_.end())
                {
                        //std::cout << "MAKING\n";

                        std::vector<frontend::range> players;
                        for (auto const& cid : cv)
                        {
                                auto const& decl = holdem_class_decl::get(cid);

                                auto mapped_cid = holdem_class_decl::make_id(
                                        decl.category(),
                                        mapping[decl.first()],
                                        mapping[decl.second()]);

                                players.push_back(frontend::range(holdem_class_decl::get(mapped_cid).to_string()));
                        }
                        auto instr_list = frontend_to_instruction_list("Group0", players);

                        computation_pass_manager mgr;
                        mgr.add_pass<pass_permutate_class>();
                        mgr.add_pass<pass_collect_class>();
                        mgr.add_pass<pass_class2cards>();
                        mgr.add_pass<pass_permutate>();
                        mgr.add_pass<pass_sort_type>();
                        mgr.add_pass<pass_collect>();

                        const size_t common_size = cv.size();
                        const int verboseicity = 0;
                        auto comp_ctx = std::make_shared< computation_context>(common_size, verboseicity);
                        mgr.execute_(comp_ctx.get(), &instr_list, nullptr);

                        std::vector<holdem_hand_vector> agg_hv;
                        std::vector<matrix_t> agg_trans;

                        for (auto const& untyped_instr : instr_list)
                        {
                                auto instr = reinterpret_cast<const card_eval_instruction*>(untyped_instr.get());

                                agg_hv.push_back(instr->get_vector());
                                agg_trans.push_back(instr->result_desc().back().transform());

                        }

                        prototype_map_[pattern] = std::make_shared<prototype>(agg_hv, agg_trans);
                }
                return prototype_map_.find(pattern)->second->emit(cv, inv_mapping, out, transform);
        }

        std::tuple<std::vector<match_ty>,rank_mapping_ty,rank_mapping_ty > make_pattern(holdem_class_vector const& cv)const
        {
                rank_mapping_ty rank_mapping;
                rank_mapping.fill(-1);
                rank_id rank_alloc(rank_decl::max_id);
                auto map_rank = [&rank_mapping,&rank_alloc](rank_id rid)mutable->rank_id
                {
                        if(rank_mapping[rid] == -1)
                        {
                                --rank_alloc;
                                const rank_id mapped_rank = rank_alloc;
                                rank_mapping[rid] = mapped_rank;
                                return mapped_rank;
                        }
                        else
                        {
                                return rank_mapping[rid];
                        }
                };

                std::vector<match_ty> pattern;
                for(holdem_class_id cid : cv)
                {
                        holdem_class_decl const& decl = holdem_class_decl::get(cid);

                        auto mapped_first_rank = map_rank(decl.first());
                        auto mapped_second_rank = map_rank(decl.second());
                        pattern.emplace_back(
                                decl.category(),
                                mapped_first_rank,
                                mapped_second_rank);
                }

                rank_mapping_ty inv_rank_mapping;
                inv_rank_mapping.fill(-1);
                for (rank_id iter = 0; iter != rank_decl::max_id; ++iter)
                {
                        if( rank_mapping[iter] != -1 )
                                inv_rank_mapping[rank_mapping[iter]] = iter;
                }

                //std::cout << "rank_mapping = " << std_vector_to_string(rank_mapping) << "\n";
                //std::cout << "inv_rank_mapping = " << std_vector_to_string(inv_rank_mapping) << "\n";
                return std::make_tuple(pattern,rank_mapping,inv_rank_mapping);

        }
private:
        std::map<
                std::vector<match_ty>,
                std::shared_ptr<prototype>
        > prototype_map_;
};

static void BM_ClassHands(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AA,KK,QQ");

        for (auto _ : state)
        {
                for( auto hv : cv.get_hand_vectors()){
                        benchmark::DoNotOptimize(hv);
                }
        }
}
BENCHMARK(BM_ClassHands);

static void BM_ClassHands_MakeProto_PP(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AA,KK,QQ");

        std::vector<holdem_hand_vector> out;
        std::vector<matrix_t> trans;

        for (auto _ : state)
        {
                holdem_class_hard_from_proto proto;
                out.clear();
                trans.clear();
                proto.populate(cv, out, trans);
        }
}
BENCHMARK(BM_ClassHands_MakeProto_PP);

static void BM_ClassHands_FromProto_PP(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AA,KK,QQ");

        std::vector<holdem_hand_vector> out;
        std::vector<matrix_t> trans;

        holdem_class_hard_from_proto proto;
        out.clear();
        trans.clear();
        proto.populate(cv, out, trans);

        for (auto _ : state)
        {
                out.clear();
                trans.clear();
                proto.populate(cv, out, trans);
        }
}
BENCHMARK(BM_ClassHands_FromProto_PP);

static void BM_ClassHands_MakeProto_ThreeOffsuit(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AKo,QJo,T9o");

        std::vector<holdem_hand_vector> out;
        std::vector<matrix_t> trans;

        for (auto _ : state)
        {
                holdem_class_hard_from_proto proto;
                out.clear();
                trans.clear();
                proto.populate(cv, out, trans);
        }
}
BENCHMARK(BM_ClassHands_MakeProto_ThreeOffsuit);

static void BM_ClassHands_FromProto_ThreeOffsuit(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AKo,QJo,T9o");

        std::vector<holdem_hand_vector> out;
        std::vector<matrix_t> trans;

        holdem_class_hard_from_proto proto;
        out.clear();
        trans.clear();
        proto.populate(cv, out, trans);

        for (auto _ : state)
        {
                out.clear();
                trans.clear();
                proto.populate(cv, out, trans);
        }
}
BENCHMARK(BM_ClassHands_FromProto_ThreeOffsuit);

static void BM_ClassHands_Proto_2PP(benchmark::State& state)
{
        const auto cv = holdem_class_vector::parse("AA,KK");

        holdem_class_hard_from_proto proto;

        std::vector<holdem_hand_vector> out;
        std::vector<matrix_t> trans;

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("AA,KK"), out, trans);
        std::cout << "-------------- first --------\n";
        for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("AA,KK"), out, trans);
        std::cout << "-------------- second --------\n";
         for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("KK,AA"), out, trans);
        std::cout << "-------------- second --------\n";
         for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("99,22"), out, trans);
        std::cout << "-------------- second --------\n";
         for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("AKo,QTo,98o"), out, trans);
        std::cout << "-------------- second --------\n";
         for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

         out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("A2o,QTo,98o"), out, trans);
        std::cout << "-------------- second --------\n";
         for (size_t idx=0;idx!=out.size();++idx)
        {
                std::cout << out[idx] << " - " << matrix_to_string(trans[idx]) << "\n";
        }

        std::exit(0);
        return;

        out.clear();
        trans.clear();
        proto.populate(holdem_class_vector::parse("AA,JJ"), out, trans);
        std::cout << "-------------- third --------\n";
        for (auto const& cv : out)
        {
                std::cout << cv << "\n";
        }

}

#if 0
template <class ...Args>
static void HoldemHandVector_Permutate(benchmark::State& state, Args&&... args) {

        const auto hv = holdem_hand_vector::parse(args...);

        for (auto _ : state)
        {
                auto p =  permutate_for_the_better(hv) ;
                benchmark::DoNotOptimize(p);
        }
}
BENCHMARK_CAPTURE(HoldemHandVector_Permutate, JcTc7s7dAsKd, std::string("JcTc7s7dAsKd"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(HoldemHandVector_Permutate, JcTc7s7dAdQsAsKd, std::string("JcTc7s7dAdQsAsKd"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(HoldemHandVector_Permutate, JcTc7s7dAdQsAsKd8d9d, std::string("JcTc7s7dAdQsAsKd8d9d"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(HoldemHandVector_Permutate, JcTc7s7dAdQsAsKd8d9d3c2s, std::string("JcTc7s7dAdQsAsKd8d9d3c2s"))->Unit(benchmark::kMillisecond);
#endif

#if 0

template <class ...Args>
void ClassVector_GetHandVectors(benchmark::State& state, Args&&... args) {

        const auto cv = holdem_class_vector::parse(args...);

        for (auto _ : state)
        {
                for( auto hv : cv.get_hand_vectors()){
                        benchmark::DoNotOptimize(hv);
                }
        }
}
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AA_KK, std::string("AA,KK"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AA_KK_QQ, std::string("AA,KK,QQ"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, QQ_KK_AA, std::string("QQ,KK,AA"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AA_KK_QQ_JJ, std::string("AA,KK,QQ,JJ"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AA_KK_QQ_JJ_TT, std::string("AA,KK,QQ,JJ,TT"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AKo_QJo, std::string("AKo,QJo"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AKo_QJo_T9o, std::string("AKo,QJo,T9o"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AKo_QJo_T9o_87o, std::string("AKo,QJo,T9o,87o"))->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(ClassVector_GetHandVectors, AKo_QJo_T9o_87o_65o, std::string("AKo,QJo,T9o,87o,65o"))->Unit(benchmark::kMillisecond);

static void Iterator_BitmaskFiveCardBoard(benchmark::State& state) {

        //constexpr size_t end_board = 0b1111100000000000000000000000000000000000000000000000ull;
        constexpr size_t end_board = 0b11111000000000000000000000000000ull;
        constexpr size_t end = end_board + 1;
        for (auto _ : state)
        {
                size_t counter = 0;
                for (size_t iter = 0; iter < end; ++iter)
                {
                        if (detail::popcount(iter) != 5)
                        {
                                continue;
                        }
                        ++counter;

                        benchmark::DoNotOptimize(iter);
                }
                std::cout << "counter = " << counter << "\n";

        }
}
static void Iterator_FiveCardBoard(benchmark::State& state) {

        for (auto _ : state)
        {
                for(board_combination_iterator iter(5),end;iter!=end ;++iter){
                        benchmark::DoNotOptimize(*iter);
                }
        }
}
BENCHMARK(Iterator_FiveCardBoard)->Unit(benchmark::kMillisecond);

static void Iterator_ThreePlayerClass(benchmark::State& state) {

        for (auto _ : state)
        {
                for(holdem_class_iterator iter(3),end;iter!=end;++iter){
                        benchmark::DoNotOptimize(*iter);
                }
        }
}
BENCHMARK(Iterator_ThreePlayerClass)->Unit(benchmark::kMillisecond);

static void Vs_ClassRange_First1000(benchmark::State& state, size_t n) {

        for (auto _ : state)
        {
                size_t counter = 0;
                for(holdem_class_iterator iter(n),end;iter!=end;++iter,++counter){
                        auto const& cv = *iter;
                        for( auto hv : cv.get_hand_vectors()){
                                benchmark::DoNotOptimize(hv);
                        }
                        if( counter == 1000 )
                                break;
                }
        }
}
BENCHMARK_CAPTURE(Vs_ClassRange_First1000, Two, 2)->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(Vs_ClassRange_First1000, Three, 3)->Unit(benchmark::kMillisecond);
BENCHMARK_CAPTURE(Vs_ClassRange_First1000, Four, 4)->Unit(benchmark::kMillisecond);

#endif

BENCHMARK_MAIN();


#include "abstract_histogram.hpp"

#include <algorithm>
#include <cmath>
#include <limits>
#include <map>
#include <memory>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/container_hash/hash.hpp>
#include <boost/lexical_cast.hpp>

#include "expression/evaluation/like_matcher.hpp"
#include "generic_histogram.hpp"
#include "generic_histogram_builder.hpp"
#include "lossy_cast.hpp"
#include "resolve_type.hpp"
#include "statistics/statistics_objects/abstract_statistics_object.hpp"
#include "storage/create_iterable_from_segment.hpp"
#include "storage/segment_iterate.hpp"

namespace opossum {

template <typename T>
AbstractHistogram<T>::AbstractHistogram(const HistogramDomain<T>& domain)
    : AbstractStatisticsObject(data_type_from_type<T>()), _domain(domain) {}

template <typename T>
const HistogramDomain<T>& AbstractHistogram<T>::domain() const {
  return _domain;
}

template <typename T>
std::string AbstractHistogram<T>::description() const {
  std::stringstream stream;

  stream << name();
  stream << " value count: " << total_count() << ";";
  stream << " distinct count: " << total_distinct_count() << ";";
  stream << " bin count: " << bin_count() << ";";

  stream << "  Bins" << std::endl;
  for (BinID bin_id = 0u; bin_id < bin_count(); ++bin_id) {
    if constexpr (std::is_same_v<T, pmr_string>) {
      stream << "  ['" << bin_minimum(bin_id) << "' (" << _domain.string_to_number(bin_minimum(bin_id)) << ") -> '";
      stream << bin_maximum(bin_id) << "' (" << _domain.string_to_number(bin_maximum(bin_id)) << ")]: ";
    } else {
      stream << "  [" << bin_minimum(bin_id) << " -> " << bin_maximum(bin_id) << "]: ";
    }
    stream << "Height: " << bin_height(bin_id) << "; DistinctCount: " << bin_distinct_count(bin_id) << std::endl;
  }

  return stream.str();
}

template <typename T>
HistogramBin<T> AbstractHistogram<T>::bin(const BinID index) const {
  return {bin_minimum(index), bin_maximum(index), bin_height(index), bin_distinct_count(index)};
}

template <typename T>
bool AbstractHistogram<T>::bin_contains(const BinID index, const T& value) const {
  return bin_minimum(index) <= value && value <= bin_maximum(index);
}

template <typename T>
typename AbstractHistogram<T>::HistogramWidthType AbstractHistogram<T>::bin_width(const BinID index) const {
  DebugAssert(index < bin_count(), "Index is not a valid bin.");

  // The width of an float bin [5.0, 7.0] is 2, as one would expect, but the width of an integer bin [5, 7] is 3.
  // An integer (e.g. 3) is thought to have a width of 1, whereas the floating point number 3.0 is thought to have no
  // width.
  // This makes our math more correct in multiple places, e.g., for a bin [0, 3] with a width of 4, the ratio of
  // values smaller than 2 is (2 - 0) / (width=4) -> 0.5: 0 and 1 are smaller, 2 and 3 are larger.
  // For a float bin [0.0, 3.0] it is obvious that only two-thirds of the bin are smaller than 2:
  // 2 - 0 / (width=3) -> 0.6666

  if constexpr (std::is_same_v<T, pmr_string>) {
    const auto repr_min = _domain.string_to_number(bin_minimum(index));
    const auto repr_max = _domain.string_to_number(bin_maximum(index));
    return repr_max - repr_min + 1u;
  } else if constexpr (std::is_floating_point_v<T>) {
    return bin_maximum(index) - bin_minimum(index);
  } else {
    // The width of a integral-histogram bin [4,5] is 5 - 4 + 1 = 2
    return bin_maximum(index) - bin_minimum(index) + 1;
  }
}

template <typename T>
float AbstractHistogram<T>::bin_ratio_less_than(const BinID bin_id, const T& value) const {
  if (value <= bin_minimum(bin_id)) {
    return 0.0f;
  }

  if (value > bin_maximum(bin_id)) {
    return 1.0f;
  }

  if constexpr (!std::is_same_v<T, pmr_string>) {
    return (static_cast<float>(value) - static_cast<float>(bin_minimum(bin_id))) /
           static_cast<float>(bin_width(bin_id));
  } else {
    /*
    * We need to convert strings to their numerical representation to calculate a share.
    * This conversion is done based on prefixes because strings of arbitrary length cannot be converted to a numerical
    * representation that satisfies the following requirements:
    *  1. For two strings s1 and s2: s1 < s2 -> repr(s1) < repr(s2)
    *  2. For two strings s1 and s2: dist(s1, s2) == repr(s2) - repr(s1)
    *  repr(s) is the numerical representation for a string s, and dist(s1, s2) returns the number of strings between
    *  s1 and s2 in the domain of strings with at most length `_domain.prefix_length` and the set of supported
    * characters in the `_domain`.
    *
    * Thus, we calculate the range based only on a domain of strings with a maximum length of `_domain.prefix_length`
    * characters. To improve estimations where strings exceed that length, but share a common prefix, we strip that
    * common prefix and take the substring starting after that prefix.
    *
    * Example:
    *  - bin: ["intelligence", "intellij"]
    *  - _domain.supported_characters: [a-z]
    *  - _domain.prefix_length: 4
    *  - value: intelligent
    *
    *  Traditionally, if we did not strip the common prefix, we would calculate the range based on the
    *  substring of length `_domain.prefix_length`, which is "inte" for both lower and upper edge of the bin.
    *  We could not make a reasonable assumption how large the share is.
    *  Instead, we strip the common prefix ("intelli") and calculate the share based on the numerical representation
    *  of the substring after the common prefix.
    *  That is, what is the share of values smaller than "gent" in the range ["gence", "j"]?
    */

    const auto bin_min = bin_minimum(bin_id);
    const auto bin_max = bin_maximum(bin_id);

    // Determine the common_prefix_lengths of bin_min and bin_max. E.g. bin_max=abcde and bin_max=abcz have a
    // common_prefix_length=3
    auto common_prefix_length = size_t{0};
    const auto max_common_prefix_length =
        std::min(bin_min.length() - _domain.prefix_length, bin_max.length() - _domain.prefix_length);
    for (; common_prefix_length < max_common_prefix_length; ++common_prefix_length) {
      if (bin_min[common_prefix_length] != bin_max[common_prefix_length]) {
        break;
      }
    }

    const auto in_domain_value = _domain.string_to_domain(value.substr(common_prefix_length));
    const auto value_repr = _domain.string_to_number(in_domain_value);
    const auto min_repr = _domain.string_to_number(bin_min.substr(common_prefix_length));
    const auto max_repr = _domain.string_to_number(bin_max.substr(common_prefix_length));
    const auto bin_ratio = static_cast<float>(value_repr - min_repr) / static_cast<float>(max_repr - min_repr + 1);

    return bin_ratio;
  }
}

template <typename T>
float AbstractHistogram<T>::bin_ratio_less_than_equals(const BinID bin_id, const T& value) const {
  if (value < bin_minimum(bin_id)) {
    return 0.0f;
  }

  if (value >= bin_maximum(bin_id)) {
    return 1.0f;
  }

  if constexpr (!std::is_same_v<T, pmr_string>) {
    return bin_ratio_less_than(bin_id, _domain.next_value_clamped(value));
  } else {
    // See bin_ratio_less_than for comment.
    const auto bin_min = bin_minimum(bin_id);
    const auto bin_max = bin_maximum(bin_id);

    auto common_prefix_length = size_t{0};
    const auto max_common_prefix_length =
        std::min(bin_min.length() - _domain.prefix_length, bin_max.length() - _domain.prefix_length);
    for (; common_prefix_length < max_common_prefix_length; ++common_prefix_length) {
      if (bin_min[common_prefix_length] != bin_max[common_prefix_length]) {
        break;
      }
    }

    const auto in_domain_value = _domain.string_to_domain(value.substr(common_prefix_length));
    const auto value_repr = _domain.string_to_number(in_domain_value) + 1;
    const auto min_repr = _domain.string_to_number(bin_min.substr(common_prefix_length));
    const auto max_repr = _domain.string_to_number(bin_max.substr(common_prefix_length));
    const auto bin_ratio = static_cast<float>(value_repr - min_repr) / static_cast<float>(max_repr - min_repr + 1);

    return bin_ratio;
  }
}

template <typename T>
float AbstractHistogram<T>::bin_ratio_between(const BinID bin_id, const T& value, const T& value2) const {
  // All values that are less than or equal to the upper boundary minus all values that are smaller than the lower
  // boundary
  return bin_ratio_less_than_equals(bin_id, value2) - bin_ratio_less_than(bin_id, value);
}

template <typename T>
bool AbstractHistogram<T>::does_not_contain(const PredicateCondition predicate_condition,
                                            const AllTypeVariant& variant_value,
                                            const std::optional<AllTypeVariant>& variant_value2) const {
  if (bin_count() == 0) {
    return true;
  }

  const auto value = lossy_variant_cast<T>(variant_value);
  if (!value) {
    return false;
  }

  switch (predicate_condition) {
    case PredicateCondition::Equals: {
      const auto bin_id = _bin_for_value(*value);
      // It is possible for EqualWidthHistograms to have empty bins.
      return bin_id == INVALID_BIN_ID || bin_height(bin_id) == 0ul;
    }
    case PredicateCondition::NotEquals:
      return bin_minimum(BinID{0}) == value && bin_maximum(bin_count() - 1) == value;
    case PredicateCondition::LessThan:
      return value <= bin_minimum(BinID{0});
    case PredicateCondition::LessThanEquals:
      return value < bin_minimum(BinID{0});
    case PredicateCondition::GreaterThanEquals:
      return value > bin_maximum(bin_count() - 1);
    case PredicateCondition::GreaterThan:
      return value >= bin_maximum(bin_count() - 1);

    // For simplicity we do not distinguish between the different Between* types.
    case PredicateCondition::BetweenInclusive:
    case PredicateCondition::BetweenLowerExclusive:
    case PredicateCondition::BetweenUpperExclusive:
    case PredicateCondition::BetweenExclusive: {
      Assert(static_cast<bool>(variant_value2), "Between operator needs two values.");

      if (does_not_contain(PredicateCondition::GreaterThanEquals, *value)) {
        return true;
      }

      const auto value2 = lossy_variant_cast<T>(*variant_value2);
      if (!value2) {
        return false;
      }
      if (does_not_contain(PredicateCondition::LessThanEquals, *value2) || value2 < value) {
        return true;
      }

      const auto value_bin = _bin_for_value(*value);
      const auto value2_bin = _bin_for_value(*value2);

      // In an EqualDistinctCountHistogram, if both values fall into the same gap, we can prune the predicate.
      // We need to have at least two bins to rule out pruning if value < min and value2 > max.
      if (value_bin == INVALID_BIN_ID && value2_bin == INVALID_BIN_ID && bin_count() > 1ul &&
          _next_bin_for_value(*value) == _next_bin_for_value(*value2)) {
        return true;
      }

      // In an EqualWidthHistogram, if both values fall into a bin that has no elements,
      // and there are either no bins in between or none of them have any elements, we can also prune the predicate.
      if (value_bin != INVALID_BIN_ID && value2_bin != INVALID_BIN_ID && bin_height(value_bin) == 0 &&
          bin_height(value2_bin) == 0) {
        for (auto current_bin = value_bin + 1; current_bin < value2_bin; ++current_bin) {
          if (bin_height(current_bin) > 0ul) {
            return false;
          }
        }
        return true;
      }

      return false;
    }
    case PredicateCondition::Like:
    case PredicateCondition::NotLike:
      if constexpr (std::is_same_v<pmr_string, T>) {
        // Generally, (NOT) LIKE is to hard to perform estimations on. Some special patterns could be estimated,
        // e.g. `LIKE 'a'` is the same as `= 'a'` or `LIKE 'a%'` is the same as `>= a AND < b`, but we leave it to other
        // components (e.g., the optimizer) to perform these transformations.
        // Thus, (NOT) LIKE is not estimated and we cannot determine whether does_not_contain() could be true.
        return false;
      } else {
        Fail("Predicate NOT LIKE is not supported for non-string columns.");
      }

    default:
      // Do not prune predicates we cannot (yet) handle.
      return false;
  }
}

template <typename T>
std::pair<Cardinality, DistinctCount> AbstractHistogram<T>::estimate_cardinality_and_distinct_count(
    const PredicateCondition predicate_condition, const AllTypeVariant& variant_value,
    const std::optional<AllTypeVariant>& variant_value2) const {
  auto value = lossy_variant_cast<T>(variant_value);
  if (!value) {
    return {static_cast<Cardinality>(total_count()), static_cast<float>(total_distinct_count())};
  }

  // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
  if constexpr (std::is_same_v<T, pmr_string>) {
    value = _domain.string_to_domain(*value);
  }

  if (does_not_contain(predicate_condition, variant_value, variant_value2)) {
    return {Cardinality{0}, 0.0f};
  }

  switch (predicate_condition) {
    case PredicateCondition::Equals: {
      const auto bin_id = _bin_for_value(*value);
      const auto bin_distinct_count = this->bin_distinct_count(bin_id);

      if (bin_distinct_count == 0) {
        return {Cardinality{0.0f}, 0.0f};
      } else {
        const auto cardinality = Cardinality{bin_height(bin_id) / bin_distinct_count};
        return {cardinality, std::min(bin_distinct_count, HistogramCountType{1.0f})};
      }
    }

    case PredicateCondition::NotEquals:
      return _invert_estimate(estimate_cardinality_and_distinct_count(PredicateCondition::Equals, variant_value));

    case PredicateCondition::LessThan: {
      if (*value > bin_maximum(bin_count() - 1)) {
        return {static_cast<Cardinality>(total_count()), static_cast<float>(total_distinct_count())};
      }

      // This should never be false because does_not_contain should have been true further up if this was the case.
      DebugAssert(*value >= bin_minimum(BinID{0}), "Value smaller than min of histogram.");

      auto cardinality = Cardinality{0};
      auto distinct_count = 0.f;
      auto bin_id = _bin_for_value(*value);

      if (bin_id == INVALID_BIN_ID) {
        // The value is within the range of the histogram, but does not belong to a bin.
        // Therefore, we need to sum up the counts of all bins with a max < value.
        bin_id = _next_bin_for_value(*value);
      } else if (value == bin_minimum(bin_id) || bin_height(bin_id) == 0u) {
        // If the value is exactly the lower bin edge or the bin is empty,
        // we do not have to add anything of that bin and know the cardinality exactly.
      } else {
        const auto share = bin_ratio_less_than(bin_id, *value);
        cardinality += share * bin_height(bin_id);
        distinct_count += share * bin_distinct_count(bin_id);
      }

      DebugAssert(bin_id != INVALID_BIN_ID, "Should have been caught by does_not_contain().");

      // Sum up all bins before the bin (or gap) containing the value.
      for (BinID bin = 0u; bin < bin_id; ++bin) {
        cardinality += bin_height(bin);
        distinct_count += bin_distinct_count(bin);
      }

      /**
       * The cardinality is capped at total_count().
       * It is possible for a value that is smaller than or equal to the max of the EqualHeightHistogram
       * to yield a calculated cardinality higher than total_count.
       * This is due to the way EqualHeightHistograms store the count for a bin,
       * which is in a single value (count_per_bin) for all bins rather than a vector (one value for each bin).
       * Consequently, this value is the desired count for all bins.
       * In practice, _bin_count(n) >= _count_per_bin for n < bin_count() - 1,
       * because bins are filled up until the count is at least _count_per_bin.
       * The last bin typically has a count lower than _count_per_bin.
       * Therefore, if we calculate the share of the last bin based on _count_per_bin
       * we might end up with an estimate higher than total_count(), which is then capped.
       */
      return {std::min(cardinality, static_cast<Cardinality>(total_count())), distinct_count};
    }
    case PredicateCondition::LessThanEquals:
      return estimate_cardinality_and_distinct_count(PredicateCondition::LessThan, _domain.next_value_clamped(*value));

    case PredicateCondition::GreaterThanEquals:
      return _invert_estimate(estimate_cardinality_and_distinct_count(PredicateCondition::LessThan, variant_value));

    case PredicateCondition::GreaterThan:
      return _invert_estimate(
          estimate_cardinality_and_distinct_count(PredicateCondition::LessThanEquals, variant_value));

    case PredicateCondition::BetweenInclusive:
    case PredicateCondition::BetweenLowerExclusive:
    case PredicateCondition::BetweenUpperExclusive:
    case PredicateCondition::BetweenExclusive: {
      Assert(static_cast<bool>(variant_value2), "Between operator needs two values.");
      const auto value2 = lossy_variant_cast<T>(*variant_value2);
      if (!value2) {
        return {total_count(), total_distinct_count()};
      }

      if (*value2 < *value) {
        return {Cardinality{0}, 0.0f};
      }

      // Adjust value (lower_bound) and value2 (lower_bin_id) so that both values are contained within a bin
      auto lower_bound = *value;
      auto lower_bin_id = _bin_for_value(*value);
      if (lower_bin_id == INVALID_BIN_ID) {
        lower_bin_id = _next_bin_for_value(*value);
        lower_bound = bin_minimum(lower_bin_id);
      }

      auto upper_bound = *value2;
      auto upper_bin_id = _bin_for_value(*value2);
      if (upper_bin_id == INVALID_BIN_ID) {
        upper_bin_id = _next_bin_for_value(*value2);
        if (upper_bin_id == INVALID_BIN_ID) {
          upper_bin_id = bin_count() - 1;
        } else {
          upper_bin_id = upper_bin_id - 1;
        }
        upper_bound = bin_maximum(upper_bin_id);
      }

      // Accumulate the cardinality/distinct count of all bins from the lower bound to the upper bound
      auto cardinality = HistogramCountType{0};
      auto distinct_count = HistogramCountType{0};
      for (auto bin_id = lower_bin_id; bin_id <= upper_bin_id; ++bin_id) {
        cardinality += bin_height(bin_id);
        distinct_count += bin_distinct_count(bin_id);
      }

      // Subtract the cardinality/distinct below the lower bound and above the upper bound
      const auto bin_ratio_less_than_lower_bound = bin_ratio_less_than(lower_bin_id, lower_bound);
      cardinality -= bin_height(lower_bin_id) * bin_ratio_less_than_lower_bound;
      distinct_count -= bin_distinct_count(lower_bin_id) * bin_ratio_less_than_lower_bound;

      const auto bin_ratio_greater_than_upper_bound = 1.0f - bin_ratio_less_than_equals(upper_bin_id, upper_bound);
      cardinality -= bin_height(upper_bin_id) * bin_ratio_greater_than_upper_bound;
      distinct_count -= bin_distinct_count(upper_bin_id) * bin_ratio_greater_than_upper_bound;

      return {cardinality, distinct_count};
    }

    case PredicateCondition::Like:
    case PredicateCondition::NotLike:
      if constexpr (std::is_same_v<pmr_string, T>) {
        // Generally, (NOT) LIKE is too hard to perform estimations on. Some special patterns could be estimated,
        // e.g. `LIKE 'a'` is the same as `= 'a'` or `LIKE 'a%'` is the same as `>= a AND < b`, but we leave it to other
        // components (e.g., the optimizer) to perform these transformations.
        // Thus, (NOT) LIKE is not estimated and we return a selectivity of 1.
        return {total_count(), total_distinct_count()};
      } else {
        Fail("Predicate NOT LIKE is not supported for non-string columns.");
      }

    default:
      Fail("Predicate not supported");
  }
}

template <typename T>
std::pair<Cardinality, DistinctCount> AbstractHistogram<T>::_invert_estimate(
    const std::pair<Cardinality, DistinctCount>& estimate) const {
  DebugAssert(total_count() >= estimate.first, "Estimate cannot be higher than total_count()");
  DebugAssert(total_distinct_count() >= estimate.second, "Estimate cannot be higher than total_count()");

  return {Cardinality{total_count() - estimate.first}, total_distinct_count() - estimate.second};
}

// Specialization for numbers.
template <typename T>
Cardinality AbstractHistogram<T>::estimate_cardinality(const PredicateCondition predicate_condition,
                                                       const AllTypeVariant& variant_value,
                                                       const std::optional<AllTypeVariant>& variant_value2) const {
  const auto estimate = estimate_cardinality_and_distinct_count(predicate_condition, variant_value, variant_value2);
  return estimate.first;
}

template <typename T>
std::shared_ptr<AbstractStatisticsObject> AbstractHistogram<T>::sliced(
    const PredicateCondition predicate_condition, const AllTypeVariant& variant_value,
    const std::optional<AllTypeVariant>& variant_value2) const {
  if (does_not_contain(predicate_condition, variant_value, variant_value2)) {
    return nullptr;
  }

  const auto value = lossy_variant_cast<T>(variant_value);
  DebugAssert(value, "sliced() cannot be called with NULL");

  switch (predicate_condition) {
    case PredicateCondition::Equals: {
      GenericHistogramBuilder<T> builder{1, _domain};
      builder.add_bin(*value, *value,
                      static_cast<HistogramCountType>(estimate_cardinality(PredicateCondition::Equals, variant_value)),
                      1);
      return builder.build();
    }

    case PredicateCondition::NotEquals: {
      const auto value_bin_id = _bin_for_value(*value);
      if (value_bin_id == INVALID_BIN_ID) return clone();

      auto minimum = bin_minimum(value_bin_id);
      auto maximum = bin_maximum(value_bin_id);
      const auto distinct_count = bin_distinct_count(value_bin_id);

      // Do not create empty bin if `value` is the only value in the bin
      const auto new_bin_count = minimum == maximum ? bin_count() - 1 : bin_count();

      GenericHistogramBuilder<T> builder{new_bin_count, _domain};

      builder.add_copied_bins(*this, BinID{0}, value_bin_id);

      // Do not create empty bin if `value` is the only value in the bin
      if (minimum != maximum) {
        // A bin [50, 60] sliced with `!= 60` becomes [50, 59]
        // TODO(anybody) Implement bin bounds trimming for strings
        // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
        if constexpr (!std::is_same_v<pmr_string, T>) {
          if (minimum == *value) {
            minimum = _domain.next_value_clamped(*value);
          }

          if (maximum == *value) {
            maximum = _domain.previous_value_clamped(*value);
          }
        }

        const auto estimate = estimate_cardinality_and_distinct_count(PredicateCondition::Equals, variant_value);
        const auto new_height = bin_height(value_bin_id) - estimate.first;
        const auto new_distinct_count = distinct_count - estimate.second;

        builder.add_bin(minimum, maximum, new_height, new_distinct_count);
      }

      builder.add_copied_bins(*this, value_bin_id + 1, bin_count());

      return builder.build();
    }

    case PredicateCondition::LessThanEquals:
      return sliced(PredicateCondition::LessThan, _domain.next_value_clamped(*value));

    case PredicateCondition::LessThan: {
      auto last_included_bin_id = _bin_for_value(*value);

      if (last_included_bin_id == INVALID_BIN_ID) {
        const auto next_bin_id_after_value = _next_bin_for_value(*value);

        if (next_bin_id_after_value == INVALID_BIN_ID) {
          last_included_bin_id = bin_count() - 1;
        } else {
          last_included_bin_id = next_bin_id_after_value - 1;
        }
      } else if (*value == bin_minimum(last_included_bin_id)) {
        --last_included_bin_id;
      }

      auto last_bin_maximum = T{};
      // previous_value_clamped(value) is not available for strings, but we do not expect it to make a big difference.
      // TODO(anybody) Correctly implement bin bounds trimming for strings
      // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
      if constexpr (!std::is_same_v<T, pmr_string>) {
        last_bin_maximum = std::min(bin_maximum(last_included_bin_id), _domain.previous_value_clamped(*value));
      } else {
        last_bin_maximum = std::min(bin_maximum(last_included_bin_id), *value);
      }

      GenericHistogramBuilder<T> builder{last_included_bin_id + 1, _domain};
      builder.add_copied_bins(*this, BinID{0}, last_included_bin_id);
      builder.add_sliced_bin(*this, last_included_bin_id, bin_minimum(last_included_bin_id), last_bin_maximum);

      return builder.build();
    }

    case PredicateCondition::GreaterThan:
      return sliced(PredicateCondition::GreaterThanEquals, _domain.next_value_clamped(*value));

    case PredicateCondition::GreaterThanEquals: {
      auto first_new_bin_id = _bin_for_value(*value);

      if (first_new_bin_id == INVALID_BIN_ID) {
        first_new_bin_id = _next_bin_for_value(*value);
      }

      DebugAssert(first_new_bin_id < bin_count(), "This should have been caught by does_not_contain().");

      GenericHistogramBuilder<T> builder{bin_count() - first_new_bin_id, _domain};

      builder.add_sliced_bin(*this, first_new_bin_id, std::max(*value, bin_minimum(first_new_bin_id)),
                             bin_maximum(first_new_bin_id));
      builder.add_copied_bins(*this, first_new_bin_id + 1, bin_count());

      return builder.build();
    }

    case PredicateCondition::BetweenInclusive:
    case PredicateCondition::BetweenLowerExclusive:
    case PredicateCondition::BetweenUpperExclusive:
    case PredicateCondition::BetweenExclusive:
      Assert(variant_value2, "BETWEEN needs a second value.");
      return sliced(PredicateCondition::GreaterThanEquals, variant_value)
          ->sliced(PredicateCondition::LessThanEquals, *variant_value2);

    case PredicateCondition::Like:
    case PredicateCondition::NotLike:
      // TODO(anybody) Slicing for (NOT) LIKE not supported, yet
      return clone();

    case PredicateCondition::In:
    case PredicateCondition::NotIn:
    case PredicateCondition::IsNull:
    case PredicateCondition::IsNotNull:
      Fail("PredicateCondition not supported by Histograms");
  }

  Fail("Invalid enum value");
}

template <typename T>
std::shared_ptr<AbstractStatisticsObject> AbstractHistogram<T>::pruned(
    const size_t num_values_pruned, const PredicateCondition predicate_condition, const AllTypeVariant& variant_value,
    const std::optional<AllTypeVariant>& variant_value2) const {
  const auto value = lossy_variant_cast<T>(variant_value);
  DebugAssert(value, "pruned() cannot be called with NULL");

  // For each bin, bin_prunable_height holds the number of values that do not fulfill the predicate. If we had some
  // information about the sort order of the table, we might start pruning a GreaterThan predicate with the lowest
  // value instead of uniformly pruning across all affected chunks as a future optimization.
  std::vector<HistogramCountType> bin_prunable_height(bin_count());

  switch (predicate_condition) {
    case PredicateCondition::Equals: {
      // Bins that do not contain the value are fully prunable. In the bin that contains the value, there are
      // `height / distinct_count` instances of the value that satisfies the predicate. All other values are prunable.
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        if (bin_contains(bin_id, *value)) {
          bin_prunable_height[bin_id] = bin_height(bin_id) - (bin_height(bin_id) / bin_distinct_count(bin_id));
        } else {
          bin_prunable_height[bin_id] = bin_height(bin_id);
        }
      }
    } break;

    case PredicateCondition::NotEquals: {
      // Bins that do not contain the value are NOT prunable. In the bin that contains the value, there are
      // `height / distinct_count` instances of the value that DO NOT satisfy the predicate. All other values are
      // prunable.
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        if (bin_contains(bin_id, *value)) {
          bin_prunable_height[bin_id] = bin_height(bin_id) / bin_distinct_count(bin_id);
        } else {
          bin_prunable_height[bin_id] = 0.0f;
        }
      }
    } break;

    case PredicateCondition::LessThanEquals: {
      // For bins that are completely less than/equal to the value, bin_ratio_less_than_equals is 100% and no values
      // are prunable. After that, the inverse bin_ratio_less_than_equals is prunable.
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        bin_prunable_height[bin_id] = bin_height(bin_id) * (1.0f - bin_ratio_less_than_equals(bin_id, *value));
      }
    } break;

    case PredicateCondition::LessThan: {
      // Analogous to LessThanEquals
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        bin_prunable_height[bin_id] = bin_height(bin_id) * (1.0f - bin_ratio_less_than(bin_id, *value));
      }
    } break;

    case PredicateCondition::GreaterThan: {
      // Analogous to LessThanEquals
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        bin_prunable_height[bin_id] = bin_height(bin_id) * bin_ratio_less_than_equals(bin_id, *value);
      }
    } break;

    case PredicateCondition::GreaterThanEquals: {
      // Analogous to LessThanEquals
      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        bin_prunable_height[bin_id] = bin_height(bin_id) * bin_ratio_less_than(bin_id, *value);
      }
    } break;

    case PredicateCondition::BetweenInclusive:
    case PredicateCondition::BetweenLowerExclusive:
    case PredicateCondition::BetweenUpperExclusive:
    case PredicateCondition::BetweenExclusive: {
      // Treating all between conditions the same for now. Analogous to LessThanEquals.
      DebugAssert(variant_value2, "Expected second value for between");
      const auto value2 = lossy_variant_cast<T>(*variant_value2);
      DebugAssert(value2, "pruned() cannot be called with NULL");

      for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
        bin_prunable_height[bin_id] = bin_height(bin_id) * (1.0f - bin_ratio_between(bin_id, *value, *value2));
      }
    } break;

    case PredicateCondition::Like:
    case PredicateCondition::NotLike:
      // TODO(anybody) Pruning for (NOT) LIKE not supported, yet
      return clone();

    case PredicateCondition::In:
    case PredicateCondition::NotIn:
    case PredicateCondition::IsNull:
    case PredicateCondition::IsNotNull:
      Fail("PredicateCondition not supported by Histograms");
  }

  auto total_prunable_values = HistogramCountType{0};
  for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
    total_prunable_values += bin_prunable_height[bin_id];
  }

  // Without prunable values, the histogram is out-of-date. Return an unmodified histogram to avoid DIV/0 below.
  if (total_prunable_values == HistogramCountType{0}) {
    return clone();
  }

  // For an up-to-date histogram, the pruning ratio should never exceed 100%. If, however, the histogram is outdated
  // we must make sure that no bin becomes more than empty and that we do not touch the unpruned part of the bin.
  const auto pruning_ratio = std::min(static_cast<Selectivity>(num_values_pruned) / total_prunable_values, 1.0f);

  GenericHistogramBuilder<T> builder(bin_count(), _domain);
  for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
    const auto pruned_height = bin_prunable_height[bin_id] * pruning_ratio;
    const auto new_bin_height = bin_height(bin_id) - pruned_height;
    if (new_bin_height <= std::numeric_limits<float>::epsilon()) continue;

    builder.add_bin(bin_minimum(bin_id), bin_maximum(bin_id), new_bin_height, bin_distinct_count(bin_id));
  }

  return builder.build();
}

template <typename T>
std::shared_ptr<AbstractStatisticsObject> AbstractHistogram<T>::scaled(const Selectivity selectivity) const {
  Assert(!std::isnan(selectivity), "Unexpected selectivity.");
  GenericHistogramBuilder<T> builder(bin_count(), _domain);

  // Scale the number of values in the bin with the given selectivity.
  for (auto bin_id = BinID{0}; bin_id < bin_count(); ++bin_id) {
    builder.add_bin(bin_minimum(bin_id), bin_maximum(bin_id), bin_height(bin_id) * selectivity,
                    _scale_distinct_count(bin_height(bin_id), bin_distinct_count(bin_id), selectivity));
  }

  return builder.build();
}

template <typename T>
std::shared_ptr<AbstractHistogram<T>> AbstractHistogram<T>::split_at_bin_bounds(
    const std::vector<std::pair<T, T>>& additional_bin_edges) const {
  // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
  if constexpr (std::is_same_v<T, pmr_string>) {
    Fail("Cannot split_at_bin_bounds() on string histogram");
  }

  const auto input_bin_count = bin_count();

  /**
   * Collect "candidate" splits from the histogram itself and the `additional_bin_edges`.
   *
   * A split is a pair, where pair.first is the upper bound of a (potential) bin in the
   * result histogram and pair.second is the lower bound of the (potential) subsequent bin.
   *
   * E.g. if the histogram has the bins {[0, 10], [15, 20]} and additional_bin_edges is {[-4, 5], [16, 18]})
   * then candidate_split_set becomes {[-5, -4], [-1, 0], [5, 6], [10, 11], [14, 15], [15, 16], [20, 21], [18, 19]}
   */
  auto candidate_split_set = std::unordered_set<std::pair<T, T>, boost::hash<std::pair<T, T>>>{};

  for (auto bin_id = BinID{0}; bin_id < input_bin_count; ++bin_id) {
    const auto bin_min = bin_minimum(bin_id);
    const auto bin_max = bin_maximum(bin_id);
    // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
    if constexpr (std::is_arithmetic_v<T>) {
      candidate_split_set.insert(std::make_pair(_domain.previous_value_clamped(bin_min), bin_min));
      candidate_split_set.insert(std::make_pair(bin_max, _domain.next_value_clamped(bin_max)));
    }
  }

  for (const auto& edge_pair : additional_bin_edges) {
    // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
    if constexpr (std::is_arithmetic_v<T>) {
      candidate_split_set.insert(std::make_pair(_domain.previous_value_clamped(edge_pair.first), edge_pair.first));
      candidate_split_set.insert(std::make_pair(edge_pair.second, _domain.next_value_clamped(edge_pair.second)));
    }
  }

  if (candidate_split_set.empty()) {
    GenericHistogramBuilder<T> builder{0, _domain};
    return builder.build();
  }

  /**
   * Sequentialize `candidate_split_set` and sort it.
   *
   * E.g. {[-1, 0], [10, 11], [14, 15], [20, 21], [-5, -4], [5, 6], [15, 16], [18, 19]} becomes
   * [-5, -4, -1, 0, 5, 6, 10, 11, 14, 15, 15, 16, 18, 19, 20, 21]
   */
  auto candidate_edges = std::vector<T>(candidate_split_set.size() * 2);
  auto edge_idx = size_t{0};
  for (const auto& split_pair : candidate_split_set) {
    candidate_edges[edge_idx] = split_pair.first;
    candidate_edges[edge_idx + 1] = split_pair.second;
    edge_idx += 2;
  }

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

  /**
   * Remove the first and last value.
   * These are the values introduced in the first step for the first and last split.
   * The lower edge of bin 0 basically added the upper edge of bin -1,
   * and the upper edge of the last bin basically added the lower edge of the bin after the last bin.
   * Both values are obviously not needed.
   *
   * E.g., [-5, -4, -1, 0, 5, 6, 10, 11, 14, 15, 15, 16, 18, 19, 20, 21] becomes
   * [-4, -1, 0, 5, 6, 10, 11, 14, 15, 15, 16, 18, 19, 20]
   */
  candidate_edges.erase(candidate_edges.begin());
  candidate_edges.pop_back();

  /**
   * Create new bins.
   * Bin edges are defined by two consecutive values in candidate_edges in pairs of two.
   *
   * E.g. [-4, -1, 0, 5, 6, 10, 11, 14, 15, 15, 16, 18, 19, 20] becomes
   * {[-4, -1], [0, 5], [6, 10], [11, 14], [15, 15], [16, 18], [19, 20]} but since some of these bins contain not data
   * in the original histogram, the resulting histogram is {[0, 5], [6, 10], [15, 15], [16, 18], [19, 20]}
   */
  const auto result_bin_count = candidate_edges.size() / 2;
  GenericHistogramBuilder<T> builder{result_bin_count, _domain};

  for (auto bin_id = BinID{0}; bin_id < result_bin_count; ++bin_id) {
    const auto bin_min = candidate_edges[bin_id * 2];
    const auto bin_max = candidate_edges[bin_id * 2 + 1];

    const auto estimate =
        estimate_cardinality_and_distinct_count(PredicateCondition::BetweenInclusive, bin_min, bin_max);
    // Skip empty bins
    if (estimate.first == Cardinality{0}) {
      continue;
    }

    builder.add_bin(bin_min, bin_max, static_cast<HistogramCountType>(estimate.first),
                    static_cast<HistogramCountType>(estimate.second));
  }

  return builder.build();
}

template <typename T>
std::vector<std::pair<T, T>> AbstractHistogram<T>::bin_bounds() const {
  std::vector<std::pair<T, T>> bin_edges(bin_count());

  for (auto bin_id = BinID{0}; bin_id < bin_edges.size(); ++bin_id) {
    bin_edges[bin_id] = std::make_pair(bin_minimum(bin_id), bin_maximum(bin_id));
  }

  return bin_edges;
}

template <typename T>
void AbstractHistogram<T>::_assert_bin_validity() {
  for (BinID bin_id{0}; bin_id < bin_count(); ++bin_id) {
    Assert(bin_minimum(bin_id) <= bin_maximum(bin_id), "Bin minimum must be <= bin maximum.");

    if (bin_id < bin_count() - 1) {
      Assert(bin_maximum(bin_id) < bin_minimum(bin_id + 1), "Bins must be sorted and cannot overlap.");
    }

    // NOLINTNEXTLINE clang-tidy is crazy and sees a "potentially unintended semicolon" here...
    if constexpr (std::is_same_v<T, pmr_string>) {
      Assert(_domain.contains(bin_minimum(bin_id)), "Invalid string bin minimum");
      Assert(_domain.contains(bin_maximum(bin_id)), "Invalid string bin maximum");
    }
  }
}

template <typename T>
Cardinality AbstractHistogram<T>::_scale_distinct_count(Cardinality value_count, Cardinality distinct_count,
                                                        Selectivity selectivity) {
  return std::min(distinct_count, Cardinality{value_count * selectivity});
}

EXPLICITLY_INSTANTIATE_DATA_TYPES(AbstractHistogram);

}  // namespace opossum


// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>

#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"
#include "badslam/cuda_depth_processing.cuh"

using namespace vis;

// This checks that the geometric residual in bundle adjustment works correctly.
// It sets up a keyframe and a scene with surfels that match its depth image.
// Then it perturbs the keyframe's pose and tests whether optimizing the frame's
// pose yields its original pose again.
TEST(Optimization, PoseOptimizationWithGeometricResidual) {
  srand(0);

  // Initialize camera
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.5f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Set ground truth keyframe pose
  SE3f global_tr_frame = SE3f();

  // Initialize CUDA stream
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 1000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 1.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 2,
      /*min_observation_count_while_bootstrapping_2*/ 2,
      /*min_observation_count*/ 2,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ true,
      /*use_descriptor_residuals*/ false,
      nullptr,
      /*global_T_anchor_frame*/ SE3f());

  // Create a known depth map with 3 planes that (hopefully) have different normals.
  Image<u16> depth_image(width, height);
  depth_image.SetTo(numeric_limits<u16>::max());

  // Insert 3 planes
  for (int plane_index = 0; plane_index < 3; ++ plane_index) {
    Vec3f plane_normal = Vec3f::Random();
    plane_normal.z() = -1.f;
    plane_normal.normalize();
    Hyperplane<float, 3> plane(plane_normal, 2.5f);

    int max_x = width - 10 - 1;
    int min_x = 10;
    int left = min_x + (max_x - min_x) * ((2 * plane_index) / (2.0f * 3 - 1));
    int right = min_x + (max_x - min_x) * ((2 * plane_index + 1) / (2.0f * 3 - 1));

    for (int y = 10; y < height - 10; ++ y) {
      for (int x = left; x < right; ++ x) {
        Vec3f frame_direction = camera.UnprojectFromPixelCenterConv(Vec2f(x, y));
        ASSERT_EQ(frame_direction.z(), 1.0f);
        ParametrizedLine<float, 3> ray(global_tr_frame.translation(), global_tr_frame.rotationMatrix() * frame_direction);
        float z = ray.intersectionParameter(plane);
        depth_image(x, y) = z / raw_to_float_depth + 0.5f;
      }
    }
  }

//   ImageDisplay debug_display;
//   debug_display.Update(inv_depth_image, "debug: inv depth map", 0.f, 0.5f);
//   std::getchar();

  // Create a homogeneous color image.
  Image<Vec3u8> color_image(width, height);
  memset(color_image.data(), 0, color_image.stride() * color_image.height());

  shared_ptr<Keyframe> new_keyframe(new Keyframe(
      stream,
      /*frame_index*/ 0,
      direct_ba.depth_params(),
      direct_ba.depth_camera(),
      depth_image,
      color_image,
      global_tr_frame));
  direct_ba.AddKeyframe(new_keyframe);

  // Create surfels from the depth map.
  direct_ba.CreateSurfelsForKeyframe(stream, /*filter_new_surfels*/ false, new_keyframe);

  // Attempt to estimate the frame's pose, starting from different offsets.
  constexpr float kTranslationOffset = 0.005f;
  constexpr float kRotationOffset = 0.001f;
  SE3f offsets[] = {
      // Identity
      SE3f(),
      // Positive offsets
      SE3f::exp((Matrix<float, 6, 1>() << kTranslationOffset, 0, 0, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, kTranslationOffset, 0, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, kTranslationOffset, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, kRotationOffset, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, 0, kRotationOffset, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, 0, 0, kRotationOffset).finished()),
      // Negative offsets
      SE3f::exp((Matrix<float, 6, 1>() << -kTranslationOffset, 0, 0, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, -kTranslationOffset, 0, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, -kTranslationOffset, 0, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, -kRotationOffset, 0, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, 0, -kRotationOffset, 0).finished()),
      SE3f::exp((Matrix<float, 6, 1>() << 0, 0, 0, 0, 0, -kRotationOffset).finished())};

  for (int offset_index = 0; offset_index < static_cast<int>(sizeof(offsets) / sizeof(offsets[0])); ++ offset_index) {
    SE3f global_tr_frame_estimate;
    direct_ba.EstimateFramePose(
        stream,
        offsets[offset_index] * global_tr_frame.inverse(),
        new_keyframe->depth_buffer(),
        new_keyframe->normals_buffer(),
        new_keyframe->color_texture(),
        &global_tr_frame_estimate,
        /*called_within_ba*/ false);

    Matrix<float, 6, 1> error = (global_tr_frame_estimate.inverse() * global_tr_frame).log();
//     LOG(INFO) << "Case " << offset_index << " error: " << error.transpose();
    for (int i = 0; i < 6; ++ i) {
      constexpr float kTolerance = 1e-6f;
      EXPECT_NEAR(0.f, error(i), kTolerance) << "Error in test case " << offset_index;
    }
  }

  cudaStreamDestroy(stream);
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (c) 2021, Institut de Robotica i Informatica Industrial (CSIC-UPC)
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef EAGLE_MPC_UTILS_TOOLS_HPP_
#define EAGLE_MPC_UTILS_TOOLS_HPP_

#include <Eigen/Dense>

#include "eagle_mpc/multicopter-base-params.hpp"

namespace eagle_mpc
{
class Tools
{
    public:
    Tools();
    ~Tools();
    static void thrustToSpeed(const Eigen::VectorXd&                          thrust,
                              const boost::shared_ptr<MultiCopterBaseParams>& params,
                              Eigen::VectorXd&                                speed);

    static void thrustToSpeedNormalized(const Eigen::VectorXd&                          thrust,
                                        const boost::shared_ptr<MultiCopterBaseParams>& params,
                                        Eigen::VectorXd&                                speed);
};

}  // namespace eagle_mpc
#endif


#include <boost/geometry/strategies/cartesian/distance_pythagoras_point_box.hpp>