/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101111    E. Iorfida        File created.
 *      101111    E. Iorfida        Implementation of all the equations up to the Newton method.
 *      101117    E. Iorfida        Velocities computations added.
 *      101126    E. Iorfida        Get/set codes deleted.
 *      101206    E. Iorfida        LambertTargetingElements class deleted,
 *                                  added setInitialState, modified punctuation. Set single
 *                                  variables, change variables names in more understandable ones.
 *      101209    E. Iorfida        Corrected some coding errors.
 *      101213    E. Iorfida        Deleted lambertAngle, added numberOfRevolution, modified
 *                                  implementation.
 *      101214    E. Iorfida        Implementation only for the case with numberOfRevolution = 0.
 *      110113    E. Iorfida        Added necessary elements to build pointer-to-member-function
 *                                  to RootFinderAlgorithms and NewtonRaphsonMethod classes.
 *      110124    E. Iorfida        Added necessary piece of code to be able to use the last
 *                                  version of Newton-Raphson code.
 *      110126    E. Iorfida        Initialized member functions.
 *      110130    J. Melman         Simplified variable names, e.g., 'normOfdeletVelocityVector'
 *                                  became 'speed'. Requested references to specific formulas. Also
 *                                  corrected 'transverse' to 'transverse'. Simplified computation
 *                                  of radial unit vector. Corrected computation of transverse
 *                                  heliocentric velocity.
 *      110201    E. Iorfida        Added pointerToCelestialBody and modified variable names (from
 *                                  heliocentric, to inertial). Added patch for negative case of
 *                                  initialLambertGuess. Added equations references.
 *      110206    E. Iorfida        Added unique function for Newton-Raphson method. Added
 *                                  computeAbsoluteValue to the initialLambertGuess for
 *                                  non-converging cases.
 *      110208    E. Iorfida        Added CartesianPositionElements objects as input and
 *                                  CartesianVelocityElements objects as output.
 *      110418    E. Iorfida        Added a new normal plane that take into account the case of two
 *                                  parallel position vector (with a relative angle of 180
 *                                  degrees). Better defined the pointers to the output
 *                                  CartesianVelocityElements.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120619    T. Secretin       Converted to free functions. Added Izzo's approach.
 *      120704    P. Musegaas       Various small changes during code check.
 *      120713    P. Musegaas       Changed tolerance to relative tolerance in Gooding's rootfinder.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *
 *    References
 *      Battin, R.H. An Introduction to the Mathematics and Methods of Astrodynamics,
 *          AIAA Education Series, 1999.
 *      Izzo, D. lambert_problem.h, keptoolbox.
 *      Gooding, R.H. A procedure for the solution of Lambert's orbital boundary-value problem,
 *          Celestial Mechanics and Dynamical Astronomy, 48:145-165, 1990.
 *
 *    Notes
 *
 */

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

#include <boost/bind.hpp>
#include <boost/exception/all.hpp>
#include <boost/make_shared.hpp>
#include <boost/math/special_functions.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

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

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"
#include "Tudat/Mathematics/BasicMathematics/functionProxy.h"

namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Solve Lambert Problem using Izzo's algorithm.
void solveLambertProblemIzzo( const Eigen::Vector3d& cartesianPositionAtDeparture,
                              const Eigen::Vector3d& cartesianPositionAtArrival,
                              const double timeOfFlight,
                              const double gravitationalParameter,
                              Eigen::Vector3d& cartesianVelocityAtDeparture,
                              Eigen::Vector3d& cartesianVelocityAtArrival,
                              const bool isRetrograde,
                              const double convergenceTolerance,
                              const unsigned int maximumNumberOfIterations )
{
    // Sanity check for specified time-of-flight.
    if ( timeOfFlight <= 0.0 )
    {
        // Define error message.
        std::stringstream errorMessage;
        errorMessage << "Specified time-of-flight must be strictly positive.\n"
                     << "Specified time-of-flight: " << timeOfFlight << " days." << std::endl;

        // Throw exception.
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error( errorMessage.str( ) ) ) );
    }

    // Compute normalizing values.
    const double distanceNormalizingValue = cartesianPositionAtDeparture.norm( );
    const double velocityNormalizingValue = std::sqrt( gravitationalParameter /
                                                       distanceNormalizingValue );
    const double timeNormalizingValue = distanceNormalizingValue / velocityNormalizingValue;

    // Compute transfer geometry parameters in adimensional units.
    // Cosine of transfer angle.
    const double cosineOfTransferAngle =
            cartesianPositionAtDeparture.dot( cartesianPositionAtArrival )
            / (distanceNormalizingValue * cartesianPositionAtArrival.norm( ) );

    // Normalized Cartesian position at arrival.
    const double normalizedRadiusAtArrival = cartesianPositionAtArrival.norm( )
            / distanceNormalizingValue;

    // Chord.
    const double chord = std::sqrt( 1.0 + normalizedRadiusAtArrival
                                    * ( normalizedRadiusAtArrival - 2.0 * cosineOfTransferAngle ) );

    // Semi-perimeter.
    const double semiPerimeter = ( 1.0 + normalizedRadiusAtArrival + chord ) / 2.0;

    // Assuming a prograde motion, determine whether the transfer corresponds to the long- or the
    // short-way solution: longway if x1*y2 - x2*y1 < 0.
    bool isLongway = false;
    if ( cartesianPositionAtDeparture.x( ) * cartesianPositionAtArrival.y( )
         - cartesianPositionAtDeparture.y( ) * cartesianPositionAtArrival.x( ) < 0.0 )
    {
        isLongway = true;
    }

    // If retrograde is true, switch longway flag.
    if ( isRetrograde )
    {
        isLongway = !isLongway;
    }

    // Semi-major axis of the minimum energy ellipse.
    const double semiMajorAxisOfTheMinimumEnergyEllipse = semiPerimeter / 2.0;

    // Transfer angle.
    double transferAngle = std::acos( cosineOfTransferAngle );
    if ( isLongway )
    {
        transferAngle = 2.0 * mathematical_constants::PI - transferAngle;
    }

    // Lambda parameter.
    const double lambdaParameter = std::sqrt( normalizedRadiusAtArrival )
            * std::cos( transferAngle / 2.0 ) / semiPerimeter;

    // Optimize log(t_spec).
    const double normalizedSpecifiedTimeOfFlight = timeOfFlight / timeNormalizingValue;
    const double logarithmOfTheSpecifiedTimeOfFlight = std::log( normalizedSpecifiedTimeOfFlight );

    // Secant Method.
    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1 = std::log( 0.5 ), x2 = std::log( 1.5 );

    double y1 = std::log( computeTimeOfFlightIzzo( -0.5, semiPerimeter, chord, isLongway,
                                                   semiMajorAxisOfTheMinimumEnergyEllipse ) )
            - logarithmOfTheSpecifiedTimeOfFlight;

    double y2 = std::log( computeTimeOfFlightIzzo( 0.5, semiPerimeter, chord, isLongway,
                                                   semiMajorAxisOfTheMinimumEnergyEllipse ) )
            - logarithmOfTheSpecifiedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    unsigned int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && (y1 != y2)
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = std::log( computeTimeOfFlightIzzo( std::exp( xNew ) - 1.0, semiPerimeter, chord,
                                                  isLongway,
                                                  semiMajorAxisOfTheMinimumEnergyEllipse ) )
                - logarithmOfTheSpecifiedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        std::cerr << "Lambert Solver did not converge within the maximum number of iterations ("
                  << maximumNumberOfIterations << ")." << std::endl;
    }

    // Revert to x parameter.
    const double xParameter = std::exp( xNew ) - 1.0;

    // Determine semi-major axis of the conic.
    const double semiMajorAxis = semiMajorAxisOfTheMinimumEnergyEllipse
            / ( 1.0 - xParameter * xParameter );

    // Declare variables.
    double etaParameter, etaParameterSquared, psiParameter;

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * std::asin( std::sqrt( ( semiPerimeter - chord )
                                                           / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter.
        psiParameter = ( alphaParameter - betaParameter ) / 2.0;

        // Eta parameter.
        etaParameterSquared = 2.0 * semiMajorAxis * std::sin( psiParameter )
                * std::sin( psiParameter ) / semiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * boost::math::asinh (
                    std::sqrt( ( semiPerimeter - chord ) / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter.
        psiParameter = (alphaParameter - betaParameter ) / 2.0;

        // Eta parameter.
        etaParameterSquared = -2.0 * semiMajorAxis * std::sinh( psiParameter )
                * std::sinh( psiParameter ) / semiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Determine semi-latus rectum, p.
    const double semiLatusRectum = ( normalizedRadiusAtArrival
                                     / ( semiMajorAxisOfTheMinimumEnergyEllipse
                                         * etaParameterSquared ) )
            * std::sin( transferAngle / 2.0 )
            * std::sin( transferAngle / 2.0 );

    // Determine sigma.
    const double sigmaParameter =
            ( 1.0 / ( etaParameter * std::sqrt( semiMajorAxisOfTheMinimumEnergyEllipse ) ) )
            * ( 2.0 * lambdaParameter * semiMajorAxisOfTheMinimumEnergyEllipse
                - ( lambdaParameter + xParameter * etaParameter ) );

    // Velocity components at departure.
    const double radialVelocityAtDeparture = sigmaParameter;
    const double transverseVelocityAtDeparture = std::sqrt( semiLatusRectum );

    // Velocity components at arrival.
    const double transverseVelocityAtArrival = transverseVelocityAtDeparture
            / normalizedRadiusAtArrival;
    const double radialVelocityAtArrival = ( transverseVelocityAtDeparture
                                             - transverseVelocityAtArrival )
                                           / std::tan( transferAngle / 2.0 )
                                           - radialVelocityAtDeparture;

    // Determine radial unit vectors.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Determine plane of motion.
    Eigen::Vector3d angularMomentumVector;

    if ( isLongway )
    {
        angularMomentumVector = radialUnitVectorAtArrival.cross( radialUnitVectorAtDeparture );
    }
    else
    {
        angularMomentumVector = radialUnitVectorAtDeparture.cross( radialUnitVectorAtArrival );
    }

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Compute transverse unit vectors.
    const Eigen::Vector3d transverseUnitVectorAtDeparture =
            radialUnitVectorAtDeparture.cross( angularMomentumUnitVector );
    const Eigen::Vector3d transverseUnitVectorAtArrival =
            radialUnitVectorAtArrival.cross( angularMomentumUnitVector );

    // Reconstruct non-dimensional velocity vectors.
    cartesianVelocityAtDeparture
            << radialVelocityAtDeparture * radialUnitVectorAtDeparture.x( )
               - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.x( ),
            radialVelocityAtDeparture * radialUnitVectorAtDeparture.y( )
            - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.y( ),
            radialVelocityAtDeparture * radialUnitVectorAtDeparture.z( )
            - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture.z( );

    cartesianVelocityAtArrival
            << radialVelocityAtArrival * radialUnitVectorAtArrival.x( )
               - transverseVelocityAtArrival * transverseUnitVectorAtArrival.x( ),
            radialVelocityAtArrival * radialUnitVectorAtArrival.y( )
            - transverseVelocityAtArrival * transverseUnitVectorAtArrival.y( ),
            radialVelocityAtArrival * radialUnitVectorAtArrival.z( )
            - transverseVelocityAtArrival * transverseUnitVectorAtArrival.z( );

    // Return dimensions.
    cartesianVelocityAtDeparture *= velocityNormalizingValue;
    cartesianVelocityAtArrival *= velocityNormalizingValue;

}

//! Compute time-of-flight using Lagrange's equation.
double computeTimeOfFlightIzzo( const double xParameter, const double semiPerimeter,
                                const double chord, const bool isLongway,
                                const double semiMajorAxisOfTheMinimumEnergyEllipse )
{
    // Determine semi-major axis.
    const double semiMajorAxis = semiMajorAxisOfTheMinimumEnergyEllipse
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * std::asin ( std::sqrt( ( semiPerimeter - chord )
                                                            / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) ) );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter.
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter.
        double betaParameter = 2.0 * boost::math::asinh(
                    std::sqrt( ( semiPerimeter - chord ) / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlight = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlight;
    }

}

//! Solve Lambert Problem using Gooding's algorithm.
void solveLambertProblemGooding( const Eigen::Vector3d& cartesianPositionAtDeparture,
                                 const Eigen::Vector3d& cartesianPositionAtArrival,
                                 const double timeOfFlight,
                                 const double gravitationalParameter,
                                 Eigen::Vector3d& cartesianVelocityAtDeparture,
                                 Eigen::Vector3d& cartesianVelocityAtArrival,
                                 RootFinderPointer rootFinder )
{
    if ( !rootFinder.get( ) )
    {
        rootFinder = boost::make_shared< root_finders::NewtonRaphson >( 1.0e-12, 1000 );
    }

    // Normalize positions.
    const double radiusAtDeparture = cartesianPositionAtDeparture.norm( );
    const double radiusAtArrival = cartesianPositionAtArrival.norm( );

    // Compute angle between positions.
    Eigen::Vector3d planeNormalPosition =
            cartesianPositionAtDeparture.cross( cartesianPositionAtArrival ).normalized( );

    double reducedLambertAngle = linear_algebra::computeAngleBetweenVectors(
                cartesianPositionAtDeparture, cartesianPositionAtArrival );

    if ( planeNormalPosition.z( ) < 0.0 )
    {
        reducedLambertAngle = 2.0 * mathematical_constants::PI
                - reducedLambertAngle;
    }

    // Compute chord.
    const double chord = std::sqrt( radiusAtDeparture * radiusAtDeparture +
                                    radiusAtArrival * radiusAtArrival -
                                    2.0 * radiusAtDeparture * radiusAtArrival *
                                    std::cos( reducedLambertAngle ) );

    // Compute semi-perimeter.
    const double semiPerimeter = ( radiusAtDeparture + radiusAtArrival + chord ) / 2.0;

    // Compute normalized time of flight.
    // Formula (7) [3].
    const double normalizedTimeOfFlight = std::sqrt( 8.0 * gravitationalParameter / semiPerimeter
                                                     / semiPerimeter / semiPerimeter )
                                          * timeOfFlight;

    // Compute q-parameter.
    // Formula (5) [3].
    const double qParameter = ( std::sqrt( radiusAtDeparture * radiusAtArrival ) /
                                semiPerimeter ) * cos( reducedLambertAngle / 2.0 );

    // Compute values of parameters needed to compute the initial guess of x-parameter.
    const double zParameterInitial = std::sqrt( 1.0 - qParameter * qParameter );

    // Formula (6.11) [2].
    const double lambertEccentricAnomalyInitial = -1.0;

    // Formula (6.12) [2].
    const double yParameterInitial = std::sqrt( std::fabs( lambertEccentricAnomalyInitial ) );

    // Formula (6.14) [2].
    const double fParameterInitial = yParameterInitial * zParameterInitial;

    // Formula (6.15) [2].
    const double gParameterInitial = -1.0 * qParameter * lambertEccentricAnomalyInitial;

    // Page 47 [2].
    const double dParameterInitial = atan( fParameterInitial / gParameterInitial );

    // Value of T(x) for x=0.
    // Formula (6.9) [2].
    const double tFunctionInitial = -2.0 * ( qParameter * zParameterInitial +
            dParameterInitial / yParameterInitial ) / lambertEccentricAnomalyInitial;

    // Declare initial guess of x-parameter.
    double initialLambertGuess;

    // Determine initial Lambert guess.
    if ( tFunctionInitial > normalizedTimeOfFlight )
    {
        // Formula (11) [3].
        initialLambertGuess = tFunctionInitial *
                ( tFunctionInitial - normalizedTimeOfFlight ) /
                ( 4.0 * normalizedTimeOfFlight );
    }
    else
    {
        // Formula (13) [3].
        const double x01 = -1.0 * ( normalizedTimeOfFlight - tFunctionInitial ) /
                     ( normalizedTimeOfFlight - tFunctionInitial + 4.0 );

        // Formula (15) [3].
        const double x02 = -1.0 * std::sqrt( ( normalizedTimeOfFlight -
                     tFunctionInitial ) / ( normalizedTimeOfFlight +
                     0.5 * tFunctionInitial ) );

        // Formula (20) [3].
        const double phi = 2.0 * atan2( ( 1.0 - qParameter * qParameter ), 2.0 * qParameter );

        // Formula (16) [3].
        const double W = x01 + 1.7 * std::sqrt(
                    2.0 - phi / mathematical_constants::PI );

        // Formula (17) [3].
        double x03;
        if ( W >= 0.0 )
        {
           x03 = x01;
        }
        else
        {
           x03 = x01 + std::pow( -W , 1.0 / 16.0 ) * ( x02 - x01 );
        }

        // Formula (19) [3].
        const double lambdax = 1.0 + 0.5 * x03 * ( 1.0 + x01 ) - 0.03 * x03 * x03
                * std::sqrt( 1.0 + x01 );

        // Formula (18) [3].
        initialLambertGuess = lambdax * x03;
    }

    // Newton-Raphson method implementation.
    // Set the class that contains the functions needed for Newton-Raphson.
    LambertFunctionsGooding lambertFunctionsGooding( qParameter, normalizedTimeOfFlight );

    // Create an object containing the function of which we whish to obtain the root from.
    using basic_mathematics::UnivariateProxyPointer;
    using basic_mathematics::UnivariateProxy;
    UnivariateProxyPointer rootFunction = boost::make_shared< UnivariateProxy >(
                boost::bind( &LambertFunctionsGooding::computeLambertFunctionGooding,
                             lambertFunctionsGooding, _1 ) );

    // Add the first derivative of the root function.
    rootFunction->addBinding( -1, boost::bind( &LambertFunctionsGooding::
                                               computeFirstDerivativeLambertFunctionGooding,
                                               lambertFunctionsGooding, _1 ) );

    // Initialize the xParameter.
    double xParameter = TUDAT_NAN;

    // Set initial guess of the variable computed in Newton-Rapshon method. A patch for negative
    // initialLambertGuess is applied. This has not been stated in the paper by Gooding, but this
    // patch has been found by trial and error.
    if ( initialLambertGuess * initialLambertGuess - 1.0 < 0.0 )
    {
        // Set xParameter based on result of Newton-Raphson root-finding algorithm.
        xParameter = rootFinder->execute( rootFunction, std::fabs( initialLambertGuess ) );
    }
    else
    {
        // Set xParameter based on result of Newton-Raphson root-finding algorithm.
        xParameter = rootFinder->execute( rootFunction, initialLambertGuess );
    }

    // Compute velocities at departure and at arrival.

    // Compute gamma, rho and sigma parameters, needed to compute the velocities.
    // Formula (8) [3].
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
            qParameter * qParameter * xParameter * xParameter );

    // Formula (6.23) [2].
    const double lambertGamma = std::sqrt( gravitationalParameter * semiPerimeter / 2.0 );

    // Formula (6.24) [2].
    const double lambertRho = ( radiusAtDeparture - radiusAtArrival ) / chord;

    // Formula (6.25) [2].
    const double lambertSigma = 2.0 * ( std::sqrt(
                                            radiusAtDeparture * radiusAtArrival / chord / chord ) )
            * std::sin( reducedLambertAngle / 2.0 );

    // Compute radial speeds at departure and at arrival.
    // Formula (23) [3].
    const double radialSpeedAtDeparture =
            lambertGamma * ( qParameter * zParameter - xParameter -
                             lambertRho * ( qParameter * zParameter + xParameter ) ) /
            radiusAtDeparture;

    // Formula (24) [3].
    const double radialSpeedAtArrival =
            -lambertGamma * ( qParameter * zParameter - xParameter +
                              lambertRho * ( qParameter * zParameter + xParameter ) ) /
            radiusAtArrival;

    // Compute transverse speeds at departure and at arrival.
    // Compute large part of formulas (25) and (26).
    const double transverseSpeedHelper = lambertGamma * lambertSigma *
            ( zParameter + qParameter * xParameter );

    // Formula (25) [3].
    const double transverseSpeedAtDeparture = transverseSpeedHelper / radiusAtDeparture;

    // Formula (26) [3].
    const double transverseSpeedAtArrival = transverseSpeedHelper / radiusAtArrival;

    // Compute inertial velocities (those velocity are computed with respect to the central body,
    // so they can be either heliocentric or planetocentric).

    // Compute radial unit vectors at departure and at arrival.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    Eigen::Vector3d unitZVector;
    unitZVector.x( ) = 0.0;
    unitZVector.y( ) = 0.0;
    unitZVector.z( ) = 1.0;

    Eigen::Vector3d planeNormal =
            ( ( cartesianPositionAtDeparture.cross( unitZVector ) ).cross(
                    cartesianPositionAtDeparture ) ).normalized( );

    // Compute unit vector that is normal to the plane in which the trajectory takes place, and
    // points in the positive z-direction.
    if ( planeNormal.z( ) < 0.0 )
    {
        planeNormal = -planeNormal;
    }

    // Compute transverse unit vectors at departure and at arrival.
    const Eigen::Vector3d transverseUnitVectorAtDeparture = planeNormal.cross(
            cartesianPositionAtDeparture.normalized( ) );
    const Eigen::Vector3d transverseUnitVectorAtArrival = planeNormal.cross(
            cartesianPositionAtArrival.normalized( ) );

    // Compute radial inertial velocities at departure and at arrival.
    const Eigen::Vector3d radialInertialVelocityAtDeparture
            = radialSpeedAtDeparture * radialUnitVectorAtDeparture;
    const Eigen::Vector3d radialInertialVelocityAtArrival
            = radialSpeedAtArrival * radialUnitVectorAtArrival;

    // Compute transverse heliocentric velocities at departure and
    // at arrival.
    const Eigen::Vector3d transverseInertialVelocityAtDeparture
            = transverseSpeedAtDeparture * transverseUnitVectorAtDeparture;
    const Eigen::Vector3d transverseInertialVelocityAtArrival
            = transverseSpeedAtArrival * transverseUnitVectorAtArrival;

    // Compute heliocentric velocities at departure and at arrival.
    // Define output velocities.
    cartesianVelocityAtDeparture
            = radialInertialVelocityAtDeparture + transverseInertialVelocityAtDeparture;
    cartesianVelocityAtArrival
            = radialInertialVelocityAtArrival + transverseInertialVelocityAtArrival;
}

//! Define general Lambert function.
double LambertFunctionsGooding::computeLambertFunctionGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;

    if ( lambertEccentricAnomaly > 0.0 )
    {
        return lambertFunctionPositiveGooding( xParameter );
    }

    else
    {
        return lambertFunctionNegativeGooding( xParameter );
    }
}

//! Define first derivative of general Lambert function.
double LambertFunctionsGooding::computeFirstDerivativeLambertFunctionGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;

    if ( lambertEccentricAnomaly > 0.0 )
    {
        return lambertFirstDerivativeFunctionPositiveGooding( xParameter );
    }

    else
    {
        return lambertFirstDerivativeFunctionNegativeGooding( xParameter );
    }
}

