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

#include <iostream>
#include <sstream>
#include <stdexcept>

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

#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace basic_mathematics
{

//! Default constructor, initializes cache object with 0 maximum degree and order.
LegendreCache::LegendreCache( const bool useGeodesyNormalization )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( 1, 1 );

    computeSecondDerivatives_ = 0;

}

//! Constructor
LegendreCache::LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization  )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( maximumDegree, maximumOrder );
    computeSecondDerivatives_ = 0;
}

//! Get Legendre polynomial from cache when possible, and from direct computation otherwise.
void LegendreCache::update( const double polynomialParameter  )
{
    // Check if cache needs update
    if( !( polynomialParameter == currentPolynomialParameter_ ) )
    {
        currentPolynomialParameter_ = polynomialParameter;

        // Set complement of argument (assuming it to be sine of latitude) cosine of latitude is always positive.
        currentPolynomialParameterComplement_ = std::sqrt( 1.0 - polynomialParameter * polynomialParameter );

        LegendreCache& thisReference = *this;

        int jMax = -1;
        for( int i = 0; i <= maximumDegree_; i++ )
        {
            jMax = std::min( i, maximumOrder_ );
            for( int j = 0; j <= jMax ; j++ )
            {
                // Compute legendre polynomial
                legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] = legendrePolynomialFunction_( i, j, thisReference );

                if( j != 0 )
                {
                    // Compute legendre polynomial derivative
                    if( useGeodesyNormalization_ )
                    {
                        legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] =
                                computeGeodesyLegendrePolynomialDerivative(
                                    i, j - 1, currentPolynomialParameter_,
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                legendreValues_[ i * ( maximumOrder_ + 1 ) + j ],
                                derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] );
                    }
                    else
                    {
                        legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] =
                                computeLegendrePolynomialDerivative(
                                    j - 1, currentPolynomialParameter_,
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] );
                    }

                }
            }

            // Compute legendre polynomial derivative for i = j  (if needed)
            if( jMax == i )
            {
                if( useGeodesyNormalization_ )
                {
                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) +  jMax ] =
                            computeGeodesyLegendrePolynomialDerivative(
                                i, jMax, currentPolynomialParameter_,
                                legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0,
                            derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + jMax ] );
                }
                else
                {
                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ] =
                            computeLegendrePolynomialDerivative(
                                jMax, currentPolynomialParameter_,
                                legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0 );
                }
            }
        }

        // Compute second derivatives of Legendre polynomials if needed
        if( computeSecondDerivatives_ )
        {
            for( int i = 0; i <= maximumDegree_; i++ )
            {
                jMax = std::min( i, maximumOrder_ );
                for( int j = 0; j <= jMax ; j++ )
                {
                    if( j != 0 )
                    {
                        // Compute legendre polynomial second derivatives
                        if( useGeodesyNormalization_ )
                        {
                            legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] =
                                    computeGeodesyLegendrePolynomialSecondDerivative(
                                        i, j - 1, currentPolynomialParameter_,
                                        legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + j ],
                                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + j ],
                                    derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] );
                        }
                        else
                        {
                            legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ] =
                                    computeGeodesyLegendrePolynomialSecondDerivative(
                                        i, j - 1, currentPolynomialParameter_,
                                        legendreValues_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + j ],
                                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + ( j - 1 ) ],
                                    legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + j ], 1.0 );
                        }

                    }
                }
                // Compute legendre polynomial second derivative for i = j  (if needed)
                if( jMax == i )
                {
                    if( useGeodesyNormalization_ )
                    {
                        legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) +  jMax ] =
                                computeGeodesyLegendrePolynomialSecondDerivative(
                                    i, jMax, currentPolynomialParameter_,
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0,
                                legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0,
                                derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + jMax ] );
                    }
                    else
                    {
                        legendreSecondDerivatives_[ i * ( maximumOrder_ + 1 ) +  jMax ] =
                                computeGeodesyLegendrePolynomialSecondDerivative(
                                    i, jMax, currentPolynomialParameter_,
                                    legendreValues_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0,
                                legendreDerivatives_[ i * ( maximumOrder_ + 1 ) + jMax ], 0.0,
                                1.0 );
                    }
                }

            }
        }
    }
}

//! Update maximum degree and order of cache
void LegendreCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;

    if( maximumOrder_ > maximumDegree_ )
    {
        maximumOrder_ = maximumDegree_;
    }

    legendreValues_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );
    legendreDerivatives_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );
    legendreSecondDerivatives_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );

    derivativeNormalizations_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );

    for( int i = 0; i <= maximumDegree_; i++ )
    {
        for( int j = 0; ( ( j <= i ) && ( j <= maximumOrder_ ) ) ; j++ )
        {
            // Compute normalization correction factor.
            derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + j ] = std::sqrt(
                        ( static_cast< double >( i + j + 1 ) )
                        * ( static_cast< double >( i - j ) ) );

            // If order is zero apply multiplication factor.
            if ( j == 0 )
            {
                derivativeNormalizations_[ i * ( maximumOrder_ + 1 ) + j ] *= std::sqrt( 0.5 );
            }
        }
    }

    currentPolynomialParameter_ = TUDAT_NAN;
    currentPolynomialParameterComplement_ = TUDAT_NAN;
}

//! Get Legendre polynomial value from the cache.
double LegendreCache::getLegendrePolynomial(
        const int degree, const int order )
{
    if( degree > maximumDegree_ || order > maximumOrder_ )
    {
        std::string errorMessage = "Error when requesting legendre cache, maximum degree or order exceeded " +
                std::to_string( degree ) + " " +
                std::to_string( maximumDegree_ ) + " " +
                std::to_string( order ) + " " +
                std::to_string( maximumOrder_ );
        throw std::runtime_error( errorMessage );
        return TUDAT_NAN;
    }
    else if( order > degree )
    {
        return 0.0;
    }
    else
    {
        return legendreValues_[ degree * ( maximumOrder_ + 1  ) + order ];
    };
}

//! Get first derivative of Legendre polynomial value from the cache.
double LegendreCache::getLegendrePolynomialDerivative(
        const int degree, const int order )
{
    if( degree > ( maximumDegree_ ) || order > maximumOrder_ )
    {
        std::string errorMessage = "Error when requesting legendre cache first derivatives, maximum degree or order exceeded " +
                std::to_string( degree ) + " " +
                std::to_string( maximumDegree_ ) + " " +
                std::to_string( order ) + " " +
                std::to_string( maximumOrder_ );
        throw std::runtime_error( errorMessage );
        return TUDAT_NAN;
    }
    else if( order > degree )
    {
        return 0.0;
    }
    else
    {
        return legendreDerivatives_[ degree * ( maximumOrder_ + 1  ) + order ];
    };
}

//! Get second derivative of Legendre polynomial value from the cache.
double LegendreCache::getLegendrePolynomialSecondDerivative(
        const int degree, const int order )
{
    if( degree > ( maximumDegree_  ) || order > maximumOrder_ )
    {
        std::string errorMessage = "Error when requesting legendre cache second derivatives, maximum degree or order exceeded " +
                std::to_string( degree ) + " " +
                std::to_string( maximumDegree_ ) + " " +
                std::to_string( order ) + " " +
                std::to_string( maximumOrder_ );
        throw std::runtime_error( errorMessage );
        return TUDAT_NAN;
    }
    else if( computeSecondDerivatives_ == 0 )
    {
        throw std::runtime_error( "Error when requesting legendre cache second derivatives, no computations performed" );
    }
    else if( order > degree )
    {
        return 0.0;
    }
    else
    {
        return legendreSecondDerivatives_[ degree * ( maximumOrder_ + 1  ) + order ];
    };
}

