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


#ifndef FSTCLASSIFIERMULTINOMNAIVEBAYES_H
#define FSTCLASSIFIERMULTINOMNAIVEBAYES_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    classifier_multinom_naivebayes.hpp
   \brief   Implements Naive-like Bayes classifier based on multinomial model
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "classifier.hpp"
#include "model_multinom.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Implements Naive-like Bayes classifier based on multinomial model */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Classifier_Multinomial_NaiveBayes : public Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> { // abstract class
	// \note In this case the classifier should be better called Naive-like Bayes than Naive Bayes
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> const PSubset;
	typedef typename DATAACCESSOR::PPattern PPattern;
	Classifier_Multinomial_NaiveBayes();
	virtual ~Classifier_Multinomial_NaiveBayes() {notify("Classifier_Multinomial_NaiveBayes destructor.");}

	/*! pre-learning mode:
	    if true, calls once _model->learn() for full set size and later in train() uses only narrow()ing to access submatrixes
	             (makes sense only as long as training data do not change, i.e., within one split)
	    if false (default), learns new model in each train() call
	*/
	void enable_prelearn_mode(const PDataAccessor da);
	void disable_prelearn_mode() {_prelearn_mode=false;}
	bool get_prelearn_mode() const {return _prelearn_mode;}

	// NOTE: must! be called before train() or test() calls whenever the training set changes (i.e., with changing da Splits)
	void initialize(const PDataAccessor da); // must be called to pre-compute mean[] and cov[]

	virtual bool classify(DIMTYPE &cls, const PPattern &pattern);  // classifies pattern, returns the respective class index
	virtual bool train(const PDataAccessor da, const PSubset sub); // learns from designated training part of data
	virtual bool test(RETURNTYPE &result, const PDataAccessor da); // estimates accuracy using designated test data

	DIMTYPE get_n() const {assert(_model); return _model->get_n();}
	DIMTYPE get_d() const {assert(_model); return _model->get_d();}

	Classifier_Multinomial_NaiveBayes* clone() const;
	Classifier_Multinomial_NaiveBayes* sharing_clone() const {throw fst_error("Classifier_Multinomial_NaiveBayes::sharing_clone() not supported, use Classifier_Multinomial_NaiveBayes::clone() instead.");}
	Classifier_Multinomial_NaiveBayes* stateless_clone() const {throw fst_error("Classifier_Multinomial_NaiveBayes::stateless_clone() not supported, use Classifier_Multinomial_NaiveBayes::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Classifier_Multinomial_NaiveBayes()"; return os;}
private:
	Classifier_Multinomial_NaiveBayes(const Classifier_Multinomial_NaiveBayes& cmnb); // copy-constructor
protected:
	boost::scoped_ptr<Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> > _model;

	DIMTYPE _classes, _features; // size of arrays below
	boost::scoped_array<REALTYPE> _Pcd; // P(class|document) (lazy allocation)

private:
	bool _prelearn_mode;
	boost::scoped_array<DIMTYPE> _index;
	DIMTYPE _subfeatures;
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_Multinomial_NaiveBayes(const Classifier_Multinomial_NaiveBayes& cmnb) :
	_classes(cmnb._classes),
	_features(cmnb._features),
	_prelearn_mode(cmnb._prelearn_mode),
	_subfeatures(cmnb._subfeatures)
{
	notify("Classifier_Multinomial_NaiveBayes copy-constructor.");
	if(cmnb._model) _model.reset(new Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*cmnb._model));
	if(_classes>0)
	{
		_Pcd.reset(new REALTYPE[_classes]); memcpy((void *)_Pcd.get(),(void *)(cmnb._Pcd).get(),sizeof(REALTYPE)*_classes);
	}
	_index.reset(new DIMTYPE[_features]); memcpy((void *)_index.get(),(void *)(cmnb._index).get(),sizeof(DIMTYPE)*_features);
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_Multinomial_NaiveBayes()
{
	notify("Classifier_Multinomial_NaiveBayes constructor.");
	_model.reset(new Model_Multinomial<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>);
	_classes=0;
	_features=0;
	_subfeatures=0;
	_prelearn_mode=false;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::enable_prelearn_mode(const PDataAccessor da)
{
	notify("Classifier_Multinomial_NaiveBayes::init_prelearn_mode().");
	assert(_model);
	assert(da);
	assert(da->getNoOfFeatures()>0);
	assert(da->getNoOfClasses()>0);
	_model->learn(da);
	_prelearn_mode=true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	notify("Classifier_Multinomial_NaiveBayes::initialize().");
	//assert(_model);
	assert(da);
	assert(da->getNoOfFeatures()>0);
	assert(da->getNoOfClasses()>0);

	if(!_Pcd || _classes!=da->getNoOfClasses()) _Pcd.reset(new REALTYPE[da->getNoOfClasses()]);
	_classes=da->getNoOfClasses();

	if(!_index || _features!=da->getNoOfFeatures()) _index.reset(new DIMTYPE[da->getNoOfFeatures()]);
	_features=da->getNoOfFeatures();
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::train(const PDataAccessor da, const PSubset sub)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: explicit call is needed here to ensure wrapper functionality
	//       but a work-aroung can be implemented to optionally disable the call
	//       (would make sense when testing different subsets for the same da split)
	initialize(da);

	notify("Classifier_Multinomial_NaiveBayes::train().");
	assert(_model);
	assert(da);
	if(_prelearn_mode) assert(get_n()>0);
	assert(da->getNoOfFeatures()>0);
	assert(sub);
	assert(sub->get_frozen_mode()==false);
	if(_prelearn_mode) assert(sub->get_n_raw()==get_n());
	//{
	//	ostringstream sos;
	//	sos << "sub->get_n_raw()="<<sub->get_n_raw() << " da->getNoOfFeatures()="<<da->getNoOfFeatures() << std::endl;
	//	syncout::print(std::cout,sos);
	//}
	assert(sub->get_n_raw()==da->getNoOfFeatures());

	if(_prelearn_mode) _model->narrow_to(sub); // assume _n-dimensional model is pre-learned and needs to be narrow()ed only (NOTE: training data must not change since)
	else {_model->learn(da,sub); _model->denarrow();} // re-learn from scratch, training data has changed (i.e., after switch to next data split)

//	if(sub->get_d_raw()==1) // ??? single feature subsets can not be used for multinomial model based classification
//	{
//	}

	// prepare feature subset index buffering
	DIMTYPE f;
	bool b;
	for(b=sub->getFirstFeature(f),_subfeatures=0;b==true;b=sub->getNextFeature(f),_subfeatures++) {assert(_subfeatures<get_n()); _index[_subfeatures]=f;}

	_model->compute_theta(); //thetas to be used in classify() and test()

#ifdef DEBUG
	{ostringstream sos; sos << "index: "; for(f=0;f<_subfeatures;f++) sos << _index[f] << " "; syncout::print(std::cout,sos);}
#endif
	assert(_subfeatures==sub->get_d_raw());
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::classify(DIMTYPE &cls, const PPattern &pattern)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: _inverse[] _det[] and _constant[] must be pre-computed
	notify("Classifier_Multinomial_NaiveBayes::test().");
	assert(_model);
	if(_prelearn_mode) assert(get_n()>0);
	assert(_model->get_classes()>1);
	assert(_classes==_model->get_classes());
	assert(_index);
	assert(_subfeatures==_model->get_d());
	if(!_prelearn_mode) assert(_subfeatures==_model->get_n());
	assert(_subfeatures>0);

	DIMTYPE f;
	DIMTYPE c_cand;
	REALTYPE res;
	DIMTYPE wTH;
	REALTYPE *theta=&(_model->get_theta()[0]); // dirty, but this object owns _model thus no memory corruption is possible
	REALTYPE *theta_tmp;

	if(_subfeatures==1) {
		// NOTE: single features are unusable for classification because theta=1 in each class,
		//       thus log(theta)=0 and consequently Pcd[] does not depend on feature frequency
		cls=0; // consider all single features equally unusable
		return false;
	} else {
		// compute P(c|pattern) for each class
		wTH=0;
		for(c_cand=0;c_cand<_classes;c_cand++)
		{
			theta_tmp=&theta[wTH];
			res=0.0;
			for(f=0;f<_subfeatures;f++)
			{
				//{
				//	ostringstream sos; sos << "f:" << _index[f] << " ptmp[]=" << (REALTYPE)ptmp[_index[f]] << ", theta=" << _model->get_theta()[wTH+f] << ", log=" << log(_model->get_theta()[wTH+f]) << std::endl;
				//	syncout::print(std::cout,sos);
				//}
				res+=(REALTYPE)pattern[_index[f]] * log(theta_tmp[f]);
			}
			_Pcd[c_cand]=log(_model->get_Pc(c_cand))+res;
			//{
			//	ostringstream sos; sos << " Pc["<<c_cand<<"]=" << _model->get_Pc(c_cand) << ", log(Pc)=" << log(_model->get_Pc(c_cand)) << ", res="<< res << ", _Pcd[]=" << _Pcd[c_cand] << std::endl << std::endl;
			//	syncout::print(std::cout,sos);
			//}
			wTH+=_subfeatures;
		}
		// find maximum P[c|pattern]
		res=_Pcd[0]; cls=0;
		for(c_cand=1;c_cand<_classes;c_cand++) if(_Pcd[c_cand]>res) {res=_Pcd[c_cand]; cls=c_cand;}
	}
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_Multinomial_NaiveBayes<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::test(RETURNTYPE &result, const PDataAccessor da)
{
	// NOTE: mean[] and cov[] must be pre-computed using initialize()
	// NOTE: _inverse[] _det[] and _constant[] must be pre-computed
	notify("Classifier_Multinomial_NaiveBayes::test().");
	assert(_model);
	if(_prelearn_mode) assert(get_n()>0);
	assert(_model->get_classes()>1);
	assert(_classes==_model->get_classes());
	assert(da);
	if(_prelearn_mode) assert(da->getNoOfFeatures()==get_n());
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfClasses()==_classes);
	assert(_index);
	assert(_subfeatures==_model->get_d());
	if(!_prelearn_mode) assert(_subfeatures==_model->get_n());
	assert(_subfeatures>0);

	typename DATAACCESSOR::PPattern p;
	IDXTYPE s,i;
	IDXTYPE count, correct;
	DIMTYPE _features=da->getNoOfFeatures();
	DIMTYPE clstmp;

	if(_subfeatures==1) {
		// NOTE: single features are unusable for classification because theta=1 in each class,
		//       thus log(theta)=0 and consequently Pcd[] does not depend on feature frequency
		correct=0; count=1; // consider all single features equally unusable
	} else {
		bool b;
		const DIMTYPE da_test_loop=1; // to avoid mixup of get*Block() loops of different types

		count=0;
		correct=0;
		for(DIMTYPE c_test=0;c_test<_classes;c_test++)
		{
			da->setClass(c_test);
			for(b=da->getFirstBlock(TEST,p,s,da_test_loop);b==true;b=da->getNextBlock(TEST,p,s,da_test_loop)) for(i=0;i<s;i++)
			{
				if(!classify(clstmp,&p[i*_features])) return false;
				if(clstmp==c_test) correct++;
				count++;
			}
		}
	}
	assert(count>0);
	result=(RETURNTYPE)correct/(RETURNTYPE)count;
#ifdef DEBUG
	{
		ostringstream sos; sos << " result=" << result << std::endl;
		syncout::print(std::cout,sos);
	}
#endif
	return true;
}

} // namespace
#endif // FSTCLASSIFIERMULTINOMNAIVEBAYES_H ///:~


#include <Engine/MeshEdit/ARAP.h>
#include <Engine/MeshEdit/MinSurf.h>
#include <Engine/MeshEdit/Paramaterize.h>
#include <Engine/Primitive/TriMesh.h>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <set>

using namespace Ubpa;
using namespace Eigen;
using namespace std;

Ubpa::ARAP::ARAP(Ptr<TriMesh> triMesh, int iter_n, int log_verbosity)
	: heMesh(make_shared<HEMesh<V>>())
	, heMesh_orig(make_shared<HEMesh<V>>())
	, iter_n(iter_n), log_verbosity(log_verbosity)
{
	Init(triMesh, false, true);

	// copy it into triMesh_orig
}

void Ubpa::ARAP::Clear()
{
	heMesh->Clear();
	heMesh_orig->Clear();
	triMesh = nullptr;
}

std::array<pointf2, 3> Ubpa::ARAP::genEmbed(pointf3 v0, pointf3 v1, pointf3 v2) {
	vecf3 v10 = v1 - v0;
	vecf3 v20 = v2 - v0;
	vecf3 v21 = v2 - v1;

	float cos_theta = v10.cos_theta(v20);
	return std::array{ pointf2(0, 0), pointf2(v10.norm(), 0)
		, pointf2(v20.norm() * cos_theta, v20.norm() * sqrtf(1 - (cos_theta * cos_theta))) };
}

bool Ubpa::ARAP::Init(Ptr<TriMesh> triMesh, bool noClear, bool initOrig)
{
	if (!noClear)
		Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (initOrig) {
		heMesh_orig->Clear();
		heMesh_orig->Reserve(nV);
		heMesh_orig->Init(triangles);
	}

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::ARAP::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		if (initOrig) {
			heMesh_orig->Clear();
		}
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
		if (initOrig) {
			auto u = heMesh_orig->Vertices().at(i);
			u->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
		}
	}

	this->triMesh = triMesh;
	return true;
}

