/* -------------------------------------------------------------------------- *
 *        OpenSim:  testSmoothSegmentedFunctionFactory.cpp                    *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2012 Stanford University and the Authors                *
 * Author(s): Matthew Millard                                                 *
 *                                                                            *
 * 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.                                             *
 * -------------------------------------------------------------------------- */
/*
    Update:
     This is a port of the original code so that it will work with
     the multibody code RBDL written by Martin Felis.
    
     This also includes additional curves (the Anderson2007 curves)
     which are not presently in OpenSim.

    Author:
     Matthew Millard
    
    Date:
     Nov 2015

*/
/* 
Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory
class, a class that enables the easy generation of C2 continuous curves 
that define the various characteristic curves required in a muscle model
 */

// Author:  Matthew Millard

//==============================================================================
// INCLUDES
//==============================================================================

#include "../geometry.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>

using namespace RigidBodyDynamics::Addons::Geometry;

using namespace std;

static double EPSILON = numeric_limits<double>::epsilon();
static double SQRTEPSILON = sqrt(EPSILON);
static double TOL = std::numeric_limits<double>::epsilon()*1e4;

static bool FLAG_PLOT_CURVES    = false;
static string FILE_PATH         = "";
static double TOL_DX            = 5e-3;
static double TOL_DX_BIG        = 1e-2;
static double TOL_BIG           = 1e-6;
static double TOL_SMALL         = 1e-12;

/**
This function will print cvs file of the column vector col0 and the matrix 
 data

@params col0: A vector that must have the same number of rows as the data matrix
       This column vector is printed as the first column
@params data: A matrix of data
@params filename: The name of the file to print
*/
void printMatrixToFile(
    const RigidBodyDynamics::Math::VectorNd& col0, 
    const RigidBodyDynamics::Math::MatrixNd& data, 
    string& filename);


/**
This function will print cvs file of the matrix 
 data

@params data: A matrix of data
@params filename: The name of the file to print
*/
void printMatrixToFile( 
    const RigidBodyDynamics::Math::MatrixNd& data, 
    string& filename);


/**
    This function computes a standard central difference dy/dx. If 
    extrap_endpoints is set to 1, then the derivative at the end points is 
    estimated by linearly extrapolating the dy/dx values beside the end points

 @param x domain vector
 @param y range vector
 @param extrap_endpoints: (false)   Endpoints of the returned vector will be 
               zero, because a central difference
               is undefined at these endpoints
             (true)  Endpoints are computed by linearly 
               extrapolating using a first difference from 
               the neighboring 2 points
 @returns dy/dx computed using central differences
*/
RigidBodyDynamics::Math::VectorNd 
    calcCentralDifference(  RigidBodyDynamics::Math::VectorNd& x, 
             RigidBodyDynamics::Math::VectorNd& y, 
             bool extrap_endpoints);

/**
    This function computes a standard central difference dy/dx at each point in
    a vector x, for a SmoothSegmentedFunction mcf, to a desired tolerance. This 
    function will take the best step size at each point to minimize the 
    error caused by taking a numerical derivative, and the error caused by
    numerical rounding error:

    For a step size of h/2 to the left and to the right of the point of 
    interest the error is
    error = 1/4*h^2*c3 + r*f(x)/h,         (1)
      
    Where c3 is the coefficient of the 3rd order Taylor series expansion
    about point x. Thus c3 can be computed if the order + 2 derivative is
    known
     
     c3 = (d^3f(x)/dx^3)/(6)            (2)
     
    And r*f(x)/h is the rounding error that occurs due to the central 
    difference.

    Taking a first derivative of 1 and solving for h yields

    h = (r*f(x)*2/c3)^(1/3)

    Where r is EPSILON

  @param x domain vector
  @param mcf the SmoothSegmentedFunction of interest
  @param order the order of the numerical derivative
  @param tolerance desired tolerance on the returned result
  @returns dy/dx computed using central differences
*/
RigidBodyDynamics::Math::VectorNd 
calcCentralDifference(  RigidBodyDynamics::Math::VectorNd& x, 
            SmoothSegmentedFunction& mcf,
            double tol, int order);