//! Define Lambert function for positive lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFunctionPositiveGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter+
                                         qParameter * qParameter * xParameter  * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = log( fParameter + gParameter );

    return normalizedTimeOfFlight - 2.0 * ( xParameter - qParameter * zParameter - dParameter /
                                            yParameter ) / lambertEccentricAnomaly;
}

//! Define Lambert function for negative lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFunctionNegativeGooding( const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::atan( fParameter / gParameter );

    return normalizedTimeOfFlight - 2.0 * ( xParameter - qParameter * zParameter - dParameter
                                            / yParameter ) / lambertEccentricAnomaly;
}

//! Define first derivative of Lambert function for positive lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFirstDerivativeFunctionPositiveGooding(
        const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::log( fParameter + gParameter );
    const double lambertEccentricAnomalyDerivative = 2.0 * xParameter;
    const double yParameterDerivative = xParameter / std::sqrt( xParameter * xParameter - 1.0 );
    const double zParameterDerivative = qParameter * qParameter * xParameter / zParameter;
    const double fParameterDerivative = yParameterDerivative  *
            ( zParameter - xParameter * qParameter ) + yParameter *
            ( zParameterDerivative - qParameter );
    const double gParameterDerivative = zParameter + xParameter *
            zParameterDerivative - qParameter * lambertEccentricAnomalyDerivative;
    const double dParameterDerivative = ( fParameterDerivative + gParameterDerivative ) /
            ( fParameter + gParameter );

    return - 2.0 * ( lambertEccentricAnomaly
                     * ( 1.0 - qParameter * zParameterDerivative
                         - ( ( dParameterDerivative * yParameter
                               - yParameterDerivative * dParameter )
                             / ( yParameter * yParameter ) ) )
                     - ( ( xParameter - qParameter * zParameter - ( dParameter / yParameter ) )
                         * lambertEccentricAnomalyDerivative ) )
            / ( lambertEccentricAnomaly * lambertEccentricAnomaly );
}