// calculate energy with HeMesh

//namespace Ubpa {
//	template<typename V, typename E, typename P>
//	const std::vector<E*> TPolygon<V, E, P>::BoundaryEdges() {
//		std::vector<E*> edges;
//		for (auto he : BoundaryHEs())
//			edges.push_back(he->Edge());
//		return edges;
//	}
//
//	template<typename V, typename E, typename P>
//	const std::vector<V*> TPolygon<V, E, P>::BoundaryVertice() {
//		std::vector<V*> vertices;
//		for (auto he : BoundaryHEs())
//			vertices.push_back(he->Origin());
//		return vertices;
//	}
//}

Ubpa::ARAP::V* Ubpa::ARAP::findOpp(P* trig, V* v1, V* v2) {
	decltype(v1) opp_v = nullptr;;
	if (trig != nullptr) {
		for (auto vv : trig->BoundaryVertice()) {
			if (vv != v1 && vv != v2) {
				opp_v = vv;
			}
		}
		assert(opp_v != nullptr);
	}
	return opp_v;
}

std::tuple<Vector2f, Vector2f> Ubpa::ARAP::calcXVecFull(int xi, int xj) {
	auto vi = heMesh_orig->Vertices()[xi];
	auto vj = heMesh_orig->Vertices()[xj];

	auto tri_ij = vi->HalfEdgeTo(vj)->Polygon();
	auto tri_ji = vj->HalfEdgeTo(vi)->Polygon();

	Vector2f x_vec_ij = Vector2f::Zero();
	Vector2f x_vec_ji = Vector2f::Zero();

	// calculate x vec in mapped 2d space
	auto getIdx = [](decltype(tri_ij) trig, decltype(vi) vert) {
		int i = 0;
		for (auto vv : trig->BoundaryVertice()) {
			if (vv == vert) {
				break;
			}
			else {
				i++;
			}
		}
		assert(i < 3);
		return i;
	};

	if (tri_ij != nullptr) {
		auto tri_ij_embed = genEmbed(tri_ij->BoundaryVertice()[0]->pos.cast_to<pointf3>()
			, tri_ij->BoundaryVertice()[1]->pos.cast_to<pointf3>()
			, tri_ij->BoundaryVertice()[2]->pos.cast_to<pointf3>());

		int vi_idx = getIdx(tri_ij, vi);
		int vj_idx = getIdx(tri_ij, vj);
		x_vec_ij = { (tri_ij_embed[vi_idx] - tri_ij_embed[vj_idx])[0]
						 , (tri_ij_embed[vi_idx] - tri_ij_embed[vj_idx])[1] };

		if (log_verbosity > 40) {
			printf("- embed_ij: ");
			print(tri_ij_embed);
			printf("- x_vec_ij: \n");
			cout << x_vec_ij << endl;
		}
	}

	if (tri_ji != nullptr) {
		auto tri_ji_embed = genEmbed(tri_ji->BoundaryVertice()[0]->pos.cast_to<pointf3>()
			, tri_ji->BoundaryVertice()[1]->pos.cast_to<pointf3>()
			, tri_ji->BoundaryVertice()[2]->pos.cast_to<pointf3>());

		int vi_idx = getIdx(tri_ji, vi);
		int vj_idx = getIdx(tri_ji, vj);
		x_vec_ji = { (tri_ji_embed[vi_idx] - tri_ji_embed[vj_idx])[0]
						 , (tri_ji_embed[vi_idx] - tri_ji_embed[vj_idx])[1] };

		if (log_verbosity > 40) {
			printf("- embed_ji: ");
			print(tri_ji_embed);
			printf("- x_vec_ji: \n");
			cout << x_vec_ji << endl;
		}
	}

	return std::make_tuple(x_vec_ij, x_vec_ji);

}