/**
    This function tests numerically for continuity of a curve. The test is 
    performed by taking a point on the curve, and then two points (called the 
    shoulder points) to the left and right of the point in question. The 
    shoulder points are located half way between the sample points in xV.

    The value of the function's derivative is evaluated at each of the shoulder 
    points and used to linearly extrapolate from the shoulder points back to the 
    original point. If the original point and the linear extrapolations of each 
    of the shoulder points agree within a tolerance, then the curve is assumed 
    to be continuous. This tolerance is evaluated to be the smaller of the 2nd 
    derivative times a multiplier (taylorErrorMult) or minValueSecondDerivative


    @param x     Values to test for continuity

    @param yx    The SmoothSegmentedFunction to test

    @param order The order of the curve of SmoothSegmentedFunction to test
        for continuity

    @param minValueSecondDerivative the minimum value allowed for the 2nd 
           term in the Taylor series. Here we need to define a minimum because
           this 2nd term is used to define a point specific tolerance for the
           continuity test. If the 2nd derivative happens to go to zero, we 
           still cannot let the minimum error tolerance to go to zero - else
           the test will fail on otherwise good curves. 

    @param taylorErrorMult  This scales the error tolerance. The default error
             tolerance is the the 2nd order Taylor series term. This term is
             dependent on the size of the higher-order-terms and the sample 
             spacing used for xV (since the shoulder points are picked half-
             way between the sampled points)
*/
bool isFunctionContinuous(  RigidBodyDynamics::Math::VectorNd& xV, 
                            SmoothSegmentedFunction& yV, 
                            int order, 
                            double minValueSecondDerivative,
                            double taylorErrorMult);


/**
This function will scan through a vector and determine if it is monotonic or
not

@param y the vector of interest
@param multEPS The tolerance on the monotoncity check, expressed as a scaling of
       EPSILON
@return true if the vector is monotonic, false if it is not
*/
bool isVectorMonotonic( RigidBodyDynamics::Math::VectorNd& y, 
                        int multEPS);


/**
This function will compute the numerical integral of y(x) using the trapezoidal
method

@param x the domain vector
@param y the range vector, of y(x), evaluated at x
@param flag_TrueIntForward_FalseIntBackward 
    When this flag is set to true, the integral of y(x) will be evaluated from
    left to right, starting with int(y(0)) = 0. When this flag is false, then
    y(x) will be evaluated from right to left with int(y(n)) = 0, where n is 
    the maximum number of elements.                    
@return the integral of y(x)
*/
RigidBodyDynamics::Math::VectorNd calcTrapzIntegral(
            RigidBodyDynamics::Math::VectorNd& x, 
            RigidBodyDynamics::Math::VectorNd& y, 
            bool flag_TrueIntForward_FalseIntBackward);


/**
   @param a The first vector
   @param b The second vector
   @return Returns the maximum absolute difference between vectors a and b
*/
double calcMaximumVectorError(RigidBodyDynamics::Math::VectorNd& a, 
                              RigidBodyDynamics::Math::VectorNd& b);


/*
This function tests the SmoothSegmentedFunction to see if it is C2 continuous. 
This function works by using the applying the function isFunctionContinuous 
multiple times. For details of the method used please refer to the doxygen for
isFunctionContinuous.

@param mcf - a SmoothSegmentedFunction
@param mcfSample: 
  A n-by-m matrix of values where the first column is the domain (x) of interest
  and the remaining columns are the curve value (y), and its derivatives (dy/dx,
  d2y/dx2, d3y/dx3, etc).  This matrix is returned by the function 
  calcSampledCurve in SmoothSegmented Function.  
@param continuityTol  
@return bool: true if the curve is C2 continuous to the desired tolernace
              
*/ 
bool isCurveC2Continuous(SmoothSegmentedFunction& mcf,
              RigidBodyDynamics::Math::MatrixNd& mcfSample,
              double continuityTol);

/*
 4. The MuscleCurveFunctions which are supposed to be monotonic will be
    tested for monotonicity.
*/
bool isCurveMontonic(RigidBodyDynamics::Math::MatrixNd mcfSample);

/**
This function compares the i^th derivative return by a SmoothSegmented function 
against a numerical derivative computed using a central difference applied to 
the (i-1)^th derivative of the function. This function first checks the 
1st derivative and continues checking derivatives until the 4th derivative. 

@param mcf - a SmoothSegmentedFunction
@param mcfSample: 
  A n-by-m matrix of values where the first column is the domain (x) of interest
  and the remaining columns are the curve value (y), and its derivatives (dy/dx,
  d2y/dx2, d3y/dx3, etc). This matrix is returned by the function 
  calcSampledCurve in SmoothSegmented Function.

@param tol : the tolerance used to assess the relative error between the 
             numerically computed derivatives and the derivatives returned by
             the SmoothSegmentedFunction
@return bool: true if all of the derivatives up to the 4th (hard coded) are 
              within a relative tolerance of tol w.r.t. to the numerically 
              computed derivatives
*/
bool areCurveDerivativesCloseToNumericDerivatives(
     SmoothSegmentedFunction& mcf,
     RigidBodyDynamics::Math::MatrixNd& mcfSample,
     double tol);