//! Define first derivative of Lambert function for negative lambertEccentricAnomaly.
double LambertFunctionsGooding::lambertFirstDerivativeFunctionNegativeGooding(
        const double xParameter )
{
    const double lambertEccentricAnomaly = xParameter * xParameter - 1.0;
    const double yParameter = std::sqrt( std::fabs( lambertEccentricAnomaly ) );
    const double zParameter = std::sqrt( 1.0 - qParameter * qParameter +
                                         qParameter * qParameter * xParameter * xParameter );
    const double fParameter = yParameter * ( zParameter - qParameter * xParameter );
    const double gParameter = xParameter * zParameter - qParameter * lambertEccentricAnomaly;
    const double dParameter = std::atan( fParameter / gParameter );
    const double lambertEccentricAnomalyDerivative = 2.0 * xParameter;
    const double yParameterDerivative = -1.0 * xParameter
            / std::sqrt( 1.0 - xParameter * xParameter);
    const double zParameterDerivative = qParameter * qParameter * xParameter / zParameter;
    const double fParameterDerivative = yParameterDerivative *
            ( zParameter - xParameter * qParameter ) + yParameter *
            ( zParameterDerivative - qParameter );
    const double gParameterDerivative = zParameter + xParameter *
            zParameterDerivative - qParameter *  lambertEccentricAnomalyDerivative;
    const double dParameterDerivative = ( fParameterDerivative * gParameter -
                                          fParameter * gParameterDerivative ) /
            ( fParameter * fParameter + gParameter * gParameter );

    return - 2.0 * ( lambertEccentricAnomaly * ( 1.0 - qParameter * zParameterDerivative -
                                                 ( ( dParameterDerivative * yParameter -
                                                     yParameterDerivative * dParameter ) /
                                                   yParameter / yParameter ) ) -
                     ( ( xParameter - qParameter * zParameter - ( dParameter / yParameter ) ) *
                       lambertEccentricAnomalyDerivative ) ) /
            ( lambertEccentricAnomaly * lambertEccentricAnomaly );
}

} // namespace mission_segments
} // namespace tudat


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// 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 "Geomlib_PolylineIntersection.h"
#include "Geomlib_BestFitPlane.h"
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include "Polyline.h"

using Urho3D::Vector;
using Urho3D::Vector3;
using Urho3D::Vector2;

using namespace Urho3D;

// [t1, t2] interval of real number line; no promise that t1 <= t2
// [s1, s2] interval of real number line; no promise that s1 <= s2
// Returns:
//   1 if [t1, t2] and [s1, s2] have non-empty intersection
//   0 otherwise
int Geomlib::IntervalIntervalIntersection(
	double t1,
	double t2,
	double s1,
	double s2)
{
	if (t1 > t2) {
		double x = t1;
		t1 = t2;
		t2 = x;
	}
	if (s1 > s2) {
		double y = s1;
		s1 = s2;
		s2 = y;
	}

	if (s1 <= t1 && t1 <= s2) {
		return 1;
	}
	else if (s1 <= t2 && t2 <= s2) {
		return 1;
	}
	else if (t1 <= s1 && s1 <= t2) {
		return 1;
	}

	return 0;
}

//   1 if (A,B) and (C,D) have non-empty intersection
//   0 otherwise
int Geomlib::SegmentSegmentIntersection2D(
	Vector2& A,
	Vector2& B,
	Vector2& C,
	Vector2& D)
{
	// left & right half plane checks
	if (
		(A.x_ > 0 && B.x_ > 0) && // A,B in right half plane
		(C.x_ < 0 && D.x_ < 0)    // C,D in left half plane
		) {
		return 0;
	}
	if (
		(A.x_ < 0 && B.y_ < 0) && // A,B in left half plane
		(C.x_ > 0 && D.y_ > 0)    // C,D in right half plane
		) {
		return 0;
	}
	// upper & lower half plane checks
	if (
		(A.y_ > 0 && B.y_ > 0) && // A,B in upper half plane
		(C.y_ < 0 && D.y_ < 0)    // C,D in lower half plane
		) {
		return 0;
	}
	if (
		(A.y_ < 0 && B.y_ < 0) && // A,B in lower half plane
		(C.y_ > 0 && D.y_ > 0)    // C,D in upper half plane
		) {
		return 0;
	}

	// do C and D lie strictly on the same side of line through AB?
	double cval = (B.y_ - A.y_) * (C.x_ - A.x_) + (A.x_ - B.x_) * (C.y_ - A.y_);
	double dval = (B.y_ - A.y_) * (D.x_ - A.x_) + (A.x_ - B.x_) * (D.y_ - A.y_);
	if ((cval > 0 && dval > 0) || (cval < 0 && dval < 0)) {
		return 0;
	}
	// do A and B lie strictly on the same side of line through CD?
	double aval = (D.y_ - C.y_) * (A.x_ - C.x_) + (C.x_ - D.x_) * (A.y_ - C.y_);
	double bval = (D.y_ - C.y_) * (B.x_ - C.x_) + (C.x_ - D.x_) * (B.y_ - C.y_);
	if ((aval > 0 && bval > 0) || (aval < 0 && bval < 0)) {
		return 0;
	}

	// lines through AB and CD definitely intersect
	double k11 = B.x_ - A.x_;
	double k12 = C.x_ - D.x_;
	double k21 = B.y_ - A.y_;
	double k22 = C.y_ - D.y_;
	double z1 = C.x_ - A.x_;
	double z2 = C.y_ - A.y_;
	double prod1 = k11 * k22;
	double prod2 = k12 * k21;
	double prod3 = k22 * z1;
	double prod4 = k12 * z2;
    if (std::abs(prod1) > 1e16 || std::abs(prod2) > 1e16 || std::abs(prod3) > 1e16 || std::abs(prod4) > 1e16) {
		double shrink = 1e-6;
		Vector2 newA = shrink * A;
		Vector2 newB = shrink * B;
		Vector2 newC = shrink * C;
		Vector2 newD = shrink * D;
		return Geomlib::SegmentSegmentIntersection2D(newA, newB, newC, newD);
	}
	double denom = prod1 - prod2;
	double numer = prod3 - prod4;
	if (prod1 == prod2) {
		denom = 0;
	}

	const double eps = 1e-8;

	if (denom > eps || -denom > eps) {
		// there is unique point of intersection: does it lie in segment AB?
		double t = (k22 * z1 - k12 * z2) / denom;
		return 0 <= t && t <= 1;
	}
	else {
		// lines through AB and CD coincide
		double rise = B.y_ - A.y_;
		double run = B.x_ - A.x_;
		// normal is (-rise, run)
		if (rise >= run) { // x-coord of normal is bigger, project segments to y-axis
			return Geomlib::IntervalIntervalIntersection(A.y_, B.y_, C.y_, D.y_);
		}
		else { // y-coord of normal is bigger, project segments to x-axis
			return Geomlib::IntervalIntervalIntersection(A.x_, B.x_, C.x_, D.x_);
		}
	}
}

	int Geomlib::SegmentSegmentIntersection3D(
		Vector3& A,
		Vector3& B,
		Vector3& C,
		Vector3& D,
		Vector3& normal)
	{

		// converting to regular vector to use max_element
		std::vector<double> Nreg;
		Nreg.push_back(std::abs(normal.x_));
		Nreg.push_back(std::abs(normal.y_));
		Nreg.push_back(std::abs(normal.z_));

		// find greatest coordinate from plane normal
		// projection plane will be the plane generate by the OTHER coordinate axes
		// e.g. if N.y is largest, project to xz-plane
		int maxCoord = std::max_element(Nreg.begin(), Nreg.end()) - Nreg.begin();

		Vector<Vector3> startVerts;
		startVerts.Push(A);
		startVerts.Push(B);
		startVerts.Push(C);
		startVerts.Push(D);

		// project coords to the appropriate plane
		// NOTE SYNTAX!!!!!!!!!!!
		std::vector<Eigen::RowVector2d, Eigen::aligned_allocator<Eigen::RowVector2d>> flatVerts;
		//Vector<Vector2> flatVerts;
		for (int i = 0; i < startVerts.Size(); ++i)
		{
			int counter = 0;
			Eigen::RowVector2d tmpCoords;
			//Vector2 tmpCoords;
			for (int j = 0; j < 3; ++j)
			{
				if (j != maxCoord)
				{
					//std::cout << "counter=" << counter << " j=" << j << std::endl;
					//std::cout << "startVerts[" << i << "](" << j << ")=" << startVerts[i](j) << std::endl;
					if (j == 0)
						tmpCoords(counter) = startVerts[i].x_;
					else if (j == 1)
						tmpCoords(counter) = startVerts[i].y_;
					else if (j == 2)
						tmpCoords(counter) = startVerts[i].z_;
					++counter;
				}
			}
			//std::cout << "tmpCoords=" << tmpCoords << std::endl;
			flatVerts.push_back(tmpCoords);
		}

		Vector2 newA(flatVerts[0](0), flatVerts[0](1));
		Vector2 newB(flatVerts[1](0), flatVerts[1](1));
		Vector2 newC(flatVerts[2](0), flatVerts[2](1));
		Vector2 newD(flatVerts[3](0), flatVerts[3](1));

		int ret = Geomlib::SegmentSegmentIntersection2D(
			newA,
			newB,
			newC,
			newD);

		return ret;

	}

	bool Geomlib::HasSelfIntersection(const Urho3D::Variant polyline)
	{
		VariantVector verts = Polyline_ComputeSequentialVertexList(polyline);

		// This will only work for planar polylines!
		Vector3 point, normal;
		Geomlib::BestFitPlane(Polyline_ComputePointCloud(polyline), point, normal);

		bool closed = Polyline_IsClosed(polyline);
		int counter = verts.Size()-4;
		if (counter < 4)
			return false;

		for (int i = 0; i < counter; ++i) {
			Vector3 A = verts[i].GetVector3();
			Vector3 B = verts[i + 1].GetVector3();
			for (int j = i + 2; j < counter+2; ++j) {
				Vector3 C = verts[j].GetVector3();
				Vector3 D = verts[j + 1].GetVector3();
				int ret = Geomlib::SegmentSegmentIntersection3D(A, B, C, D, normal);
				if (ret == 1)
					return true;
			}
		}
		return false;
	}

	int Geomlib::GetSelfIntersections(const Urho3D::Variant polyline, Urho3D::Vector<Urho3D::Vector3>& intersections, Urho3D::Variant& revised_polyline)
	{
		VariantVector verts = Polyline_ComputeSequentialVertexList(polyline);
		Vector<Vector3> revised_verts;
		for (int i = 0; i < verts.Size(); ++i)
			revised_verts.Push(verts[i].GetVector3());

		// This will only work for planar polylines!
		Vector3 point, normal;
		Geomlib::BestFitPlane(Polyline_ComputePointCloud(polyline), point, normal);

		bool closed = Polyline_IsClosed(polyline);
		int counter = verts.Size() - 4;
		if (counter < 4)
			return 0;

		for (int i = 0; i < counter; ++i) {
			Vector3 A = verts[i].GetVector3();
			Vector3 B = verts[i + 1].GetVector3();
			for (int j = i + 2; j < counter + 2; ++j) {
				Vector3 C = verts[j].GetVector3();
				Vector3 D = verts[j + 1].GetVector3();
				int ret = Geomlib::SegmentSegmentIntersection3D(A, B, C, D, normal);
				if (ret == 1) {
					Vector3 point;
					int success = GetPointOfIntersection(A, B, C, D, point);
					if (success) {
						intersections.Push(point);
						InsertSelfIntersectionsAsPoint(A, B, C, D, point, revised_verts);
					}
				}
			}
		}
		revised_polyline = Geomlib::Polyline_Make_With_Intersections(revised_verts);
		return 1;
	}

	// inserts the point into the vertex list between A, B and C, D
	int Geomlib::InsertSelfIntersectionsAsPoint(
		const Urho3D::Vector3& A,
		const Urho3D::Vector3& B,
		const Urho3D::Vector3& C,
		const Urho3D::Vector3& D,
		const Urho3D::Vector3& point,
		Urho3D::Vector<Urho3D::Vector3>& verts)
	{
		// Assuming this inserts before the destination element?
		Vector<Vector3>::Iterator it_D= verts.Find(D);
		verts.Insert(it_D, point);

		Vector<Vector3>::Iterator it_B = verts.Find(B);
		verts.Insert(it_B, point);

		return 0;
	}

	int Geomlib::GetPointOfIntersection( const Urho3D::Vector3 & A, const Urho3D::Vector3 & B, const Urho3D::Vector3 & C, const Urho3D::Vector3 & D, Urho3D::Vector3& point)
	{
		double ta, tb;
		bool success = Geomlib::ShortestDistance(A, B, C, D, ta, tb);
		if (ta < 0 || ta > 1 || tb < 0 || tb >1)
			return 0;

		// find the points on the line segments
		Vector3 pa = A + ta*(B - A);
		Vector3 pb = C + tb*(D - C);

		// if the distance between the points is small, this is a true intersection
		double dist = (pb - pa).Length();
		if (dist < 0.0001) {
			point = pa;
			return 1;
		}

		return 0;
	}

	/// returns parameters on each line. If they are between 0-1, the closest point lies on the line. Otherwise,
	/// the point is on the ray defined by the line direction.
	/// if return is false, the closest point could not be calculated (probably due to the lines being parallel)
	bool Geomlib::ShortestDistance(
		const Vector3& A,
		const Vector3& B,
		const Vector3& C,
		const Vector3& D,
		double& ta, double& tb)
	{

		Vector3 u = B - A;
		Vector3 v = D - C;
		Vector3 w = A - C;

		double dot = u.Normalized().DotProduct(v.Normalized());

        if (std::abs(dot - 1.0) < 10e-7)
			return false;

		double uu = u.DotProduct(u);
		double uv = u.DotProduct(v);
		double vv = v.DotProduct(v);
		double uw = u.DotProduct(w);
		double vw = v.DotProduct(w);

		double t = 1.0 / (uu * vv - uv * uv);
		ta = (uv * vw - vv * uw) * t;
		tb = (uu * vw - uv * uw) * t;

		return true;

	}

	Urho3D::Variant Geomlib::Polyline_Make_With_Intersections(const Urho3D::Vector<Urho3D::Vector3>& vertexList)
	{
		Variant earlyRet;
		if (vertexList.Size() < 2) {
			return earlyRet;
		}

		Vector<Variant> vertices;
		Vector<Variant> edges;

		for (int i = 0; i < vertexList.Size(); ++i) {
			vertices.Push(Variant(vertexList[i]));
			edges.Push(Variant(i));
		}
		VariantMap var_map;
		var_map["type"] = Variant(String("Polyline"));
		var_map["vertices"] = Variant(vertices);
		var_map["edges"] = Variant(edges);

		return Variant(var_map);
	}