// x_vec, cotij, cotji
std::tuple<Vector2f, double, double> Ubpa::ARAP::calcXVec(int xi, int xj) {
	auto vi = heMesh_orig->Vertices()[xi];
	auto vj = heMesh_orig->Vertices()[xj];

	auto tri_ij = vi->HalfEdgeTo(vj)->Polygon();
	auto tri_ji = vj->HalfEdgeTo(vi)->Polygon();

	if (log_verbosity > 50) {
		cout << "- tri_ij: ";
		print(tri_ij);

		cout << "- tri_ji: ";
		print(tri_ji);
	}

	// mapped NaN (inf) to zero
	auto getCtgf = [](float cosine) {
		return (cosine == 1 || cosine == -1) ? 0 : (cosine / (sqrtf(1 - cosine * cosine)));
	};

	decltype(vj) opp_v_ji = findOpp(tri_ji, vi, vj);
	decltype(vj) opp_v_ij = findOpp(tri_ij, vi, vj);

	double cos_theta_ij = opp_v_ij != nullptr ? (vi->pos - opp_v_ij->pos).cos_theta(vj->pos - opp_v_ij->pos) : 1;
	double cos_theta_ji = opp_v_ji != nullptr ? (vi->pos - opp_v_ji->pos).cos_theta(vj->pos - opp_v_ji->pos) : 1;

	double cot_theta_ij = getCtgf(cos_theta_ij);
	double cot_theta_ji = getCtgf(cos_theta_ji);

	// calculate x vec in mapped 2d space
	auto getIdx = [](decltype(tri_ij) trig, decltype(vi) vert) {
		int i = 0;
		for (auto vv : trig->BoundaryVertice()) {
			if (vv == vert) {
				break;
			} else {
				i++;
			}
		}
		assert(i < 3);
		return i;
	};

	decltype(tri_ij) trig;
	// only one trig necessary
	if (tri_ij == nullptr) {
		trig = tri_ji;
	} else {
		trig = tri_ij;
	}

	if (log_verbosity > 50) {
		printf("- embed_input: ");
		print(trig);
	}

	auto tri_embed = genEmbed(trig->BoundaryVertice()[0]->pos.cast_to<pointf3>()
				, trig->BoundaryVertice()[1]->pos.cast_to<pointf3>()
				, trig->BoundaryVertice()[2]->pos.cast_to<pointf3>());

	if (log_verbosity > 40) {
		printf("- embed: ");
		print(tri_embed);
	}

	int vi_idx = getIdx(trig, vi);
	int vj_idx = getIdx(trig, vj);
	Vector2f x_vec = { (tri_embed[vi_idx] - tri_embed[vj_idx])[0]
					 , (tri_embed[vi_idx] - tri_embed[vj_idx])[1] };

	return std::make_tuple(x_vec, cot_theta_ij, cot_theta_ji);
}