//! Compute unnormalized associated Legendre polynomial.
double computeLegendrePolynomialFromCache( const int degree,
                                           const int order,
                                           LegendreCache& legendreCache )
{
    if( legendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        throw std::runtime_error( errorMessage.str( ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeLegendrePolynomialExplicit( degree, order, legendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        const double degreeOneOrderOnePolynomial = legendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        const double priorSectoralPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        const double oneDegreePriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        const double twoDegreesPriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeLegendrePolynomialVertical( degree,
                                                  order,
                                                  legendreCache.getCurrentPolynomialParameter( ),
                                                  oneDegreePriorPolynomial,
                                                  twoDegreesPriorPolynomial );
    }
}

double computeLegendrePolynomial( const int degree,
                                  const int order,
                                  const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 0 );
    legendreCache.update( legendreParameter );
    return computeLegendrePolynomialFromCache( degree, order, legendreCache );
}

//! Compute geodesy-normalized associated Legendre polynomial.
double computeGeodesyLegendrePolynomialFromCache( const int degree,
                                                  const int order,
                                                  LegendreCache& geodesyLegendreCache )
{

    if( !geodesyLegendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses no normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        throw std::runtime_error( errorMessage.str( ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeGeodesyLegendrePolynomialExplicit( degree, order, geodesyLegendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        double degreeOneOrderOnePolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        double priorSectoralPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        double oneDegreePriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        double twoDegreesPriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialVertical( degree,
                                                         order,
                                                         geodesyLegendreCache.getCurrentPolynomialParameter( ),
                                                         oneDegreePriorPolynomial,
                                                         twoDegreesPriorPolynomial );
    }
}

//! Compute geodesy-normalized associated Legendre polynomial.
double computeGeodesyLegendrePolynomial( const int degree,
                                         const int order,
                                         const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 1 );
    legendreCache.update( legendreParameter );
    return computeGeodesyLegendrePolynomialFromCache( degree, order, legendreCache );
}

//! Compute derivative of unnormalized Legendre polynomial.
double computeLegendrePolynomialDerivative( const int order,
                                            const double polynomialParameter,
                                            const double currentLegendrePolynomial,
                                            const double incrementedLegendrePolynomial )
{
    // Return polynomial derivative.
    return incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute derivative of geodesy-normalized Legendre polynomial.
double computeGeodesyLegendrePolynomialDerivative( const int degree,
                                                   const int order,
                                                   const double polynomialParameter,
                                                   const double currentLegendrePolynomial,
                                                   const double incrementedLegendrePolynomial,
                                                   const double normalizationCorrection )
{
    // Return polynomial derivative.
    return normalizationCorrection * incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute derivative of geodesy-normalized Legendre polynomial.
double computeGeodesyLegendrePolynomialDerivative( const int degree,
                                                   const int order,
                                                   const double polynomialParameter,
                                                   const double currentLegendrePolynomial,
                                                   const double incrementedLegendrePolynomial )
{
    // Compute normalization correction factor.
    double normalizationCorrection = std::sqrt(
                ( static_cast< double >( degree + order + 1 ) )
                * ( static_cast< double >( degree - order ) ) );

    // If order is zero apply multiplication factor.
    if ( order == 0 )
    {
        normalizationCorrection *= std::sqrt( 0.5 );
    }

    // Return polynomial derivative.
    return computeGeodesyLegendrePolynomialDerivative(
                degree, order, polynomialParameter, currentLegendrePolynomial,
                incrementedLegendrePolynomial, normalizationCorrection );
}

//! Compute second derivative of geodesy-normalized associated Legendre polynomial.
double computeGeodesyLegendrePolynomialSecondDerivative( const int degree,
                                                         const int order,
                                                         const double polynomialParameter,
                                                         const double currentLegendrePolynomial,
                                                         const double incrementedLegendrePolynomial,
                                                         const double currentLegendrePolynomialDerivative,
                                                         const double incrementedLegendrePolynomialDerivative,
                                                         const double normalizationCorrection )
{
    double polynomialParameterSquare = polynomialParameter * polynomialParameter;

    // Return polynomial derivative.
    return normalizationCorrection * (
                incrementedLegendrePolynomialDerivative / std::sqrt( 1.0 - polynomialParameterSquare ) +
                polynomialParameter * std::pow( 1.0 - polynomialParameterSquare, -1.5 ) * incrementedLegendrePolynomial ) -
            static_cast< double >( order ) *
            ( polynomialParameter / ( 1.0 - polynomialParameter * polynomialParameter ) * currentLegendrePolynomialDerivative +
              ( 1.0 + polynomialParameterSquare ) / ( ( 1.0 - polynomialParameterSquare ) * ( 1.0 - polynomialParameterSquare ) ) * currentLegendrePolynomial );
}

//! Compute low degree/order unnormalized Legendre polynomial explicitly.
double computeLegendrePolynomialExplicit( const int degree,
                                          const int order,
                                          const double polynomialParameter )
{
    // Check which order is required for Legendre polynomial.
    switch( degree )
    {
    case 0:
        switch( order )
        {
        case 0:
            return 1.0;
        default:
        {
            std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                    std::to_string( degree ) + ", " +
                    std::to_string( order );
            throw std::runtime_error( errorMessage );
        }
        }
        break;
    case 1:
        switch( order )
        {
        case 0:
            return polynomialParameter;
        case 1:
            return std::sqrt( 1 - polynomialParameter * polynomialParameter );
        default:
        {
            std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                    std::to_string( degree ) + ", " +
                    std::to_string( order );
            throw std::runtime_error( errorMessage );
        }
        }
        break;
    case 2:
        switch( order )
        {
        case 0:
            return 0.5 * ( 3.0 * polynomialParameter * polynomialParameter - 1.0 );
        case 1:
            return 3.0 * polynomialParameter
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 3.0 * ( 1.0 - polynomialParameter * polynomialParameter );
        default:
        {
            std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                    std::to_string( degree ) + ", " +
                    std::to_string( order );
            throw std::runtime_error( errorMessage );
        }
        }
        break;
    case 3:
        switch( order )
        {
        case 0:
            return 0.5 * polynomialParameter
                    * ( 5.0 * polynomialParameter * polynomialParameter - 3.0 );
        case 1:
            return 1.5 * ( 5.0 * polynomialParameter * polynomialParameter - 1.0 )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return 15.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        default:
        {
            std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                    std::to_string( degree ) + ", " +
                    std::to_string( order );
            throw std::runtime_error( errorMessage );
        }
        }
        break;
    case 4:
        switch( order )
        {
        case 0:
            return ( 35.0 * polynomialParameter * polynomialParameter
                     * polynomialParameter * polynomialParameter
                     - 30.0 * polynomialParameter * polynomialParameter + 3.0 ) / 8.0;
        case 1:
            return -2.5 * ( 7.0 * polynomialParameter * polynomialParameter * polynomialParameter
                            - 3.0 * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 / 2.0 * ( - 1.0 + 7.0 * polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return -105.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 4:
            return 105.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );

        default:
        {
            std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                    std::to_string( degree ) + ", " +
                    std::to_string( order );
            throw std::runtime_error( errorMessage );
        }
        }
        break;
    default:
    {
        std::string errorMessage = "Error, explicit legendre polynomial not possible for " +
                std::to_string( degree ) + ", " +
                std::to_string( order );
        throw std::runtime_error( errorMessage );
    }
    }
    return TUDAT_NAN;
}

//! Compute low degree/order geodesy-normalized Legendre polynomials explicitly.
double computeGeodesyLegendrePolynomialExplicit( const int degree,
                                                 const int order,
                                                 const double polynomialParameter )
{
    // If 0,0 term is requested return Legendre polynomial value.
    if ( degree == 0 && order == 0 )
    {
        return 1.0;
    }

    // Else if 1,0 term is requested return polynomial value.
    else if ( degree == 1 && order == 0 )
    {
        return std::sqrt( 3.0 ) * polynomialParameter;
    }

    // Else if 1,1 term is requested return polynomial value.
    else if ( degree == 1 && order == 1 )
    {
        return std::sqrt( 3.0 - 3.0 * polynomialParameter * polynomialParameter );
    }

    // Else the requested term cannot be computed; throw a run-time error.
    else
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage  <<  "Error: computation of Legendre polynomial of = "  <<  degree
                       <<  " and order = "  <<  order  <<  " is not supported."  <<  std::endl;

        // Throw a run-time error.
        throw std::runtime_error( errorMessage.str( ) );
    }
}

//! Compute unnormalized Legendre polynomial through sectoral recursion.
double computeLegendrePolynomialDiagonal( const int degree,
                                          const double degreeOneOrderOnePolynomial,
                                          const double priorSectoralPolynomial )
{
    // Return polynomial.
    return ( 2.0 * static_cast< double >( degree ) - 1.0 )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;

}

//! Compute geodesy-normalized Legendre polynomial through sectoral recursion.
double computeGeodesyLegendrePolynomialDiagonal( const int degree,
                                                 const double degreeOneOrderOnePolynomial,
                                                 const double priorSectoralPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( 6.0 * static_cast< double >( degree ) ) )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;
}

//! Compute unnormalized Legendre polynomial through degree recursion.
double computeLegendrePolynomialVertical( const int degree,
                                          const int order,
                                          const double polynomialParameter,
                                          const double oneDegreePriorPolynomial,
                                          const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return ( ( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter
             * oneDegreePriorPolynomial - ( static_cast< double >( degree + order ) - 1.0 )
             * twoDegreesPriorPolynomial ) / ( static_cast< double >( degree - order ) );
}

//! Compute geodesy-normalized Legendre polynomial through degree recursion.
double computeGeodesyLegendrePolynomialVertical( const int degree,
                                                 const int order,
                                                 const double polynomialParameter,
                                                 const double oneDegreePriorPolynomial,
                                                 const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( ( static_cast< double >( degree + order ) ) * ( static_cast< double >( degree - order ) ) ) )
            * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter * oneDegreePriorPolynomial
                - std::sqrt( ( static_cast< double >( degree + order ) - 1.0 )
                             * ( static_cast< double >( degree - order ) - 1.0 )
                             / ( 2.0 * static_cast< double >( degree ) - 3.0 ) )
                * twoDegreesPriorPolynomial );
}

//! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized.
double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order )
{

    double deltaFunction = 0.0;
    if( order == 0 )
    {
        deltaFunction = 1.0;
    }

    double factor = std::sqrt(
                boost::math::factorial< double >( static_cast< double >( degree + order ) )
                / ( ( 2.0 - deltaFunction ) * ( 2.0 * static_cast< double >( degree ) + 1.0 )
                    * boost::math::factorial< double >( static_cast< double >( degree - order ) ) ) );
    return 1.0 / factor;
}

//! Function to convert unnormalized to geodesy-normalized (4-pi normalized) spherical harmonic coefficients
void convertUnnormalizedToGeodesyNormalizedCoefficients(
        const Eigen::MatrixXd& unnormalizedCosineCoefficients,
        const Eigen::MatrixXd& unnormalizedSineCoefficients,
        Eigen::MatrixXd& normalizedCosineCoefficients,
        Eigen::MatrixXd& normalizedSineCoefficients )
{
    normalizedCosineCoefficients.setZero( unnormalizedCosineCoefficients.rows( ), unnormalizedCosineCoefficients.cols( ) );
    normalizedSineCoefficients.setZero( unnormalizedSineCoefficients.rows( ), unnormalizedCosineCoefficients.cols( ) );

    double normalizationFactor;

    for( unsigned degree = 0 ; degree < unnormalizedCosineCoefficients.rows( ); degree++ )
    {
        for( unsigned order = 0 ; ( order < unnormalizedCosineCoefficients.cols( ) && order <= degree ); order++ )
        {
            normalizationFactor = calculateLegendreGeodesyNormalizationFactor( degree, order );
            normalizedCosineCoefficients( degree, order ) = unnormalizedCosineCoefficients( degree, order ) /
                    normalizationFactor;
            normalizedSineCoefficients( degree, order ) = unnormalizedSineCoefficients( degree, order ) /
                    normalizationFactor;
        }
    }
}

//! Function to convert geodesy-normalized (4-pi normalized) to unnormalized spherical harmonic coefficients
void convertGeodesyNormalizedToUnnormalizedCoefficients(
        const Eigen::MatrixXd& normalizedCosineCoefficients,
        const Eigen::MatrixXd& normalizedSineCoefficients,
        Eigen::MatrixXd& unnormalizedCosineCoefficients,
        Eigen::MatrixXd& unnormalizedSineCoefficients )
{
    unnormalizedCosineCoefficients.setZero( normalizedCosineCoefficients.rows( ), normalizedCosineCoefficients.cols( ) );
    unnormalizedSineCoefficients.setZero( normalizedSineCoefficients.rows( ), normalizedSineCoefficients.cols( ) );

    double normalizationFactor;

    for( unsigned degree = 0 ; degree < unnormalizedCosineCoefficients.rows( ); degree++ )
    {
        for( unsigned order = 0 ; ( order < unnormalizedCosineCoefficients.cols( ) && order <= degree ); order++ )
        {
            normalizationFactor = calculateLegendreGeodesyNormalizationFactor( degree, order );
            unnormalizedCosineCoefficients( degree, order ) = normalizedCosineCoefficients( degree, order ) *
                    normalizationFactor;
            unnormalizedSineCoefficients( degree, order ) = normalizedSineCoefficients( degree, order ) *
                    normalizationFactor;
        }
    }
}

//! Function to convert unnormalized to geodesy-normalized (4-pi normalized) spherical harmonic coefficients
void geodesyNormalizeUnnormalizedCoefficients(
        Eigen::MatrixXd& cosineCoefficients,
        Eigen::MatrixXd& sineCoefficients )
{
    double normalizationFactor;

    for( unsigned degree = 0 ; degree < cosineCoefficients.rows( ); degree++ )
    {
        for( unsigned order = 0 ; ( order < sineCoefficients.cols( ) && order <= degree ); order++ )
        {
            normalizationFactor = calculateLegendreGeodesyNormalizationFactor( degree, order );
            cosineCoefficients( degree, order ) /=  normalizationFactor;
            sineCoefficients( degree, order ) /=  normalizationFactor;
        }
    }
}

} // namespace basic_mathematics
} // namespace tudat


#pragma once

#include <boost/circular_buffer.hpp>

#include <Geometry2d/Point.hpp>
#include <Utils.hpp>
#include <Configuration.hpp>

#include "vision/ball/CameraBall.hpp"
#include "vision/filter/KalmanFilter2D.hpp"

class WorldBall;

/**
 * Filtered ball estimation for a single camera
 */
class KalmanBall {
public:
    /**
     * Checks the previousWorldBall to see if it's valid
     *
     * @param cameraId ID of the camera this filter belongs to
     * @param creationTime Time this filter is created
     * @param initMeasurement Initial ball measurement we are creating the filter at
     * @param previousWorldBall Previous prediction of ball location to initialize the velocity smartly
     */
    KalmanBall(unsigned int cameraID, RJ::Time creationTime,
               CameraBall initMeasurement, const WorldBall& previousWorldBall);

    /**
     * Predicts one time step forward
     *
     * @param currentTime Time at the current frame
     *
     * @note Call either this OR predictAndUpdate once a frame
     */
    void predict(RJ::Time currentTime);

    /**
     * Predicts one time step forward then triangulates towards the measurement
     *
     * @param currentTime Current time of the prediction/update step
     * @param updateBall Ball measurement that we are using as feedback to the filters
     *
     * @note Call either this OR predict once a frame
     */
    void predictAndUpdate(RJ::Time currentTime, CameraBall updateBall);

    /**
     * @return Returns true when the filter hasn't been updated in a while etc and should be deleted
     */
    bool isUnhealthy() const;

    /**
     * @return The camera id this belongs to
     */
    unsigned int getCameraID() const;

    /**
     * @return How healthy this filter is. AKA How often it's been updated
     */
    int getHealth() const;

    /**
     * @return Best estimate of the position of the ball
     */
    Geometry2d::Point getPos() const;

    /**
     * @return Best estimate of the velocity of the ball
     */
    Geometry2d::Point getVel() const;

    /**
     * @return Covariance in X and Y direction of the position of the ball
     */
    Geometry2d::Point getPosCov() const;

    /**
     * @return Covariance in X and Y direction of the velocity of the ball
     */
    Geometry2d::Point getVelCov() const;

    /**
     * @return List of previous camera ball measurements for kick detection/estimation
     */
    const boost::circular_buffer<CameraBall>& getPrevMeasurements() const;

    /**
     * @param newVel new velocity to insert into the kalman filter
     *
     * Note: Only used to set the velocity when we think the ball will bounce
     * off another robot
     */
    void setVel(Geometry2d::Point newVel);

    static void createConfiguration(Configuration* cfg);

private:
    RJ::Time lastUpdateTime;
    RJ::Time lastPredictTime;

    // Keeps track of this for kick detection stuff
    boost::circular_buffer<CameraBall> previousMeasurements;

    KalmanFilter2D filter;

    int health;

    unsigned int cameraID;

    // Max time in seconds that a filter can not be updated before it is removed
    static ConfigDouble* max_time_outside_vision;
};


#ifndef YANNQ_SUPERVISED_OVERLAPOPTIMIZEREXACT_HPP
#define YANNQ_SUPERVISED_OVERLAPOPTIMIZEREXACT_HPP
#include <Eigen/Dense>
#include <limits>

#include "Utilities/Utility.hpp"
#include "Utilities/type_traits.hpp"
#include "GroundState/SRMatExact.hpp"
#include "GroundState/utils.hpp"

#include <iterator>
#include <tbb/tbb.h>

namespace yannq
{
//! Use this if the parameter type of the machine is complex.
template<class Machine>
class OverlapOptimizerExact
{
public:
	using Scalar = typename Machine::Scalar;
	using RealScalar = typename yannq::remove_complex<Scalar>::type;
	using Vector = typename Machine::Vector;
	using Matrix = typename Machine::Matrix;

private:
	const int n_;
	const Machine& qs_;
	tbb::concurrent_vector<uint32_t> basis_;

	Vector target_;

	Matrix deltas_;
	Matrix psiDeltas_;
	Vector ovs_;
	Vector oloc_;
	Scalar ov_;

public:
	template<class BasisType, typename disable_if<
		std::is_same<typename std::remove_reference<BasisType>::type,
		tbb::concurrent_vector<uint32_t>>::value,
		int>::type = 0>
	explicit OverlapOptimizerExact(const Machine& qs, BasisType&& basis)
		: n_(qs.getN()), qs_(qs)
	{
		basis_ = parallelConstructBasis(basis);
	}

	explicit OverlapOptimizerExact(const Machine& qs, tbb::concurrent_vector<uint32_t>&& basis)
		: n_(qs.getN()), qs_(qs), basis_(std::move(basis))
	{
	}

	void setTarget(Vector v)
	{
		assert(basis_.size() == v.size());

		target_ = std::move(v);
	}

	const Vector& getTarget() const&
	{
		return target_;
	}

	Vector getTarget() &&
	{
		return target_;
	}

	const tbb::concurrent_vector<uint32_t>& getBasis() const&
	{
		return basis_;
	}

	tbb::concurrent_vector<uint32_t> getBasis() &&
	{
		return basis_;
	}

	void constructExact()
	{
		using std::conj;

		Vector st = getPsi(qs_, basis_, true);

		SamplingResultExact srex(qs_, basis_);
		deltas_ = constructDelta(qs_, srex);

		psiDeltas_ = st.cwiseAbs2().asDiagonal()*deltas_;
		ovs_ = st.conjugate().cwiseProduct(target_);
		ov_ = ovs_.sum();
		oloc_ = psiDeltas_.colwise().sum();
	}

	//! return -\nabla_{\theta^*} \log |\langle \Phi | \psi_\theta \rangle|^2
	Vector calcLogGrad() const
	{
		Vector res = oloc().conjugate();
		Vector r1 = ovs_.transpose()*deltas_.conjugate();
		res -= r1/ov_;

		return res;
	}
	Vector calcGrad() const
	{
		Vector res = oloc().conjugate()*std::norm(ov_);
		res -= ovs_.transpose()*deltas_.conjugate()*conj(ov_);
		return res;
	}

	Vector oloc() const
	{
		return oloc_;
	}

	Matrix corrMat() const
	{
		Matrix res = deltas_.adjoint()*psiDeltas_;
		return res - oloc_.conjugate()*oloc_.transpose();
	}
	/**
	 *
	 */
	Matrix uncenteredCorrMat() const
	{
		Matrix res = deltas_.adjoint()*psiDeltas_;
		//Matrix ob = ovs_.conjugate().asDiagonal()*deltas_/std::conj(ov_);
		//res -= ob.adjoint()*ob;
		return res;
	}

	/**
	 * return squared fidelity: |\langle \psi_\theta | \phi \rangle|^2
	 */
	RealScalar fidelity() const
	{
		using std::norm;
		return norm(ov_);
	}
};

} //yannq
#endif//YANNQ_SUPERVISED_OVERLAPOPTIMIZEREXACT_HPP


//
// Created by Christoph Kummer on 26.02.19.
//

#include <WhiteBoxTableGenerator.h>
#include <iostream>
#include <vector>

#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>

#include <RandomPermutation.h>

namespace WhiteBox {
  WhiteBoxTableGenerator::WhiteBoxTableGenerator(
      std::array<uint8_t, AES_KEY_LENGTH_BYTES> aes_key,
      bool use_internal_encoding, bool use_mixing_bijections)
      : aesKey_(aes_key), usesMixingBijections_(use_mixing_bijections) {
    // Calculate round keys
    expandedAesKey_ = aes_key_schedule(aesKey_);

    // Encryption
    // Calculate T-Boxes and Tyi Tables
    TBoxes intermediateTBoxes = calculateTBoxes();
    calculateTyiTables(intermediateTBoxes);
    calculateXorTables(&xorTables_);

    // Decryption
    intermediateTBoxes = calculateTBoxesDecryption();
    calculateTyiTablesDecryption(intermediateTBoxes);
    calculateXorTables(&xorTablesDecryption_);

    if (use_mixing_bijections) {
      calculateMixingBijections();
    }

    if (use_internal_encoding) {
      if (use_mixing_bijections) {
        calculateInternalEncodingsWithMixingBijections();
      } else {
        calculateInternalEncodings();
      }
    }
  }

  TBoxes WhiteBoxTableGenerator::calculateTBoxes() {
    // Calculate T-Boxes for each round
    TBoxes tBoxes{};
    for (size_t i = 1; i < NUM_ROUNDS_AES_128; ++i) {
      State current_round_key;

      // Copy round key to current state
      std::copy(expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * (i - 1),
                expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * (i - 1) +
                AES_KEY_LENGTH_BYTES,
                current_round_key.begin());
      // Apply shift-rows to state
      current_round_key = shift_rows(current_round_key);

      // Create the 16 T-Boxes for this round
      for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
        for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
          tBoxes[i - 1][j][x] =
              apply_AES_SBox(static_cast<uint8_t>(x ^ current_round_key[j]));
        }
      }
    }

    // The last round has special rules
    State round_key_9{};
    State round_key_10{};

    // Copy round keys 9 and 10
    std::copy(
        expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 9,
        expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 9 + AES_KEY_LENGTH_BYTES,
        round_key_9.begin());
    std::copy(expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 10,
              expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 10 +
              AES_KEY_LENGTH_BYTES,
              round_key_10.begin());

    // Only round key 9 gets shifted in this round
    State shifted_round_key_9 = shift_rows(round_key_9);
    for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
      for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
        // These T-Boxes are inherent to the resultant algorithm, therefore
        // they must be saved separately
        finalRoundTBoxes_[j][x] =
            apply_AES_SBox(static_cast<uint8_t>(x ^ shifted_round_key_9[j])) ^
            round_key_10[j];
      }
    }

    return tBoxes;
  }

  TBoxes WhiteBoxTableGenerator::calculateTBoxesDecryption() {
    // We start with the last round in decryption
    State round_key_9{};
    State round_key_10{};

    // Copy round keys 9 and 10
    std::copy(
        expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 9,
        expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 9 + AES_KEY_LENGTH_BYTES,
        round_key_9.begin());
    std::copy(expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 10,
              expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * 10 +
              AES_KEY_LENGTH_BYTES,
              round_key_10.begin());

    // Calculate T-Boxes for each round
    TBoxes tBoxes{};
    // Only round key 9 gets shifted in this round
    State shifted_round_key_10 = inverse_shift_rows(round_key_10);

    for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
      for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
        tBoxes[0][j][x] = apply_AES_inverse_SBox(
            static_cast<uint8_t>(x ^ shifted_round_key_10[j])) ^
                          round_key_9[j];
      }
    }

    for (size_t i = 8; i >= 1; --i) {
      State current_round_key;

      // Copy round key to current state
      std::copy(expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * i,
                expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES * i +
                AES_KEY_LENGTH_BYTES,
                current_round_key.begin());

      // Create the 16 T-Boxes for this round
      for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
        for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
          tBoxes[8 - i + 1][j][x] =
              apply_AES_inverse_SBox(static_cast<uint8_t>(x)) ^
              current_round_key[j];
        }
      }
    }

    // Generate the T-Boxes for the last round
    State round_key_0{};
    std::copy(expandedAesKey_.begin(),
              expandedAesKey_.begin() + AES_KEY_LENGTH_BYTES,
              round_key_0.begin());
    for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
      for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
        finalRoundTBoxesDecryption_[j][x] =
            apply_AES_inverse_SBox(static_cast<uint8_t>(x)) ^ round_key_0[j];
      }
    }

    return tBoxes;
  }

  void WhiteBoxTableGenerator::calculateMixingBijections() {
    std::vector<MixingBijection<uint8_t>> bijections_8;
    std::vector<MixingBijection<uint8_t>> bijections_8_dec;
    std::vector<MixingBijection<uint32_t>> bijections_32;
    std::vector<MixingBijection<uint32_t>> bijections_32_dec;
    std::vector<MixingBijection<uint32_t>> bijections_8_concat;
    std::vector<MixingBijection<uint32_t>> bijections_8_concat_dec;

    calculateXorTables(&mixingXorTables_);
    calculateXorTables(&mixingXorTablesDecryption_);

    for (uint32_t i = 0; i < 9; ++i) {
      bijections_32.clear();
      bijections_32_dec.clear();
      for (uint32_t j = 0; j < 4; ++j) {
        bijections_32.emplace_back(rng);
        bijections_32_dec.emplace_back(rng);
      }

      mixTyiTables(i, bijections_8, bijections_32, i != 0);
      mixTyiTablesDecryption(i, bijections_8_dec, bijections_32_dec, i != 0);

      bijections_8.clear();
      bijections_8_dec.clear();
      for (uint32_t j = 0; j < 16; ++j) {
        bijections_8.emplace_back(rng);
        bijections_8_dec.emplace_back(rng);
      }
      bijections_8_concat.clear();
      bijections_8_concat_dec.clear();
      for (uint32_t j = 0; j < 16; j += 4) {
        MixingBijection<uint32_t> mb_concat =
            concatenateBijections(bijections_8[j + 3], bijections_8[j + 2],
                                  bijections_8[j + 1], bijections_8[j]);
        MixingBijection<uint32_t> mb_concat_dec = concatenateBijections(
            bijections_8_dec[j + 3], bijections_8_dec[j + 2],
            bijections_8_dec[j + 1], bijections_8_dec[j]);
        bijections_8_concat.push_back(mb_concat);
        bijections_8_concat_dec.push_back(mb_concat_dec);
      }

      calculateMixingTables(&mixingTables_, i, bijections_32, bijections_8_concat,
                            true);
      calculateMixingTables(&mixingTablesDecryption_, i, bijections_32_dec,
                            bijections_8_concat_dec, true);
    }

    mixFinalRoundTBoxes(bijections_8);
    mixFinalRoundTBoxesDecryption(bijections_8_dec);
  }

  void WhiteBoxTableGenerator::calculateMixingTables(
      MixingTables *mixing_tables, size_t round,
      const std::vector<MixingBijection<uint32_t>> &bijections_32,
      const std::vector<MixingBijection<uint32_t>> &bijections_8_concat,
      bool use_output_mixing_bijections) const {
    if (mixing_tables == nullptr) return;

    for (size_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
        NTL::vec_GF2 v;
        v.SetLength(32);

        for (size_t j = 0; j < 32; ++j) {
          v.put(j, 0);
        }

        if (i % 4 == 3) {
          for (size_t k = 0; k < 8; ++k) {
            auto zero_at_pos = static_cast<uint32_t>((x & (1 << k)) != 0);
            v.put(k, zero_at_pos);
          }
        } else if (i % 4 == 2) {
          for (size_t k = 8; k < 16; ++k) {
            auto zero_at_pos = static_cast<uint32_t>((x & (1 << (k - 8))) != 0);
            v.put(k, zero_at_pos);
          }
        } else if (i % 4 == 1) {
          for (size_t k = 16; k < 24; ++k) {
            auto zero_at_pos = static_cast<uint32_t>((x & (1 << (k - 16))) != 0);
            v.put(k, zero_at_pos);
          }
        } else if (i % 4 == 0) {
          for (size_t k = 24; k < 32; ++k) {
            auto zero_at_pos = static_cast<uint32_t>((x & (1 << (k - 24))) != 0);
            v.put(k, zero_at_pos);
          }
        }

        uint32_t transformed = bijections_32[i / 4].applyInverseTransformation(v);
        if (use_output_mixing_bijections) {
          transformed =
              bijections_8_concat[i / 4].applyTransformation(transformed);
        }
        (*mixing_tables)[round][i][x] = transformed;
      }
    }
  }

  void WhiteBoxTableGenerator::mixTyiTables(
      size_t round, const std::vector<MixingBijection<uint8_t>> &bijections_8,
      const std::vector<MixingBijection<uint32_t>> &bijections_32,
      bool use_input_mixing_bijection) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      TyiTable look_up_copy = tyiTables_[round][i];
      uint32_t shifted_index = get_shifted_index(i);

      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        auto transformed_val = static_cast<uint8_t>(j);
        if (use_input_mixing_bijection) {
          transformed_val =
              bijections_8[shifted_index].applyInverseTransformation(
                  transformed_val);
        }
        uint32_t tyi_Val = look_up_copy[transformed_val];
        tyi_Val = bijections_32[i / 4].applyTransformation(tyi_Val);
        tyiTables_[round][i][j] = tyi_Val;
      }
    }
  }

  void WhiteBoxTableGenerator::mixTyiTablesDecryption(
      size_t round, const std::vector<MixingBijection<uint8_t>> &bijections_8,
      const std::vector<MixingBijection<uint32_t>> &bijections_32,
      bool use_input_mixing_bijection) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      TyiTable look_up_copy = tyiTablesDecryption_[round][i];
      uint32_t shifted_index = get_inverse_shifted_index(i);

      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        auto transformed_val = static_cast<uint8_t>(j);
        if (use_input_mixing_bijection) {
          transformed_val =
              bijections_8[shifted_index].applyInverseTransformation(
                  transformed_val);
        }
        uint32_t tyi_Val = look_up_copy[transformed_val];
        tyi_Val = bijections_32[i / 4].applyTransformation(tyi_Val);
        tyiTablesDecryption_[round][i][j] = tyi_Val;
      }
    }
  }

  void WhiteBoxTableGenerator::mixFinalRoundTBoxes(
      const std::vector<MixingBijection<uint8_t>> &bijections_8) {
    RoundTBoxes look_up_copy = finalRoundTBoxes_;
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      uint32_t shifted_index = get_shifted_index(i);
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        auto transformed_val = static_cast<uint8_t>(j);
        transformed_val = bijections_8[shifted_index].applyInverseTransformation(
            transformed_val);
        uint8_t result = look_up_copy[i][transformed_val];
        finalRoundTBoxes_[i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::mixFinalRoundTBoxesDecryption(
      const std::vector<MixingBijection<uint8_t>> &bijections_8) {
    RoundTBoxes look_up_copy = finalRoundTBoxesDecryption_;
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      uint32_t shifted_index = get_inverse_shifted_index(i);
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        auto transformed_val = static_cast<uint8_t>(j);
        transformed_val = bijections_8[shifted_index].applyInverseTransformation(
            transformed_val);
        uint8_t result = look_up_copy[i][transformed_val];
        finalRoundTBoxesDecryption_[i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::calculateInternalEncodings() {
    std::vector<RandomPermutation<uint8_t>> tyi_output;
    std::vector<RandomPermutation<uint8_t>> tyi_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor1_output;
    std::vector<RandomPermutation<uint8_t>> xor1_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor2_output;
    std::vector<RandomPermutation<uint8_t>> xor2_output_dec;

    for (size_t i = 0; i < 9; ++i) {
      // Calculate Tyi output encodings
      tyi_output.clear();
      tyi_output_dec.clear();
      for (size_t j = 0; j < 16 * 8; ++j) {
        tyi_output.emplace_back(rng, 16);
        tyi_output_dec.emplace_back(rng, 16);
      }
      encodeTyiTables(i, xor2_output, tyi_output, true, i != 0);
      encodeTyiTablesDecryption(i, xor2_output_dec, tyi_output_dec, true, i != 0);

      // Calculate XOR1 output encodings
      xor1_output.clear();
      xor1_output_dec.clear();
      for (size_t j = 0; j < 8 * 8; ++j) {
        xor1_output.emplace_back(rng, 16);
        xor1_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&xorTables_, i, tyi_output, xor1_output, true, false);
      encodeXorTables(&xorTablesDecryption_, i, tyi_output_dec, xor1_output_dec,
                      true, false);

      // Calculate XOR2 output encodings
      xor2_output.clear();
      xor2_output_dec.clear();
      for (size_t j = 0; j < 32; ++j) {
        xor2_output.emplace_back(rng, 16);
        xor2_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&xorTables_, i, xor1_output, xor2_output, true, true);
      encodeXorTables(&xorTablesDecryption_, i, xor1_output_dec, xor2_output_dec,
                      true, true);
    }

    encodeFinalTBoxes(xor2_output);
    encodeFinalTBoxesDecryption(xor2_output_dec);
  }

  void WhiteBoxTableGenerator::calculateInternalEncodingsWithMixingBijections() {
    std::vector<RandomPermutation<uint8_t>> tyi_output;
    std::vector<RandomPermutation<uint8_t>> xor1_output;
    std::vector<RandomPermutation<uint8_t>> xor2_output;
    std::vector<RandomPermutation<uint8_t>> mixing_table_output;
    std::vector<RandomPermutation<uint8_t>> xor3_output;
    std::vector<RandomPermutation<uint8_t>> xor4_output;
    std::vector<RandomPermutation<uint8_t>> tyi_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor1_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor2_output_dec;
    std::vector<RandomPermutation<uint8_t>> mixing_table_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor3_output_dec;
    std::vector<RandomPermutation<uint8_t>> xor4_output_dec;

    for (size_t i = 0; i < 9; ++i) {
      // Calculate Tyi output encodings
      tyi_output.clear();
      tyi_output_dec.clear();
      for (size_t j = 0; j < 16 * 8; ++j) {
        tyi_output.emplace_back(rng, 16);
        tyi_output_dec.emplace_back(rng, 16);
      }
      encodeTyiTables(i, xor4_output, tyi_output, true, i != 0);
      encodeTyiTablesDecryption(i, xor4_output_dec, tyi_output_dec, true, i != 0);

      // Calculate XOR1 output encodings
      xor1_output.clear();
      xor1_output_dec.clear();
      for (size_t j = 0; j < 8 * 8; ++j) {
        xor1_output.emplace_back(rng, 16);
        xor1_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&xorTables_, i, tyi_output, xor1_output, true, false);
      encodeXorTables(&xorTablesDecryption_, i, tyi_output_dec, xor1_output_dec,
                      true, false);

      // Calculate XOR2 output encodings
      xor2_output.clear();
      xor2_output_dec.clear();
      for (size_t j = 0; j < 32; ++j) {
        xor2_output.emplace_back(rng, 16);
        xor2_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&xorTables_, i, xor1_output, xor2_output, true, true);
      encodeXorTables(&xorTablesDecryption_, i, xor1_output_dec, xor2_output_dec,
                      true, true);

      mixing_table_output.clear();
      mixing_table_output_dec.clear();
      for (size_t j = 0; j < 16 * 8; ++j) {
        mixing_table_output.emplace_back(rng, 16);
        mixing_table_output_dec.emplace_back(rng, 16);
      }
      encodeMixingTables(i, xor2_output, mixing_table_output, true);
      encodeMixingTablesDecryption(i, xor2_output_dec, mixing_table_output_dec,
                                   true);

      xor3_output.clear();
      xor3_output_dec.clear();
      for (size_t j = 0; j < 8 * 8; ++j) {
        xor3_output.emplace_back(rng, 16);
        xor3_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&mixingXorTables_, i, mixing_table_output, xor3_output,
                      true, false);
      encodeXorTables(&mixingXorTablesDecryption_, i, mixing_table_output_dec,
                      xor3_output_dec, true, false);

      xor4_output.clear();
      xor4_output_dec.clear();
      for (size_t j = 0; j < 32; ++j) {
        xor4_output.emplace_back(rng, 16);
        xor4_output_dec.emplace_back(rng, 16);
      }
      encodeXorTables(&mixingXorTables_, i, xor3_output, xor4_output, true, true);
      encodeXorTables(&mixingXorTablesDecryption_, i, xor3_output_dec,
                      xor4_output_dec, true, true);
    }

    encodeFinalTBoxes(xor4_output);
    encodeFinalTBoxesDecryption(xor4_output_dec);
  }

  void WhiteBoxTableGenerator::encodeXorTables(
      XorTables *xor_tables, size_t round,
      const std::vector<RandomPermutation<uint8_t>> &input_encodings,
      const std::vector<RandomPermutation<uint8_t>> &output_encodings,
      bool add_output_encodings, bool use_offset) const {
    const size_t offset = (use_offset) ? XOR_TABLE_OFFSET : 0;
    const size_t limit = (use_offset) ? 4 : 8;
    if (xor_tables == nullptr) return;

    for (size_t i = 0; i < limit; ++i) {
      for (size_t j = 0; j < 8; ++j) {
        XorTable &current_table = (*xor_tables)[round][j + i * 8 + offset];
        for (size_t k = 0; k <= std::numeric_limits<uint8_t>::max(); ++k) {
          auto x = static_cast<uint8_t>((k & 0xf0) >> 4);
          auto y = static_cast<uint8_t>(k & 0xf);

          const RandomPermutation<uint8_t> &perm_1 =
              input_encodings.at(j + i * 16);
          const RandomPermutation<uint8_t> &perm_2 =
              input_encodings.at(j + i * 16 + 8);
          uint8_t left_nibble_encoded = perm_1.getOutputInverse(x);
          uint8_t right_nibble_encoded = perm_2.getOutputInverse(y);
          uint8_t xor_result_encoded = left_nibble_encoded ^ right_nibble_encoded;
          if (add_output_encodings) {
            const RandomPermutation<uint8_t> &perm_3 =
                output_encodings.at(j + i * 8);
            xor_result_encoded = perm_3.getOutput(xor_result_encoded);
          }
          current_table[(x << 4) | y] = xor_result_encoded;
        }
      }
    }
  }

  void WhiteBoxTableGenerator::encodeTyiTables(
      size_t round,
      const std::vector<RandomPermutation<uint8_t>> &input_encodings,
      const std::vector<RandomPermutation<uint8_t>> &output_encodings,
      bool add_output_encoding, bool add_input_encoding) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      TyiTable look_up_copy = tyiTables_[round][i];
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp = j;
        uint32_t shifted_index = get_shifted_index(i);

        if (add_input_encoding) {
          auto x = static_cast<uint8_t>((j & 0xf0));
          x >>= 4;
          auto y = static_cast<uint8_t>(j & 0xf);

          uint32_t encoded_1 =
              input_encodings.at(shifted_index * 2).getOutputInverse(x);
          uint32_t encoded_2 =
              input_encodings.at(shifted_index * 2 + 1).getOutputInverse(y);

          temp = (encoded_1 << 4) | encoded_2;
        }

        uint32_t result = look_up_copy[temp];

        // Encode nibbles
        if (add_output_encoding) {
          // Get all 8 nibbles of result
          auto nibble_8 = static_cast<uint8_t>(result & 0xf);
          auto nibble_7 = static_cast<uint8_t>((result & 0xf0) >> 4);
          auto nibble_6 = static_cast<uint8_t>((result & 0xf00) >> 8);
          auto nibble_5 = static_cast<uint8_t>((result & 0xf000) >> 12);
          auto nibble_4 = static_cast<uint8_t>((result & 0xf0000) >> 16);
          auto nibble_3 = static_cast<uint8_t>((result & 0xf00000) >> 20);
          auto nibble_2 = static_cast<uint8_t>((result & 0xf000000) >> 24);
          auto nibble_1 = static_cast<uint8_t>((result & 0xf0000000) >> 28);
          nibble_1 = output_encodings.at(i * 8).getOutput(nibble_1);
          nibble_2 = output_encodings.at(i * 8 + 1).getOutput(nibble_2);
          nibble_3 = output_encodings.at(i * 8 + 2).getOutput(nibble_3);
          nibble_4 = output_encodings.at(i * 8 + 3).getOutput(nibble_4);
          nibble_5 = output_encodings.at(i * 8 + 4).getOutput(nibble_5);
          nibble_6 = output_encodings.at(i * 8 + 5).getOutput(nibble_6);
          nibble_7 = output_encodings.at(i * 8 + 6).getOutput(nibble_7);
          nibble_8 = output_encodings.at(i * 8 + 7).getOutput(nibble_8);
          result = (nibble_1 << 28) | (nibble_2 << 24) | (nibble_3 << 20) |
                   (nibble_4 << 16) | (nibble_5 << 12) | (nibble_6 << 8) |
                   (nibble_7 << 4) | nibble_8;
        }
        tyiTables_[round][i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::encodeTyiTablesDecryption(
      size_t round,
      const std::vector<RandomPermutation<uint8_t>> &input_encodings,
      const std::vector<RandomPermutation<uint8_t>> &output_encodings,
      bool add_output_encoding, bool add_input_encoding) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      TyiTable look_up_copy = tyiTablesDecryption_[round][i];
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp = j;
        uint32_t shifted_index = get_inverse_shifted_index(i);

        if (add_input_encoding) {
          auto x = static_cast<uint8_t>((j & 0xf0));
          x >>= 4;
          auto y = static_cast<uint8_t>(j & 0xf);

          uint32_t encoded_1 =
              input_encodings.at(shifted_index * 2).getOutputInverse(x);
          uint32_t encoded_2 =
              input_encodings.at(shifted_index * 2 + 1).getOutputInverse(y);

          temp = (encoded_1 << 4) | encoded_2;
        }

        uint32_t result = look_up_copy[temp];

        // Encode nibbles
        if (add_output_encoding) {
          // Get all 8 nibbles of result
          auto nibble_8 = static_cast<uint8_t>(result & 0xf);
          auto nibble_7 = static_cast<uint8_t>((result & 0xf0) >> 4);
          auto nibble_6 = static_cast<uint8_t>((result & 0xf00) >> 8);
          auto nibble_5 = static_cast<uint8_t>((result & 0xf000) >> 12);
          auto nibble_4 = static_cast<uint8_t>((result & 0xf0000) >> 16);
          auto nibble_3 = static_cast<uint8_t>((result & 0xf00000) >> 20);
          auto nibble_2 = static_cast<uint8_t>((result & 0xf000000) >> 24);
          auto nibble_1 = static_cast<uint8_t>((result & 0xf0000000) >> 28);
          nibble_1 = output_encodings.at(i * 8).getOutput(nibble_1);
          nibble_2 = output_encodings.at(i * 8 + 1).getOutput(nibble_2);
          nibble_3 = output_encodings.at(i * 8 + 2).getOutput(nibble_3);
          nibble_4 = output_encodings.at(i * 8 + 3).getOutput(nibble_4);
          nibble_5 = output_encodings.at(i * 8 + 4).getOutput(nibble_5);
          nibble_6 = output_encodings.at(i * 8 + 5).getOutput(nibble_6);
          nibble_7 = output_encodings.at(i * 8 + 6).getOutput(nibble_7);
          nibble_8 = output_encodings.at(i * 8 + 7).getOutput(nibble_8);
          result = (nibble_1 << 28) | (nibble_2 << 24) | (nibble_3 << 20) |
                   (nibble_4 << 16) | (nibble_5 << 12) | (nibble_6 << 8) |
                   (nibble_7 << 4) | nibble_8;
        }
        tyiTablesDecryption_[round][i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::encodeFinalTBoxes(
      const std::vector<RandomPermutation<uint8_t>> &input_encodings) {
    RoundTBoxes look_up_copy = finalRoundTBoxes_;
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp;
        uint32_t shifted_index = get_shifted_index(i);

        auto x = static_cast<uint8_t>((j & 0xf0));
        x >>= 4;
        auto y = static_cast<uint8_t>(j & 0xf);

        uint32_t encoded_1 =
            input_encodings.at(shifted_index * 2).getOutputInverse(x);
        uint32_t encoded_2 =
            input_encodings.at(shifted_index * 2 + 1).getOutputInverse(y);

        temp = (encoded_1 << 4) | encoded_2;
        uint8_t result = look_up_copy[i][temp];
        finalRoundTBoxes_[i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::encodeFinalTBoxesDecryption(
      const std::vector<RandomPermutation<uint8_t>> &input_encodings) {
    RoundTBoxes look_up_copy = finalRoundTBoxesDecryption_;
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp;
        uint32_t shifted_index = get_inverse_shifted_index(i);

        auto x = static_cast<uint8_t>((j & 0xf0));
        x >>= 4;
        auto y = static_cast<uint8_t>(j & 0xf);

        uint32_t encoded_1 =
            input_encodings.at(shifted_index * 2).getOutputInverse(x);
        uint32_t encoded_2 =
            input_encodings.at(shifted_index * 2 + 1).getOutputInverse(y);

        temp = (encoded_1 << 4) | encoded_2;
        uint8_t result = look_up_copy[i][temp];
        finalRoundTBoxesDecryption_[i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::encodeMixingTables(
      size_t round,
      const std::vector<RandomPermutation<uint8_t>> &input_encodings,
      const std::vector<RandomPermutation<uint8_t>> &output_encodings,
      bool add_output_encodings) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      MixingTable look_up_copy = mixingTables_[round][i];
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp;

        auto x = static_cast<uint8_t>((j & 0xf0));
        x >>= 4;
        auto y = static_cast<uint8_t>(j & 0xf);

        uint32_t encoded_1 = input_encodings.at(i * 2).getOutputInverse(x);
        uint32_t encoded_2 = input_encodings.at(i * 2 + 1).getOutputInverse(y);

        temp = (encoded_1 << 4) | encoded_2;

        uint32_t result = look_up_copy[temp];

        // Encode nibbles
        if (add_output_encodings) {
          // Get all 8 nibbles of result
          auto nibble_8 = static_cast<uint8_t>(result & 0xf);
          auto nibble_7 = static_cast<uint8_t>((result & 0xf0) >> 4);
          auto nibble_6 = static_cast<uint8_t>((result & 0xf00) >> 8);
          auto nibble_5 = static_cast<uint8_t>((result & 0xf000) >> 12);
          auto nibble_4 = static_cast<uint8_t>((result & 0xf0000) >> 16);
          auto nibble_3 = static_cast<uint8_t>((result & 0xf00000) >> 20);
          auto nibble_2 = static_cast<uint8_t>((result & 0xf000000) >> 24);
          auto nibble_1 = static_cast<uint8_t>((result & 0xf0000000) >> 28);
          nibble_1 = output_encodings.at(i * 8).getOutput(nibble_1);
          nibble_2 = output_encodings.at(i * 8 + 1).getOutput(nibble_2);
          nibble_3 = output_encodings.at(i * 8 + 2).getOutput(nibble_3);
          nibble_4 = output_encodings.at(i * 8 + 3).getOutput(nibble_4);
          nibble_5 = output_encodings.at(i * 8 + 4).getOutput(nibble_5);
          nibble_6 = output_encodings.at(i * 8 + 5).getOutput(nibble_6);
          nibble_7 = output_encodings.at(i * 8 + 6).getOutput(nibble_7);
          nibble_8 = output_encodings.at(i * 8 + 7).getOutput(nibble_8);
          result = (nibble_1 << 28) | (nibble_2 << 24) | (nibble_3 << 20) |
                   (nibble_4 << 16) | (nibble_5 << 12) | (nibble_6 << 8) |
                   (nibble_7 << 4) | nibble_8;
        }
        mixingTables_[round][i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::encodeMixingTablesDecryption(
      size_t round,
      const std::vector<RandomPermutation<uint8_t>> &input_encodings,
      const std::vector<RandomPermutation<uint8_t>> &output_encodings,
      bool add_output_encodings) {
    for (uint32_t i = 0; i < AES_KEY_LENGTH_BYTES; ++i) {
      MixingTable look_up_copy = mixingTablesDecryption_[round][i];
      for (uint32_t j = 0; j <= std::numeric_limits<uint8_t>::max(); ++j) {
        // Need copy as to not mess up loop
        uint32_t temp;

        auto x = static_cast<uint8_t>((j & 0xf0));
        x >>= 4;
        auto y = static_cast<uint8_t>(j & 0xf);

        uint32_t encoded_1 = input_encodings.at(i * 2).getOutputInverse(x);
        uint32_t encoded_2 = input_encodings.at(i * 2 + 1).getOutputInverse(y);

        temp = (encoded_1 << 4) | encoded_2;

        uint32_t result = look_up_copy[temp];

        // Encode nibbles
        if (add_output_encodings) {
          // Get all 8 nibbles of result
          auto nibble_8 = static_cast<uint8_t>(result & 0xf);
          auto nibble_7 = static_cast<uint8_t>((result & 0xf0) >> 4);
          auto nibble_6 = static_cast<uint8_t>((result & 0xf00) >> 8);
          auto nibble_5 = static_cast<uint8_t>((result & 0xf000) >> 12);
          auto nibble_4 = static_cast<uint8_t>((result & 0xf0000) >> 16);
          auto nibble_3 = static_cast<uint8_t>((result & 0xf00000) >> 20);
          auto nibble_2 = static_cast<uint8_t>((result & 0xf000000) >> 24);
          auto nibble_1 = static_cast<uint8_t>((result & 0xf0000000) >> 28);
          nibble_1 = output_encodings.at(i * 8).getOutput(nibble_1);
          nibble_2 = output_encodings.at(i * 8 + 1).getOutput(nibble_2);
          nibble_3 = output_encodings.at(i * 8 + 2).getOutput(nibble_3);
          nibble_4 = output_encodings.at(i * 8 + 3).getOutput(nibble_4);
          nibble_5 = output_encodings.at(i * 8 + 4).getOutput(nibble_5);
          nibble_6 = output_encodings.at(i * 8 + 5).getOutput(nibble_6);
          nibble_7 = output_encodings.at(i * 8 + 6).getOutput(nibble_7);
          nibble_8 = output_encodings.at(i * 8 + 7).getOutput(nibble_8);
          result = (nibble_1 << 28) | (nibble_2 << 24) | (nibble_3 << 20) |
                   (nibble_4 << 16) | (nibble_5 << 12) | (nibble_6 << 8) |
                   (nibble_7 << 4) | nibble_8;
        }
        mixingTablesDecryption_[round][i][j] = result;
      }
    }
  }

  void WhiteBoxTableGenerator::calculateTyiTables(const TBoxes &tBoxes) {
    for (int i = 0; i < NUM_ROUNDS_AES_128 - 1; ++i) {
      for (size_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
        const TBox &t_box = tBoxes[i][j];
        if (j % 4 == 0) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0x2, t);
            uint8_t byte_2 = t;
            uint8_t byte_3 = t;
            uint8_t byte_4 = galois_mul(0x3, t);

            tyiTables_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else if (j % 4 == 1) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0x3, t);
            uint8_t byte_2 = galois_mul(0x2, t);
            uint8_t byte_3 = t;
            uint8_t byte_4 = t;

            tyiTables_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else if (j % 4 == 2) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = t;
            uint8_t byte_2 = galois_mul(0x3, t);
            uint8_t byte_3 = galois_mul(0x2, t);
            uint8_t byte_4 = t;

            tyiTables_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = t;
            uint8_t byte_2 = t;
            uint8_t byte_3 = galois_mul(0x3, t);
            uint8_t byte_4 = galois_mul(0x2, t);

            tyiTables_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        }
      }
    }
  }

  void WhiteBoxTableGenerator::calculateTyiTablesDecryption(
      const TBoxes &tBoxes) {
    for (uint32_t i = 0; i < NUM_ROUNDS_AES_128 - 1; ++i) {
      for (uint32_t j = 0; j < AES_KEY_LENGTH_BYTES; ++j) {
        const TBox &t_box = tBoxes[i][j];
        if (j % 4 == 0) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0xe, t);
            uint8_t byte_2 = galois_mul(0x9, t);
            uint8_t byte_3 = galois_mul(0xd, t);
            uint8_t byte_4 = galois_mul(0xb, t);

            tyiTablesDecryption_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else if (j % 4 == 1) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0xb, t);
            uint8_t byte_2 = galois_mul(0xe, t);
            uint8_t byte_3 = galois_mul(0x9, t);
            uint8_t byte_4 = galois_mul(0xd, t);

            tyiTablesDecryption_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else if (j % 4 == 2) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0xd, t);
            uint8_t byte_2 = galois_mul(0xb, t);
            uint8_t byte_3 = galois_mul(0xe, t);
            uint8_t byte_4 = galois_mul(0x9, t);

            tyiTablesDecryption_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        } else {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            uint8_t t = t_box[x];

            uint8_t byte_1 = galois_mul(0x9, t);
            uint8_t byte_2 = galois_mul(0xd, t);
            uint8_t byte_3 = galois_mul(0xb, t);
            uint8_t byte_4 = galois_mul(0xe, t);

            tyiTablesDecryption_[i][j][x] =
                static_cast<unsigned int>((static_cast<uint32_t>(byte_1) << 24U) |
                                          (static_cast<uint32_t>(byte_2) << 16U) |
                                          (static_cast<uint32_t>(byte_3) << 8U) |
                                          static_cast<uint32_t>(byte_4));
          }
        }
      }
    }
  }

  void WhiteBoxTableGenerator::calculateXorTables(XorTables *xor_tables) const {
    if (xor_tables != nullptr) {
      for (size_t i = 0; i < 9; ++i) {
        for (size_t j = 0; j < 96; ++j) {
          for (size_t x = 0; x <= std::numeric_limits<uint8_t>::max(); ++x) {
            auto lower_nibble = static_cast<uint8_t>(x & 0xF);
            auto upper_nibble = static_cast<uint8_t>(x & 0xF0);
            (*xor_tables)[i][j][x] = (upper_nibble >> 4) ^ lower_nibble;
          }
        }
      }
    }
  }

  WhiteBoxData *WhiteBoxTableGenerator::getEncryptionTable() const {
    auto *data = new WhiteBoxData;

    // Since array has value semantics, this works
    // It is an expensive operation though
    data->finalRoundTBoxes_ = this->finalRoundTBoxes_;
    data->tyiTables_ = this->tyiTables_;
    data->xorTables_ = this->xorTables_;

    data->usesMixingBijections_ = this->usesMixingBijections_;

    data->mixingXorTables_ = this->mixingXorTables_;
    data->mixingTables_ = this->mixingTables_;

    return data;
  }

  WhiteBoxData *WhiteBoxTableGenerator::getDecryptionTable() const {
    auto *data = new WhiteBoxData;

    data->finalRoundTBoxes_ = this->finalRoundTBoxesDecryption_;
    data->tyiTables_ = this->tyiTablesDecryption_;
    data->xorTables_ = this->xorTablesDecryption_;

    data->usesMixingBijections_ = this->usesMixingBijections_;

    data->mixingXorTables_ = this->mixingXorTablesDecryption_;
    data->mixingTables_ = this->mixingTablesDecryption_;

    return data;
  }
}  // namespace WhiteBox


/*
 * MIT License
 *
 * Copyright (c) 2015 Alexis LE GOADEC
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

#include <boost/foreach.hpp>
#include "include/HyperGraphStat.hh"
#include "../model/include/AdjacentMatrix.hh"

HyperGraphStat::HyperGraphStat(const boost::shared_ptr<HypergrapheAbstrait>& ptrHypergrapheAbstrait) {
	_ptrHypergrapheAbstrait = ptrHypergrapheAbstrait;

	_nhEdge   = 0;
	_nhVertex = 0;
	_nhLink   = 0;
	_rang     = 0;
	_coRang   = 0;
}

unsigned int
HyperGraphStat::getNbrHyperEdge() const {
	return _nhEdge;
}

unsigned int
HyperGraphStat::getNbrHyperVertex() const {
	return _nhVertex;
}

unsigned int
HyperGraphStat::getNbrLinks() const {
	return _nhLink;
}

unsigned int
HyperGraphStat::getRang() const {
	return _rang;
}

unsigned int
HyperGraphStat::getCoRang() const {
	return _coRang;
}

void
HyperGraphStat::runAlgorithme() {

	_nhEdge = _ptrHypergrapheAbstrait->getHyperEdgeList().size();
	_nhVertex = _ptrHypergrapheAbstrait->getHyperVertexList().size();

	AdjacentMatrix m( _ptrHypergrapheAbstrait->getAdjacentMatrix() );
	LibType::ListHyperEdge eList( _ptrHypergrapheAbstrait->getHyperEdgeList() );

	_rang   = m.getRank();
	_coRang = m.getCoRank();
}

RStructure
HyperGraphStat::getResult() const {
	return _result;
}

HyperGraphStat::~HyperGraphStat() {

}


/*
 * Copyright (c) 2018-2020 Steven Varga, Toronto,ON Canada
 * Author: Varga, Steven <steven@vargaconsulting.ca>
 */
#include <armadillo>
#include <h5cpp/all>

// see: https://support.hdfgroup.org/HDF5/doc/RM/RM_H5P.html#Property-SetTypeConvCb
H5T_conv_ret_t handler( H5T_conv_except_t except_type, hid_t src_id, hid_t dst_id, void *src_buf, void *dst_buf, void *op_data){
	return H5T_CONV_HANDLED;
}

int main(){
	{ // CREATE - WRITE
		h5::fd_t fd = h5::create("arma.h5",H5F_ACC_TRUNC);
		arma::mat M(4,7); M.ones();				// create a matrix

		h5::write(fd,"transform",  M,
		// data transform provides linear transformation from character descriptions
		// linear operators {*,/,+,-} allowed on x variable:
				h5::data_transform{"2*x+5"} );
	}
	{ // READ back, pass conversion handler -- if internal conversion fails, and a
	  // data transform expression
		auto m = h5::read<arma::fmat>("arma.h5","transform",
			 h5::type_conv_cb{ {handler, nullptr} } | h5::data_transform{"x/2 + 1"});
		m.print();
	}

}


/**
 * @copyright Copyright (c) 2017 B-com http://www.b-com.com/
 *
 * 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 "xpcf/xpcf.h"
#include "api/solver/pose/I3DTransformSACFinderFrom3D3D.h"
#include "api/geom/I3DTransform.h"
#include "core/Log.h"
#include <boost/log/core.hpp>

using namespace SolAR;
using namespace SolAR::datastructure;
using namespace SolAR::api;

namespace xpcf  = org::bcom::xpcf;

int main(int argc,char** argv)
{
#if NDEBUG
    boost::log::core::get()->set_logging_enabled(false);
#endif

    LOG_ADD_LOG_TO_CONSOLE();
	/* instantiate component manager*/
	/* this is needed in dynamic mode */
	SRef<xpcf::IComponentManager> xpcfComponentManager = xpcf::getComponentManagerInstance();

    if (xpcfComponentManager->load("SolARTest_ModuleTools_3DTransformEstimationSACFrom3D3D_conf.xml") != org::bcom::xpcf::_SUCCESS)
	{
        LOG_ERROR("Failed to load the configuration file SolARTest_ModuleTools_3DTransformEstimationSACFrom3D3D_conf.xml")
			return -1;
	}

	// declare and create components
	LOG_INFO("Start creating components");
	auto estimator3D = xpcfComponentManager->resolve<solver::pose::I3DTransformSACFinderFrom3D3D>();
	auto transform3D = xpcfComponentManager->resolve<geom::I3DTransform>();
	// init random input points
	int NB_POINTS = 30;
	int NB_OUTLIERS = 15;
	std::vector<Point3Df> pts1(NB_POINTS);
	for (auto &it : pts1) {
		Vector3f tmp = Vector3f::Random();
		it.setX(tmp(0));
		it.setY(tmp(1));
		it.setZ(tmp(2));
	}

	// init random 3D transformation
	Vector3f euler = Vector3f::Random();
	Eigen::Matrix3f rot;
	rot = Eigen::AngleAxisf(euler(0), Vector3f::UnitZ()) * Eigen::AngleAxisf(euler(1), Vector3f::UnitY()) * Eigen::AngleAxisf(euler(2), Vector3f::UnitX());
	Vector3f tra = Vector3f::Random();
	Transform3Df poseToTransform;
	poseToTransform.linear() = rot;
	poseToTransform.translation() = tra;
	LOG_INFO("Transform 3D used: \n{}", poseToTransform.matrix());

	// apply 3D transformation to input points
	std::vector<Point3Df> pts2;
	std::vector<bool> bInliers(NB_POINTS, true);
	transform3D->transform(pts1, poseToTransform, pts2);
	for (int i = 0; i < NB_OUTLIERS; ++i) {
		int index = rand() % NB_POINTS;
		pts2[index].setZ(pts2[index].getZ() + 0.2 + (float)rand() / (RAND_MAX));
		bInliers[index] = false;
	}
	LOG_INFO("Inliers indices: ");
	for (int i = 0; i < NB_POINTS; ++i)
		if (bInliers[i])
			std::cout << i << " ";
	std::cout << "\n\n";

	// find transformation
	Transform3Df pose;
	std::vector<int> inliers;
	if (estimator3D->estimate(pts1, pts2, pose, inliers) == FrameworkReturnCode::_ERROR_) {
		LOG_ERROR("Cannot find 3D transformation");
		return 0;
	}

	// display results
	LOG_INFO("Transform 3D found: \n{}", pose.matrix());
	LOG_INFO("Inliers indices found: ");
	for (auto &it : inliers)
		std::cout << it << " ";
	std::cout << std::endl;

    return 0;
}