/*
matrix/MatrixRxC.hpp
-----------------------
Copyright (c) 2014, theJ89

Description:
    Adds a generic matrix class, Matrix.
    You can specify what type of data the matrix uses (int, float, etc) as well as the number of rows and columns in the matrix.
    The code in this file handles rectangular matrices.
    Common specializations of Matrix are provided in Matrix2x2.hpp, Matrix3x3.hpp, Matrix4x4.hpp, and MatrixNxN.hpp.
*/
#ifndef BS_MATRIX_MATRIXRXC_HPP
#define BS_MATRIX_MATRIXRXC_HPP

//Includes
#include <iostream>                     //std::ostream
#include <iomanip>                      //std::setw, std::setprecision

#include <boost/format.hpp>             //boost::format

#include <brimstone/util/Array.hpp>     //BS_ARRAY_DECLARE_METHODS, BS_ARRAY_DEFINE_METHODS, etc
#include <brimstone/Vector.hpp>         //Vector
#include <brimstone/util/MinMax.hpp>    //electMax

//Macros
#define BS_MATRIX_DECLARE_METHODS( R, C )                                   \
    Matrix();                                                               \
                                                                            \
    template< typename T2 >                                                 \
    Matrix( const Matrix< T2, R, C >& toCopy );                             \
                                                                            \
    bool isSquare() const;                                                  \
    size_t getRows() const;                                                 \
    size_t getColumns() const;                                              \
                                                                            \
    void zero();                                                            \
    bool isZero() const;                                                    \
    void identity();                                                        \
    bool isIdentity() const;                                                \
                                                                            \
    void setRow( const size_t row, const Vector< T, C >& values );          \
    void setColumn( const size_t col, const Vector< T, R >& values );       \
                                                                            \
    Vector< T, C > getRow( const size_t row ) const;                        \
    Vector< T, R > getColumn( const size_t col ) const;                     \
                                                                            \
    template< typename T2 >                                                 \
    Matrix& operator =( const Matrix< T2, R, C >& right );                  \
    Matrix& operator +=( const Matrix& right );                             \
    Matrix& operator -=( const Matrix& right );                             \
    Matrix& operator *=( const Matrix< T, C, C >& right );                  \
    Matrix& operator +=( const T right );                                   \
    Matrix& operator -=( const T right );                                   \
    Matrix& operator *=( const T right );                                   \
    Matrix& operator /=( const T right );                                   \
    T&      operator ()( const size_t row, const size_t col );              \
    T       operator ()( const size_t row, const size_t col ) const;