void Ubpa::ARAP::UniformPara() {
	// First, detect and fix boundary
	random_set<V*> boundary_points;
	random_set<V*> inner_points;

	auto boundaries = this->heMesh->Boundaries();
	if (boundaries.size() != 1) {
		cout << "ERROR::Parameterize::DoPara:" << endl
			<< "\t" << "got boundaries = " << boundaries.size()
			<< " (expect 1)" << endl;
		return;
	}

	for (auto v : boundaries[0]) {
		boundary_points.insert(v->Origin());
	}

	for (auto v : heMesh->Vertices()) {
		if (!boundary_points.contains(v)) {
			inner_points.insert(v);
		}
	}

	//const float boost_factor = 100;
	const float boost_factor = 1;
	// Fix our boundary

	int points_total = boundary_points.size();
	float step = 4.0f / points_total;

	float curr = 0;
	for (auto v : boundary_points) {
		vecf3 new_pos;
		if (curr >= 0 && curr < 1) {
			new_pos[0] = curr;
			new_pos[1] = new_pos[2] = 0;
		} else if (curr >= 1 && curr < 2) {
			new_pos[0] = 1;
			new_pos[1] = curr - 1;
			new_pos[2] = 0;
		} else if (curr >= 2 && curr < 3) {
			new_pos[0] = 1 - (curr - 2);
			new_pos[1] = 1;
			new_pos[2] = 0;
		} else { // curr >= 3; remember to cut off as fp precision is an issue
			new_pos[0] = 0;
			new_pos[1] = curr > 4 ? 4 : 4 - curr;
			new_pos[2] = 0;
		}
		v->pos = new_pos * boost_factor;
		curr += step;
	}

	// Build sparse matrix
	size_t n = inner_points.size();
	SparseMatrix<float> coeff_mat(n, n);
	coeff_mat.setZero();
	VectorXf b_vec_x = VectorXf::Zero(n);
	VectorXf b_vec_y = VectorXf::Zero(n);
	VectorXf b_vec_z = VectorXf::Zero(n);

	cout << "coeff mat build start" << endl;

	int current_row = 0;
	for (auto v : inner_points) {
		// vidx CERTAINLY follows order (and it's redundant)
		size_t vidx = inner_points.idx(v);
		auto adj = v->AdjVertices();
		size_t degree = v->Degree();
		for (auto adjv : adj) {
				// check type
			if (boundary_points.contains(adjv)) { // this set is usually smaller
				b_vec_x(current_row) += (1.0f / degree) * adjv->pos[0];
				b_vec_y(current_row) += (1.0f / degree) * adjv->pos[1];
				b_vec_z(current_row) += (1.0f / degree) * adjv->pos[2];
			} else { // inner
				assert(inner_points.contains(adjv));
				size_t adjidx = inner_points.idx(adjv);
				// todo add assert = 0
				coeff_mat.insert(current_row, adjidx) = -1.0f / degree;
			}
		}

		// add itself
		// todo add assert
		coeff_mat.insert(current_row, vidx) = 1;
		current_row++;
	}

	cout << "coeff mat build complete" << endl;

	// Solve
	SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

	cout << "begin makeCompressed()" << endl;
	coeff_mat.makeCompressed();

	cout << "begin compute()" << endl;
	solver.compute(coeff_mat);
	if (solver.info() != Eigen::Success) {
		cout << "solver: decomposition was not successful." << endl;
		return;
	}

	cout << "begin solve() for x" << endl;
	VectorXf res_x = solver.solve(b_vec_x);

	cout << "begin solve() for y" << endl;
	VectorXf res_y = solver.solve(b_vec_y);

	cout << "begin solve() for z" << endl;
	VectorXf res_z = solver.solve(b_vec_z);

	// Update vertex coordinates
	for (int i = 0; i < n; i++) {
		// find the corresponding point
		auto v = inner_points[i];
		vecf3 new_pos = { res_x(i), res_y(i), res_z(i) }; // works?

		//cout << new_pos << endl;
		v->pos = new_pos;
	}

}

void Ubpa::ARAP::print(Ubpa::ARAP::V* v) {
	cout << "V: (" << v->pos[0] << ", " << v->pos[1] << ", " << v->pos[2] << ")";
}
void Ubpa::ARAP::print(Ubpa::ARAP::P* p) {
	if (p == nullptr) {
		cout << "P: nullptr" << endl;
		return;
	}
	cout << "P: [";
	print(p->BoundaryVertice()[0]);
	cout << ", ";
	print(p->BoundaryVertice()[1]);
	cout << ", ";
	print(p->BoundaryVertice()[2]);
	cout << "]" << endl;
}

void Ubpa::ARAP::print(pointf2& v) {
	cout << "V: (" << v[0] << ", " << v[1] << ")";
}

void Ubpa::ARAP::print(std::array<pointf2, 3>& p) {
	cout << "P: [";
	print(p[0]);
	cout << ", ";
	print(p[1]);
	cout << ", ";
	print(p[2]);
	cout << "]" << endl;
}

double Ubpa::ARAP::calcEnergy(vector<Matrix2f> L_t)
{
	double energy = 0;

	auto polys = heMesh->Polygons();
	for (int poly_id = 0; poly_id < polys.size(); poly_id++) {
		if (log_verbosity > 50) {
			print(polys[poly_id]);
		}

		auto hes = polys[poly_id]->BoundaryHEs();
		for (auto he : hes) {

			double cotij;
			Vector2f x_vec;
			std::tie(x_vec, cotij, std::ignore) = calcXVec(heMesh->Index(he->Origin()), heMesh->Index(he->End()));

			if (log_verbosity > 50) {
				cout << x_vec << endl;
			}

			Vector2f u_vec = { he->Origin()->pos[0] - he->End()->pos[0] , he->Origin()->pos[1] - he->End()->pos[1] };

			double energy_component = cotij * (u_vec - L_t[poly_id] * x_vec).squaredNorm();
			if (log_verbosity > 50) {
				printf("* energy += %lf\n", energy_component);
			}
			energy += energy_component;
		}
	}

	energy /= 2;

	return energy;
}

void Ubpa::ARAP::DoARAP()
{
	cout << "DoARAP() called." << endl;

	// initialize 2d mapping
	UniformPara();
	//for (auto v : heMesh->Vertices()) {
	//	v->pos[2] = 0;
	//}

	for (auto v : heMesh->Vertices()) {
		assert(v->pos[2] == 0);
	}

	// envvars
	int total_polys = heMesh->NumPolygons();
	auto polys = heMesh->Polygons();
	auto polys_orig = heMesh_orig->Polygons();

	// initialize L_t
	vector<Matrix2f> L_t;
	for (int i = 0; i < total_polys; i++) {
		L_t.push_back(Eigen::Matrix2f::Identity());
		//L_t.push_back(Eigen::Matrix2f::Zero());
	}

	printf("Start with energy=%lf\n", calcEnergy(L_t));

	for (int iter_count = 0; iter_count < iter_n; iter_count++) {
		{
			// local phase, update L_t
			for (int poly_id = 0; poly_id < total_polys; poly_id++) {
				// calculate S_t(u) in place of J_t(u)
				Matrix2f S_u = Eigen::Matrix2f::Zero();

				auto verts = polys[poly_id]->BoundaryVertice();
				auto verts_orig = polys_orig[poly_id]->BoundaryVertice();
				// todo assuming counterclockwise
				// actually okay for both

				for (int i = 0; i < 3; i++) {
					// cot = cos / sin
					int dest_angle = (i + 2) % 3;
					double cos_theta;
					auto embed = genEmbed(verts_orig[0]->pos.cast_to<pointf3>()
						, verts_orig[1]->pos.cast_to<pointf3>()
						, verts_orig[2]->pos.cast_to<pointf3>());

					if (dest_angle == 0) {
						cos_theta = (embed[2] - embed[0]).cos_theta(embed[1] - embed[0]);
					}
					else if (dest_angle == 1) {
						cos_theta = (embed[0] - embed[1]).cos_theta(embed[2] - embed[1]);
					}
					else {
						cos_theta = (embed[0] - embed[2]).cos_theta(embed[1] - embed[2]);
					}

					double cot_theta = cos_theta / (sqrt(1 - cos_theta * cos_theta) + 1e-5);  // todo check 1e-5

					Vector2f u_vec = { verts[i]->pos[0] - verts[(i + 1) % 3]->pos[0] , verts[i]->pos[1] - verts[(i + 1) % 3]->pos[1] };
					Vector2f x_vec = { embed[i][0] - embed[(i + 1) % 3][0] , embed[i][1] - embed[(i + 1) % 3][1] };

					S_u += cot_theta * (u_vec * x_vec.transpose());
				}

				JacobiSVD<Matrix2f> svd(S_u, ComputeFullU | ComputeFullV);
				assert(svd.singularValues()(0) >= 0 && svd.singularValues()(1) >= 0);

				// the ARAP case
				Matrix2f res = svd.matrixU() * svd.matrixV().transpose();
				L_t[poly_id] = res;
			}
		}
		printf("[%d] local iteration done, energy=%lf\n", iter_count, calcEnergy(L_t));
		// global iteration
		{
			// anchors containing vertices index
			std::set<int> anchors;
			//anchors.insert(0);
			//anchors.insert(1);

			int total_vertices = heMesh->NumVertices();
			int vertices_unknown = total_vertices - anchors.size();
			//assert(total_polys == vertices_unknown + 2);

			std::unordered_map<int, int> index_map;
			{
				int cur_row = 0;
				for (int i = 0; i < total_vertices; i++) {
					if (anchors.count(i) > 0) {
						continue;
					}
					else {
						index_map[i] = cur_row;
						cur_row++;
					}
				}
				assert(cur_row == vertices_unknown);
			}

			Eigen::SparseMatrix<float> coeff_mat(vertices_unknown, vertices_unknown);
			Eigen::VectorXf b_vec_x = VectorXf::Zero(vertices_unknown);
			Eigen::VectorXf b_vec_y = VectorXf::Zero(vertices_unknown);
			coeff_mat.setZero();

			int current_row = 0;
			for (int i = 0; i < total_vertices; i++) {
				if (anchors.count(i) > 0) {
					continue;
				}

				// prepare coefficient
				// calculate cos_theta_ij
				auto v = heMesh->Vertices()[i];
				//cout << "Processing vertice: " << v << endl;

				for (auto adjv : v->AdjVertices()) {
					int j = heMesh->Index(adjv);

					//auto he_ji = v->HalfEdgeTo(adjv);
					//auto he_ij = adjv->HalfEdgeTo(v);
					auto he_ji = adjv->HalfEdgeTo(v);
					auto he_ij = v->HalfEdgeTo(adjv);

					auto tri_ji = he_ji->Polygon();
					auto tri_ij = he_ij->Polygon();

					double ctg_ij, ctg_ji;

					if (log_verbosity > 50) {
						cout << "* i: ";
						print(v);
						cout << endl;

						cout << "* j: ";
						print(adjv);
						cout << endl;
					}

					std::tie(std::ignore, ctg_ij, ctg_ji) = calcXVec(heMesh->Index(v), heMesh->Index(adjv));

					if (log_verbosity > 50) {
						printf("* ctg_ij=%lf , ctg_ji=%lf\n", ctg_ij, ctg_ji);
					}

					// the u stuff
					if (anchors.count(j) > 0) {
						// put into b
						b_vec_x(current_row) += (ctg_ij + ctg_ji) * adjv->pos[0];
						b_vec_y(current_row) += (ctg_ij + ctg_ji) * adjv->pos[1];
					}
					else {
						coeff_mat.coeffRef(current_row, index_map[j]) += -(ctg_ij + ctg_ji);
					}

					coeff_mat.coeffRef(current_row, index_map[i]) += (ctg_ij + ctg_ji);

					// the x stuff

					Vector2f x_vec_ij, x_vec_ji;
					std::tie(x_vec_ij, x_vec_ji) = calcXVecFull(heMesh->Index(v), heMesh->Index(adjv));

					Vector2f rhs = Vector2f::Zero();

					if (tri_ij != nullptr) {
						rhs += ctg_ij * L_t[heMesh->Index(he_ij->Polygon())] * x_vec_ij;
					}

					if (tri_ji != nullptr) {
						rhs += ctg_ji * L_t[heMesh->Index(he_ji->Polygon())] * x_vec_ji;
					}

					if (log_verbosity > 50) {
						cout << "rhs: " << endl;
						cout << rhs << endl;
					}

					b_vec_x(current_row) += rhs(0);
					b_vec_y(current_row) += rhs(1);
				}
				current_row++;
			}

			// Solve
			SparseQR<SparseMatrix<float>, COLAMDOrdering<int>> solver;

			cout << "begin makeCompressed()" << endl;
			coeff_mat.makeCompressed();

			if (log_verbosity > 50) {
				cout << coeff_mat << endl;
			}

			cout << "begin compute()" << endl;
			solver.compute(coeff_mat);
			if (solver.info() != Eigen::Success) {
				cout << "solver: decomposition was not successful." << endl;
				return;
			}

			if (log_verbosity > 50) {
				cout << "b_vec_x: " << endl;
				cout << b_vec_x << endl;

				cout << "b_vec_y: " << endl;
				cout << b_vec_y << endl;
			}

			VectorXf x_res = solver.solve(b_vec_x);
			cout << "error: " << (coeff_mat * x_res - b_vec_x).squaredNorm() << " (x_res)" << endl;

			VectorXf y_res = solver.solve(b_vec_y);
			cout << "error: " << (coeff_mat * y_res - b_vec_y).squaredNorm() << " (y_res)" << endl;;

			// write back
			for (int i = 0; i < total_vertices; i++) {
				if (anchors.count(i) > 0) {
					continue;
				}
				else {
					heMesh->Vertices()[i]->pos = { x_res(index_map[i]), y_res(index_map[i]), 0 };
				}
			}

		}
		printf("[%d] global iteration done, energy=%lf\n", iter_count, calcEnergy(L_t));
	}
}