#define BS_MATRIX_DEFINE_METHODS( R, C, tmpl, spec )                        \
    tmpl                                                                    \
    size_t Matrix spec::getRows() const {                                   \
        return R;                                                           \
    }                                                                       \
    tmpl                                                                    \
    size_t Matrix spec::getColumns() const {                                \
        return C;                                                           \
    }                                                                       \
    tmpl                                                                    \
    void Matrix spec::identity() {                                          \
        (*this) = m_identity;                                               \
    }                                                                       \
    tmpl                                                                    \
    bool Matrix spec::isIdentity() const {                                  \
        return (*this) == m_identity;                                       \
    }                                                                       \
    tmpl                                                                    \
    T& Matrix spec::operator()( const size_t row, const size_t col ) {      \
        BS_ASSERT_INDEX( row, R - 1 );                                      \
        BS_ASSERT_INDEX( col, C - 1 );                                      \
        return elem[row][col];                                              \
    }                                                                       \
    tmpl                                                                    \
    T Matrix spec::operator()( const size_t row, const size_t col ) const { \
        BS_ASSERT_INDEX( row, R - 1 );                                      \
        BS_ASSERT_INDEX( col, C - 1 );                                      \
        return elem[row][col];                                              \
    }

namespace Brimstone {

template< typename T, size_t R, size_t C >
class Matrix {
public:
//C4201: nonstandard extension used : nameless struct/union
//It's a non-standard feature, but VC++, G++, and LLVM support it so it shouldn't be too much of an issue
#pragma warning( push )
#pragma warning( disable: 4201 )