bool Ubpa::ARAP::Run()
{
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::ARAP::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	DoARAP();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	//vector<pointf2> texcoords;
	//for (auto v : heMesh->Vertices())
	//	texcoords.push_back(v->pos.cast_to<pointf2>());

	//triMesh->Update(texcoords);

	return true;
}


/*
 * Copyright 2015-2017 Guillermo Frontera <guillermo.frontera@upm.es>
 *
 * 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 "graph_pathfinder.h"

#include <unordered_set>
#include <unordered_map>

#include <boost/heap/fibonacci_heap.hpp>

#include <log/logger.h>
#include <exc/exception.h>

using namespace ues::pf;

const std::string component_name = "Pathfinder";

namespace
{

/** The state type contains a point index and the estimated cost of such point.*/
template<unsigned short N>
struct state
{
    typename visibility_graph<N>::size_type point_index;
    ues::math::numeric_type accumulated_cost;
    ues::math::numeric_type estimated_cost;
};

/** The state_comparator class contains the required comparator method so the
 * states are sorted in the desired order in the priority queue. */
template<unsigned short N>
struct state_comparator
{
    bool operator() ( const state<N> & one, const state<N> & two ) const noexcept
    {
        return one.estimated_cost > two.estimated_cost;
    }
};

/** The priority_queue stores and keeps a sorted list of state objects. */
template<unsigned short N>
using priority_queue = boost::heap::fibonacci_heap< state<N>, boost::heap::compare< state_comparator<N> > >;
/** The index_set contains all the points whose cost has already been computed. */
template<unsigned short N>
using index_set = std::unordered_set< typename visibility_graph<N>::size_type >;
/** Stores the handles that allow modifying the states of the priority queue. */
template<unsigned short N>
using handle_storage = std::unordered_map< typename visibility_graph<N>::size_type, typename priority_queue<N>::handle_type >;
/** Stores the point from which current point has been reached. */
template<unsigned short N>
using parent_point = std::vector< typename visibility_graph<N>::size_type >;

}

template<unsigned short N>
path<N> graph_pathfinder<N>::find_path ( const ues::geom::point<N> & origin,
                                         const ues::geom::point<N> & target )
{
    ues::log::logger lg;

    if ( lg.min_level() <= ues::log::TRACE_LVL )
    {
        ues::log::event e ( ues::log::TRACE_LVL, component_name, "Starting pathfinding" );
        e.message() << *graph << '\n';
        lg.record ( std::move ( e ) );
    }

    // Get indices to the origin and target points.
    const typename visibility_graph<N>::size_type origin_index = graph->point_to_index ( origin );
    const typename visibility_graph<N>::size_type target_index = graph->point_to_index ( target );

    // Declare necesary data structures.
    priority_queue<N> frontier;
    handle_storage<N> handles;
    index_set<N> explored;
    parent_point<N> parents ( graph->size() );
    parents[ origin_index ] = origin_index;

    // Add initial node to the priority queue.
    typename priority_queue<N>::handle_type handle = frontier.push ( { origin_index, 0, origin.distance_to ( target ) } );
    handles.insert ( { origin_index, handle } );

    while ( !frontier.empty() )
    {
        state<N> node = frontier.top();
        frontier.pop();
        handles.erase ( node.point_index );

        if ( node.point_index == target_index )
        {
            path<N> reverse_result;
            typename visibility_graph<N>::size_type current_point = target_index;
            while ( current_point != parents[ current_point ] )
            {
                reverse_result.push_back ( graph->index_to_point ( current_point ) );
                current_point = parents[ current_point ];
            }
            reverse_result.push_back ( origin );
            path<N> result ( reverse_result.rbegin(), reverse_result.rend() );

            if ( lg.min_level() <= ues::log::DEBUG_LVL )
            {
                ues::log::event e ( ues::log::DEBUG_LVL, component_name, "Found path" );
                e.message() << "Cost of the path: " << node.accumulated_cost << '\n';
                e.message() << result << '\n';
                lg.record ( std::move ( e ) );
            }

            return std::move ( result );
        }

        explored.insert ( node.point_index );

        for ( typename visibility_graph<N>::size_type p : graph->adjacents ( node.point_index ) )
        {
            if ( explored.find ( p ) == explored.end() )
            {
                // Get the cost of the edge from node.point_index to p.
                ues::math::numeric_type last_edge;
                graph->check_visibility ( node.point_index, p, last_edge );

                // Generate a node with the cost of getting to p from node.point_index.
                state<N> new_node;
                new_node.point_index = p;
                new_node.accumulated_cost = node.accumulated_cost + last_edge;
                ues::math::numeric_type heuristic_cost = graph->index_to_point ( p ).distance_to ( target );
                new_node.estimated_cost = new_node.accumulated_cost + heuristic_cost;

                typename handle_storage<N>::const_iterator it = handles.find ( p );
                if ( it == handles.end() )
                {
                    handle = frontier.push ( new_node );
                    handles.insert ( { p, handle } );
                    parents[p] = node.point_index;
                }
                else if ( new_node.estimated_cost < ( *it->second ).estimated_cost )
                {
                    ( *it->second ).estimated_cost = new_node.estimated_cost;
                    ( *it->second ).accumulated_cost = new_node.accumulated_cost;
                    frontier.decrease ( it->second );
                    parents[p] = node.point_index;
                }
            }
        }
    }

    throw ues::exc::exception ( "Unable to find a path between points", UES_CONTEXT );
}

template<unsigned short N>
graph_pathfinder<N>::graph_pathfinder ( std::shared_ptr< visibility_graph<N> > graph )
    : graph ( std::move ( graph ) )
{
    if ( this->graph.get() == nullptr )
        throw ues::exc::exception ( "Provided graph cannot be null", UES_CONTEXT );
}

// Instantiate the templates in this translation unit, just once.
template class ues::pf::graph_pathfinder<2>;
template class ues::pf::graph_pathfinder<3>;


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/matrix_serialization.h>
#include <mrpt/opengl/CEllipsoid.h>
#include <mrpt/serialization/CArchive.h>
#include <Eigen/Dense>
#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CEllipsoid, CRenderizableDisplayList, mrpt::opengl)

/*---------------------------------------------------------------
							render
  ---------------------------------------------------------------*/
void CEllipsoid::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
	MRPT_START

	const size_t dim = m_cov.cols();

	if (m_eigVal(0, 0) != 0.0 && m_eigVal(1, 1) != 0.0 &&
		(dim == 2 || m_eigVal(2, 2) != 0.0) && m_quantiles != 0.0)
	{
		glEnable(GL_BLEND);
		checkOpenGLError();
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
		checkOpenGLError();
		glLineWidth(m_lineWidth);
		checkOpenGLError();

		if (dim == 2)
		{
			glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			// ---------------------
			//     2D ellipse
			// ---------------------

			/* Equivalent MATLAB code:
			 *
			 * q=1;
			 * [vec val]=eig(C);
			 * M=(q*val*vec)';
			 * R=M*[x;y];
			 * xx=R(1,:);yy=R(2,:);
			 * plot(xx,yy), axis equal;
			 */

			double ang;
			unsigned int i;

			// Compute the new vectors for the ellipsoid:
			auto M = CMatrixDouble(m_eigVal.asEigen() * m_eigVec.transpose());
			M *= double(m_quantiles);

			glBegin(GL_LINE_LOOP);

			// Compute the points of the 2D ellipse:
			for (i = 0, ang = 0; i < m_2D_segments;
				 i++, ang += (M_2PI / m_2D_segments))
			{
				double ccos = cos(ang);
				double ssin = sin(ang);

				const float x = ccos * M(0, 0) + ssin * M(1, 0);
				const float y = ccos * M(0, 1) + ssin * M(1, 1);

				glVertex2f(x, y);
			}  // end for points on ellipse

			glEnd();

			// 2D: Save bounding box:
			const double max_radius =
				m_quantiles * std::max(m_eigVal(0, 0), m_eigVal(1, 1));
			m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);

			glEnable(GL_LIGHTING);
		}
		else
		{
			// ---------------------
			//    3D ellipsoid
			// ---------------------
			GLfloat mat[16];

			//  A homogeneous transformation matrix, in this order:
			//
			//     0  4  8  12
			//     1  5  9  13
			//     2  6  10 14
			//     3  7  11 15
			//
			mat[3] = mat[7] = mat[11] = 0;
			mat[15] = 1;
			mat[12] = mat[13] = mat[14] = 0;

			mat[0] = m_eigVec(0, 0);
			mat[1] = m_eigVec(1, 0);
			mat[2] = m_eigVec(2, 0);  // New X-axis
			mat[4] = m_eigVec(0, 1);
			mat[5] = m_eigVec(1, 1);
			mat[6] = m_eigVec(2, 1);  // New X-axis
			mat[8] = m_eigVec(0, 2);
			mat[9] = m_eigVec(1, 2);
			mat[10] = m_eigVec(2, 2);  // New X-axis

			GLUquadricObj* obj = gluNewQuadric();
			checkOpenGLError();

			if (!m_drawSolid3D)
				glDisable(GL_LIGHTING);  // Disable lights when drawing lines

			gluQuadricDrawStyle(obj, m_drawSolid3D ? GLU_FILL : GLU_LINE);

			glPushMatrix();
			glMultMatrixf(mat);
			glScalef(
				m_eigVal(0, 0) * m_quantiles, m_eigVal(1, 1) * m_quantiles,
				m_eigVal(2, 2) * m_quantiles);

			gluSphere(obj, 1, m_3D_segments, m_3D_segments);
			checkOpenGLError();

			glPopMatrix();

			gluDeleteQuadric(obj);
			checkOpenGLError();

			// 3D: Save bounding box:
			const double max_radius =
				m_quantiles *
				std::max(
					m_eigVal(0, 0), std::max(m_eigVal(1, 1), m_eigVal(2, 2)));
			m_bb_min = mrpt::math::TPoint3D(-max_radius, -max_radius, 0);
			m_bb_max = mrpt::math::TPoint3D(max_radius, max_radius, 0);
			// Convert to coordinates of my parent:
			m_pose.composePoint(m_bb_min, m_bb_min);
			m_pose.composePoint(m_bb_max, m_bb_max);
		}

		glDisable(GL_BLEND);

		glEnable(GL_LIGHTING);
	}
	MRPT_END_WITH_CLEAN_UP(cout << "Covariance matrix leading to error is:"
								<< endl
								<< m_cov << endl;);