    union {
        T              data[ R * C ];
        T              elem[R][C];
        struct {
            Vector< T, C > row[ R ];
        };
    };

#pragma warning( pop )
public:
    BS_ARRAY_DECLARE_INHERITED_METHODS( Matrix, T )
    BS_ARRAY_DECLARE_METHODS( Matrix, T )
    BS_MATRIX_DECLARE_METHODS( R, C )
private:
    struct ConstructIdentity {};
    Matrix( const ConstructIdentity );
    static const Matrix m_identity;
};
BS_ARRAY_DEFINE_GENERIC_METHODS( Matrix, T, data, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )
BS_ARRAY_DEFINE_METHODS( Matrix, T, data, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )
BS_MATRIX_DEFINE_METHODS( R, C, BS_TMPL_3( typename T, size_t R, size_t C ), BS_SPEC_3( T, R, C ) )

//Forward declarations
template< typename T, size_t R, size_t C >
Matrix< T, C, R > transpose( const Matrix< T, R, C >& matrix );

//Note: We have a private constructor specifically for constructing
//m_identity. To differentiate it from other constructors,
//we pass an unused argument of type "ConstructIdentity",
//ConstructIdentity is a private inner class in Matrix, which means that only something
//belonging to Matrix can instantiate ConstructIdentity objects. Normally this
//means "a method in Matrix", but this also works with statements that initialize
//static members of Matrix as well.
template< typename T, size_t R, size_t C >
const Matrix< T, R, C > Matrix< T, R, C >::m_identity = Matrix< T, R, C >( ConstructIdentity() );

//We could just put this in the default constructor, but the advantage of this approach
//is that by copying a pre-built matrix, we can avoid using two loops and running a conditional
//on each element of every matrix that is default-constructed, and instead only do this once
//when constructing the identity matrix.
template< typename T, size_t R, size_t C >
Matrix< T, R, C >::Matrix( const ConstructIdentity ) {
    for( size_t r = 0; r < R; ++r )
        for( size_t c = 0; c < C; ++c )
            elem[r][c] = ( r == c ? static_cast< T >( 1 ) : static_cast< T >( 0 ) );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >::Matrix()
#ifdef BS_ZERO
    : Matrix( m_identity )
#endif //BS_ZERO
{
}

template< typename T, size_t R, size_t C >
template< typename T2 >
Matrix< T, R, C >::Matrix( const Matrix< T2, R, C >& toCopy ) {
    (*this) = toCopy;
}

template< typename T, size_t R, size_t C >
bool Matrix< T, R, C >::isSquare() const {
    //This function always returns false because the generic case of Matrix handles rectangular matrices.
    return false;
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::zero() {
    T zero = static_cast< T >( 0 );
    for( size_t i = 0; i < R * C; ++i )
        data[i] = zero;
}

template< typename T, size_t R, size_t C >
bool Matrix< T, R, C >::isZero() const {
    T zero = static_cast< T >( 0 );
    for( size_t i = 0; i < R * C; ++i )
        if( data[i] != zero )
            return false;
    return true;
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::setRow( const size_t row, const Vector< T, C >& values ) {
    BS_ASSERT_INDEX( row, R - 1 );
    for( size_t c = 0; c < C; ++c )
        elem[row][c] = values.data[c];
}

template< typename T, size_t R, size_t C >
void Matrix< T, R, C >::setColumn( const size_t col, const Vector< T, R >& values ) {
    BS_ASSERT_INDEX( col, C - 1 );
    for( size_t r = 0; r < R; ++r )
        elem[r][col] = values.data[r];
}

template< typename T, size_t R, size_t C >
Vector< T, C > Matrix< T, R, C >::getRow( const size_t row ) const {
    BS_ASSERT_INDEX( row, R - 1 );
    Vector< T, C > out;

    for( size_t c = 0; c < C; ++c )
        out.data[c] = elem[row][c];

    return out;
}

template< typename T, size_t R, size_t C >
Vector< T, R > Matrix< T, R, C >::getColumn( const size_t col ) const {
    BS_ASSERT_INDEX( col, C - 1 );
    Vector< T, R > out;

    for( size_t r = 0; r < R; ++r )
        out.data[r] = elem[r][col];

    return out;
}

template< typename T, size_t R, size_t C >
template< typename T2 >
Matrix< T, R, C >& Matrix< T, R, C >::operator =( const Matrix< T2, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] = static_cast<T>( right.data[i] );

    return (*this);
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator +=( const Matrix& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] += right.data[i];

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator -=( const Matrix& right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] -= right.data[i];

    return ( *this );
}

namespace Private {

    //Note: You can only multiply an IxJ matrix with an JxK matrix.
    //      The result is an IxK matrix.
    //      Since this function multiplies in-place (i.e. it implements leftInOut *= right ),
    //      the resulting matrix must be of size IxJ.
    //      The only way this is possible is if K = J;
    //      in other words, the given matrix must be a square JxJ matrix.
    template< typename T, size_t R, size_t C >
    void MatrixMultiplyInPlace( Matrix< T, R, C >& leftInOut, const Matrix< T, C, C >& right ) {
        T oldRow[ C - 1 ];
        T dot;
        for( size_t r = 0; r < R; ++r ) {
            //Store the old values for the current row we're working on
            //We only need to store the first C-1 values, because we need to know
            //their old values when setting their new values for all but the last
            //element in the current row
            for( size_t c = 0; c < C - 1; ++c )
                oldRow[c] = leftInOut.elem[r][c];

            //Update the values
            for( size_t c = 0; c < C; ++c ) {
                dot = static_cast< T >( 0 );

                //Partially calculate the dot product between the current row in the left matrix,
                //and the current column in the right matrix.
                for( size_t c2 = 0; c2 < C - 1; ++c2 )
                    dot += oldRow[ c2 ] * right.elem[c2][c];

                //Finalize the dot product and update the element in the left matrix.
                leftInOut.elem[r][c] = dot +
                                       leftInOut.elem[r][C-1] * right.elem[C-1][c];
            }
        }
    }
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator *=( const Matrix< T, C, C >& right ) {
    Private::MatrixMultiplyInPlace( *this, right );
    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator +=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] += right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator -=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] -= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator *=( const T right ) {
    for( size_t i = 0; i < R * C; ++i )
        data[i] *= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C >& Matrix< T, R, C >::operator /=( const T right ) {
    BS_ASSERT_NONZERO_DIVISOR( right )

    for( size_t i = 0; i < R * C; ++i )
        data[i] /= right;

    return ( *this );
}

template< typename T, size_t R, size_t C >
std::ostream& operator <<( std::ostream& left, const Matrix< T, R, C >& right ) {
    size_t maxWidth = 1;
    std::string str[ R * C ];
    for( size_t i = 0; i < R * C; ++i ) {
        str[ i ] = ( boost::format( "%|.5f|" ) % right.data[i] ).str();
        electMax( maxWidth, str[ i ].size() );
    }

    for( size_t r = 0; r < R; ++r ) {
        left << std::setw( maxWidth ) << str[r*C];
        for( size_t c = 1; c < C; ++c )
            left << " " << std::setw( maxWidth ) << str[r*C + c];
        left << std::endl;
    }
    return left;
}

template< typename T, size_t R, size_t C >
bool operator ==( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        if( left.data[i] != right.data[i] )
            return false;
    return true;
}

template< typename T, size_t R, size_t C >
bool operator !=( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    for( size_t i = 0; i < R * C; ++i )
        if( left.data[i] != right.data[i] )
            return true;
    return false;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& right ) {
    return right;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = -right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] + right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] - right.data[i];

    return out;
}

template< typename T, size_t R, size_t C, size_t C2 >
Matrix< T, R, C2 > operator *( const Matrix< T, R, C >& left, const Matrix< T, C, C2 >& right ) {
    Matrix< T, R, C2 > out;

    T dot;
    for( size_t r = 0; r < R; ++r ) {
        for( size_t c2 = 0; c2 < C2; ++c2 ) {
            dot = static_cast< T >( 0 );

            for( size_t c = 0; c < C; ++c )
                dot += left.elem[r][c] * right.elem[c][c2];

            out.elem[r][c2] = dot;
        }
    }

    return out;
}

//R-vector (as a 1xR matrix) * RxC matrix = C-vector (as a 1xC matrix)
template< typename T, size_t R, size_t C >
Vector< T, C > operator *( const Vector< T, R >& left, const Matrix< T, R, C >& right ) {
    Vector< T, C > out;

    T dot;
    for( size_t c = 0; c < C; ++c ) {
        dot = static_cast< T >( 0 );

        for( size_t r = 0; r < R; ++r )
            dot += left[r] * right.elem[r][c];

        out.data[c] = dot;
    }

    return out;
}

//RxC matrix * C-vector (as a Cx1 matrix) = R-vector (as a Rx1 matrix)
template< typename T, size_t R, size_t C >
Vector< T, R > operator *( const Matrix< T, R, C >& left, const Vector< T, C >& right ) {
    Vector< T, R > out;

    T dot;
    for( size_t r = 0; r < R; ++r ) {
        dot = static_cast< T >( 0 );

        for( size_t c = 0; c < C; ++c )
            dot += left.elem[r][c] * right[c];

        out.data[r] = dot;
    }

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const T left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left + right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator +( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] + right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator -( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] - right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator *( const T left, const Matrix< T, R, C >& right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left * right.data[i];

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator *( const Matrix< T, R, C >& left, const T right ) {
    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] * right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, R, C > operator /( const Matrix< T, R, C >& left, const T right ) {
    BS_ASSERT_NONZERO_DIVISOR( right );

    Matrix< T, R, C > out;

    for( size_t i = 0; i < R * C; ++i )
        out.data[i] = left.data[i] / right;

    return out;
}

template< typename T, size_t R, size_t C >
Matrix< T, C, R > transpose( const Matrix< T, R, C >& matrix ) {
    Matrix< T, C, R > out;

    for( size_t r = 0; r < R; ++r )
        for( size_t c = 0; c < C; ++c )
            out.elem[c][r] = matrix.elem[r][c];

    return out;
}

}

#endif //BS_MATRIX_MATRIXRXC_HPP


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file sigma_point_additive_prediction_policy.hpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class SigmaPointPredictPolicy;

template <
    typename SigmaPointQuadrature,
    typename AdditiveTransitionFunction
>
class SigmaPointPredictPolicy<
          SigmaPointQuadrature,
          Additive<AdditiveTransitionFunction>>
    : public Descriptor
{
public:
    typedef typename AdditiveTransitionFunction::State State;
    typedef typename AdditiveTransitionFunction::Input Input;

    enum : signed int
    {
        NumberOfPoints = SigmaPointQuadrature
                            ::number_of_points(SizeOf<State>::Value)
    };

    typedef PointSet<State, NumberOfPoints> StatePointSet;

    template <
        typename Belief
    >
    void operator()(const AdditiveTransitionFunction&
                              additive_transition_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Input& u,
                    Belief& predicted_belief)
    {
        auto f = [&](const State& x)
        {
            return additive_transition_function.expected_state(x, u);
        };

        quadrature.propergate_gaussian(f, prior_belief, Y, Z);

        /*
         * Obtain the centered points matrix of the prediction. The columns of
         * this matrix are the predicted points with zero mean. That is, the
         * sum of the columns in P is zero.
         *
         * P = [X_r[1]-mu_r  X_r[2]-mu_r  ... X_r[n]-mu_r]
         *
         * with weighted mean
         *
         * mu_r = Sum w_mean[i] X_r[i]
         */
        auto X_c = Z.centered_points();

        /*
         * Obtain the weights of point as a vector
         *
         * W = [w_cov[1]  w_cov[2]  ... w_cov[n]]
         *
         * Note that the covariance weights are used.
         */
        auto W = Z.covariance_weights_vector();

        /*
         * Compute and set the moments
         *
         * The first moment is simply the weighted mean of points.
         * The second centered moment is determined by
         *
         * C = Sum W[i,i] * (X_r[i]-mu_r)(X_r[i]-mu_r)^T
         *   = P * W * P^T
         *
         * given that W is the diagonal matrix
         */
        predicted_belief.dimension(prior_belief.dimension());
        predicted_belief.mean(Z.mean());
        predicted_belief.covariance(
            X_c * W.asDiagonal() * X_c.transpose()
            + additive_transition_function.noise_covariance());
    }

    virtual std::string name() const
    {
        return "SigmaPointPredictPolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "Additive<AdditiveTransitionFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Sigma Point based filter prediction policy for state "
               "transition model with additive noise";
    }

protected:
    StatePointSet Y;
    StatePointSet Z;
};

}


/*
Copyright (c) 2020 Naomasa Matsubayashi

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 <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <boost/program_options.hpp>
#include <enoki/cuda.h>
#include <enoki/dynamic.h>
#include <enoki/autodiff.h>
#include <enoki/special.h>
#include <enoki/array_router.h>
#include "ifm/fft.h"
#include "ifm/load_monoral.h"
#include "ifm/spectrum_image.h"

using i_t = enoki::DiffArray< enoki::CUDAArray< int > >;
using v_t = enoki::DiffArray< enoki::CUDAArray< float > >;

float j1( float b, int n ) {
  float prev = 0;
  float width = 0.001;
  float sum = 0;
  for( float w = width; w < float( M_PI/2 ); w += width ) {
    float cur = std::sin( b * std::sin( w ) ) * std::sin( n*w );
    sum += ( cur + prev ) * width / 2;
    prev = cur;
  }
  return 2.f / M_PI * sum;
}

float j0( float b, int n ) {
  float prev = 1;
  float width = 0.001;
  float sum = 0;
  for( float w = width; w < float( M_PI/2 ); w += width ) {
    float cur = std::cos( b * std::sin( w ) ) * std::cos( n*w );
    sum += ( cur + prev ) * width / 2;
    prev = cur;
  }
  return 2.f / M_PI * sum;
}

float j( float b, int n ) {
  if( n % 2 ) return j1( b, n );
  else return j0( b, n );
}

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("harmonic,H", boost::program_options::value<int>()->default_value(0),  "倍音のみ")
    ("match,m", boost::program_options::value<bool>()->default_value(false),  "減衰曲線をマッチさせる")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const auto [audio,sample_rate] = ifm::load_monoral( input_filename, true );
  ifm::spectrum_image conv( params["note"].as<int>(), sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  unsigned int width = conv.get_width();
  float resolution = 1 << params["resolution"].as<int>();
  std::vector< float > envelope;

  if( params["harmonic"].as<int>() > 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 1; x < width; x += 24 )
        sum += image[ x + y * width ];
      envelope.push_back( sum );
    }
  }
  else if( params["harmonic"].as<int>() < 0 ) {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = 0.f;
      for( unsigned int x = 25; x < width; ++x ) {
        unsigned int step =  x % 24;
        if( step > 1 && step < 23 )
          sum += image[ x + y * width ];
      }
      envelope.push_back( sum );
    }
  }
  else {
    for( unsigned int y = 0; y != image.size() / width; ++y ) {
      float sum = std::accumulate( image.data() + y * width, image.data() + ( y + 1 ) * width, 0.f );
      envelope.push_back( sum );
    }
  }
  if( params["match"].as<bool>() ) {
    using v_t = enoki::DiffArray< enoki::CUDAArray< float > >;
    const auto max = std::max_element( envelope.begin(), envelope.end() );
    envelope.erase( envelope.begin(), max );
    const auto min = std::find_if( envelope.begin(), envelope.end(), []( float v ) { return v < 0.01f; } );
    envelope.erase( min, envelope.end() );
    if( params["harmonic"].as<int>() < 0 )
      envelope.resize( std::min( size_t( 50 ), envelope.size() ) );
    auto expected = v_t::copy( envelope.data(), envelope.size() );
    float a = 1;
    float b = 10;
    float c = 0.3;
    v_t beta1_ = 0.9;
    v_t beta2_ = 0.999;
    v_t alpha = 0.001;
    v_t beta1 = 0.9;
    v_t beta2 = 0.999;
    v_t a_( a );
    v_t b_( b );
    v_t c_( c );
    v_t m_a( 0 );
    v_t m_b( 0 );
    v_t m_c( 0 );
    v_t v_a( 0 );
    v_t v_b( 0 );
    v_t v_c( 0 );
    auto t = enoki::arange< v_t >( envelope.size() ) * v_t( 0.01 );
    for( int i = 1; i != 100000; ++i ) {
      a_ = a;
      b_ = b;
      c_ = c;
      enoki::set_requires_gradient( a_ );
      enoki::set_requires_gradient( b_ );
      enoki::set_requires_gradient( c_ );
      auto generated = a_ * enoki::exp( -t * b_ ) + c_;
      auto diff = ( expected - generated );
      //auto loss = enoki::hsum( diff * diff );
      //std::cout << loss << " ";
      enoki::backward( diff * diff );

      v_t g_a = gradient( a_ );
      v_t g_b = gradient( b_ );
      v_t g_c = gradient( c_ );
      auto ms = beta1;//enoki::pow( beta1, i_ );
      auto vs = beta2;//enoki::pow( beta2, i_ );
      beta1 *= beta1_;
      beta2 *= beta2_;
      m_a = beta1 * m_a + ( 1 - beta1 ) * g_a;
      v_a = beta2 * v_a + ( 1 - beta2 ) * g_a * g_a;
      auto mhat_a = m_a / ( 1 - ms );
      auto vhat_a = v_a / ( 1 - vs );
      a_ -= ( alpha * mhat_a / ( enoki::sqrt( vhat_a ) + 0.000001f ) );

      m_b = beta1 * m_b + ( 1 - beta1 ) * g_b;
      v_b = beta2 * v_b + ( 1 - beta2 ) * g_b * g_b;
      auto mhat_b = m_b / ( 1 - ms );
      auto vhat_b = v_b / ( 1 - vs );
      b_ -= ( alpha * mhat_b / ( enoki::sqrt( vhat_b ) + 0.000001f ) );

      m_c = beta1 * m_c + ( 1 - beta1 ) * g_c;
      v_c = beta2 * v_c + ( 1 - beta2 ) * g_c * g_c;
      auto mhat_c = m_c / ( 1 - ms );
      auto vhat_c = v_c / ( 1 - vs );
      c_ -= ( alpha * mhat_c / ( enoki::sqrt( vhat_c ) + 0.000001f ) );
      a = a_[ 0 ];
      b = b_[ 0 ];
      c = c_[ 0 ];
      if( !( i % 100 ) )
        std::cout << enoki::hsum( diff * diff ) << " " << a_ << " " << b_ << " " << c_ << std::endl;
      enoki::cuda_eval( true );
      enoki::cuda_sync();

    }
  }
  else {
    for( unsigned int y = 0; y != envelope.size(); ++y ) {
      std::cout << 0.01f * y << " " << envelope[ y ] << std::endl;
    }
  }
}


///////////////////////////////////////////////////////////////////////////////
// weighted_covariance.hpp
//
//  Copyright 2006 Daniel Egloff, Olivier Gygi. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_COVARIANCE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_COVARIANCE_HPP_DE_01_01_2006

#include <vector>
#include <limits>
#include <numeric>
#include <functional>
#include <complex>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/range.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/type_traits/is_scalar.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/covariance.hpp> // for numeric::outer_product() and type traits
#include <boost/accumulators/statistics/weighted_mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_covariance_impl
    //
    /**
        @brief Weighted Covariance Estimator

        An iterative Monte Carlo estimator for the weighted covariance \f$\mathrm{Cov}(X,X')\f$, where \f$X\f$ is a sample
        and \f$X'\f$ a variate, is given by:

        \f[
            \hat{c}_n = \frac{\bar{w}_n-w_n}{\bar{w}_n} \hat{c}_{n-1} + \frac{w_n}{\bar{w}_n-w_n}(X_n - \hat{\mu}_n)(X_n' - \hat{\mu}_n'),
            \quad n\ge2,\quad\hat{c}_1 = 0,
        \f]

        \f$\hat{\mu}_n\f$ and \f$\hat{\mu}_n'\f$ being the weighted means of the samples and variates and
        \f$\bar{w}_n\f$ the sum of the \f$n\f$ first weights \f$w_i\f$.
    */
    template<typename Sample, typename Weight, typename VariateType, typename VariateTag>
    struct weighted_covariance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Weight, typename numeric::functional::average<Sample, std::size_t>::result_type>::result_type weighted_sample_type;
        typedef typename numeric::functional::multiplies<Weight, typename numeric::functional::average<VariateType, std::size_t>::result_type>::result_type weighted_variate_type;
        // for boost::result_of
        typedef typename numeric::functional::outer_product<weighted_sample_type, weighted_variate_type>::result_type result_type;

        template<typename Args>
        weighted_covariance_impl(Args const &args)
          : cov_(
                numeric::outer_product(
                    numeric::average(args[sample | Sample()], (std::size_t)1)
                      * numeric::one<Weight>::value
                  , numeric::average(args[parameter::keyword<VariateTag>::get() | VariateType()], (std::size_t)1)
                      * numeric::one<Weight>::value
                )
            )
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if (cnt > 1)
            {
                extractor<tag::weighted_mean_of_variates<VariateType, VariateTag> > const some_weighted_mean_of_variates = {};

                this->cov_ = this->cov_ * (sum_of_weights(args) - args[weight]) / sum_of_weights(args)
                           + numeric::outer_product(
                                 some_weighted_mean_of_variates(args) - args[parameter::keyword<VariateTag>::get()]
                               , weighted_mean(args) - args[sample]
                             ) * args[weight] / (sum_of_weights(args) - args[weight]);
            }
        }