#endif
}

uint8_t CEllipsoid::serializeGetVersion() const { return 1; }
void CEllipsoid::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	out << m_cov << m_drawSolid3D << m_quantiles << (uint32_t)m_2D_segments
		<< (uint32_t)m_3D_segments << m_lineWidth;
}

void CEllipsoid::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			uint32_t i;
			readFromStreamRender(in);
			if (version == 0)
			{
				CMatrixF c;
				in >> c;
				m_cov = c.cast_double();
			}
			else
			{
				in >> m_cov;
			}

			in >> m_drawSolid3D >> m_quantiles;
			in >> i;
			m_2D_segments = i;
			in >> i;
			m_3D_segments = i;
			in >> m_lineWidth;

			// Update cov. matrix cache:
			setCovMatrix(m_cov);
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	CRenderizableDisplayList::notifyChange();
}

bool quickSolveEqn(double a, double b_2, double c, double& t)
{
	double delta = square(b_2) - a * c;
	if (delta == 0)
		return (t = -b_2 / a) >= 0;
	else if (delta > 0)
	{
		delta = sqrt(delta);
		if ((t = (-b_2 - delta) / a) >= 0)
			return true;
		else
			return (t = (-b_2 + delta) / a) >= 0;
	}
	else
		return false;
}

bool CEllipsoid::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
	if (m_cov.rows() != 3) return false;
	TLine3D lin, lin2;
	createFromPoseX((o - this->m_pose).asTPose(), lin);
	lin.unitarize();  // By adding this line, distance from any point of the
	// line to its base is exactly equal to the "t".
	for (size_t i = 0; i < 3; i++)
	{
		lin2.pBase[i] = 0;
		lin2.director[i] = 0;
		for (size_t j = 0; j < 3; j++)
		{
			double vji = m_eigVec(j, i);
			lin2.pBase[i] += vji * lin.pBase[j];
			lin2.director[i] += vji * lin.director[j];
		}
	}
	double a = 0, b_2 = 0, c = -square(m_quantiles);
	for (size_t i = 0; i < 3; i++)
	{
		double ev = m_eigVal(i, i);
		a += square(lin2.director[i] / ev);
		b_2 += lin2.director[i] * lin2.pBase[i] / square(ev);
		c += square(lin2.pBase[i] / ev);
	}
	return quickSolveEqn(a, b_2, c, dist);
}

void CEllipsoid::setCovMatrix(
	const mrpt::math::CMatrixDouble& m, int resizeToSize)
{
	MRPT_START

	ASSERT_(m.cols() == m.rows());
	ASSERT_(
		m.rows() == 2 || m.rows() == 3 ||
		(resizeToSize > 0 && (resizeToSize == 2 || resizeToSize == 3)));

	m_cov = m;
	if (resizeToSize > 0 && resizeToSize < (int)m.rows())
		m_cov.setSize(resizeToSize, resizeToSize);

	if (m_cov == m_prevComputedCov) return;  // Done.

	m_prevComputedCov = m_cov;

	CRenderizableDisplayList::notifyChange();

	// Handle the special case of an ellipsoid of volume = 0
	const double d = m_cov.det();
	if (d == 0 || d != d)  // Note: "d!=d" is a great test for invalid numbers,
	// don't remove!
	{
		// All zeros:
		m_eigVec.setZero(3, 3);
		m_eigVal.setZero(3, 3);
	}
	else
	{
		// Not null matrix: compute the eigen-vectors & values:
		std::vector<double> eigvals;
		if (m_cov.eig_symmetric(m_eigVec, eigvals))
		{
			// Do the scale at render to avoid recomputing the m_eigVal for
			// different m_quantiles
			m_eigVal.setDiagonal(eigvals);
			m_eigVal.array() = m_eigVal.array().sqrt().matrix();
		}
		else
		{
			m_eigVec.setZero(3, 3);
			m_eigVal.setZero(3, 3);
		}
	}

	MRPT_END
}

void CEllipsoid::setCovMatrix(
	const mrpt::math::CMatrixFloat& m, int resizeToSize)
{
	CRenderizableDisplayList::notifyChange();
	setCovMatrix(CMatrixDouble(m), resizeToSize);
}

/** Evaluates the bounding box of this object (including possible children) in
 * the coordinate frame of the object parent. */
void CEllipsoid::getBoundingBox(
	mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
	bb_min = m_bb_min;
	bb_max = m_bb_max;
}