        result_type result(dont_care) const
        {
            return this->cov_;
        }

    private:
        result_type cov_;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_covariance
//
namespace tag
{
    template<typename VariateType, typename VariateTag>
    struct weighted_covariance
      : depends_on<count, sum_of_weights, weighted_mean, weighted_mean_of_variates<VariateType, VariateTag> >
    {
        typedef accumulators::impl::weighted_covariance_impl<mpl::_1, mpl::_2, VariateType, VariateTag> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_covariance
//
namespace extract
{
    extractor<tag::abstract_covariance> const weighted_covariance = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_covariance)
}

using extract::weighted_covariance;

}} // namespace boost::accumulators

#endif


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Incompressible module (body solver)
// Compute sources on the body surface from the updated  impermeability B.C. and solve for doublets on the body surface
//
// Ref: Katz & Plotkin (2001), Low-Speed Aerodynamics
// 1) tau = - (U_inf + Usigma) \cdot n
// 2) mu = A \ (B * tau)
//
// I/O:
// - vInf: freestream velocity vector
// - RHS: right-hand side of the linear system of equations
// - vSigma: field source induced body velocity
// - bPan: (network of) body panels (structure)
// - b2bAIC: body to body AIC (structure)

#include <iostream>
#include <Eigen/Dense>
#include "solve_body.h"

using namespace std;
using namespace Eigen;

void solve_body(Vector3d &vInf, VectorXd &RHS, MatrixX3d &vSigma, Network &bPan, Body_AIC &b2bAIC) {

    //// Begin
    cout << "Computing surface singularities... " << flush;

    //// Update B.C.
    // BC: tau_i = n_i * V_inf + n_i * V_sigma
    for (int i = 0; i < bPan.nP; ++i)
        bPan.tau(i) = - bPan.n.row(i).dot(vInf.transpose()) - bPan.n.row(i).dot(vSigma.row(i));

    //// Solve
    // Compute RHS
    RHS = - b2bAIC.B * bPan.tau;
    // Solve: A*mu + B*sigma = 0 (Dirichlet: interior potential = 0)
    #ifdef ON_UNIX
        bPan.mu = b2bAIC.A.householderQr().solve(RHS);
    #else
        bPan.mu = b2bAIC.A.fullPivLu().solve(RHS);
    #endif

    //// Control display
    cout << "Done!" << endl;
    cout << "Surface sources strength: " << bPan.tau.norm() << endl;
    #ifdef VERBOSE
        cout << "Sources min. value: " << bPan.tau.minCoeff() << endl;
        cout << "Sources max. value: " << bPan.tau.maxCoeff() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.tau(i) << endl;
    #endif
    cout << "Surface doublets strength: " << bPan.mu.norm() << endl;
    #ifdef VERBOSE
        cout << "Doublets min. value: " << bPan.mu.minCoeff() << endl;
        cout << "Doublets max. value: " << bPan.mu.maxCoeff() << endl;
        for (int i = 0; i < bPan.nP; ++i)
            cout << i << ' ' << bPan.mu(i) << endl;
            cout << "System solved with relative error: " << (b2bAIC.A * bPan.mu - RHS).norm() / RHS.norm() << endl;
    #endif
    cout << endl;
}