/* -------------------------------------------------------------------------- *
 *        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 MuscleFunctionFactory
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/geometry.h"
#include "../MuscleFunctionFactory.h"
#include "../../geometry/tests/numericalTestFunctions.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 RigidBodyDynamics::Addons::Muscle;
using namespace std;
//using namespace OpenSim;
//using namespace SimTK;

/*
static double EPSILON = numeric_limits<double>::epsilon();

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



TEST(tendonCurve)
{
    //cout <<"**************************************************"<<endl;
    //cout <<"TENDON CURVE TESTING            "<<endl;
    double e0   = 0.04;
    double kiso = 1.5/e0;
    double c    = 0.5;//0.75;    
    double ftoe = 1.0/3.0;

    SmoothSegmentedFunction tendonCurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::createTendonForceLengthCurve(
         e0,kiso,ftoe,c, "test_tendonCurve", tendonCurve);
    

    RigidBodyDynamics::Math::MatrixNd tendonCurveSample
     =tendonCurve.calcSampledCurve(6,1.0,1+e0);
    //tendonCurve.printMuscleCurveToCSVFile(FILE_PATH);

//0. Test that each curve fulfills its contract at the end points.
    //cout << "   Keypoint Testing" << endl;
    RigidBodyDynamics::Math::VectorNd tendonCurveDomain = 
     tendonCurve.getCurveDomain();
    CHECK(abs(tendonCurve.calcValue(tendonCurveDomain[0]))<TOL_SMALL);
    CHECK(abs(tendonCurve.calcValue(tendonCurveDomain[1])-ftoe)<TOL_SMALL);

    CHECK(abs(tendonCurve.calcValue(1.0)    )< TOL_SMALL);  
    CHECK(abs(tendonCurve.calcDerivative(1.0,1))< TOL_BIG);
    CHECK(abs(tendonCurve.calcDerivative(1.0,2))< TOL_BIG);

    CHECK(abs(tendonCurve.calcValue(1+e0)    -1.0 ) < TOL_SMALL);  
    CHECK(abs(tendonCurve.calcDerivative(1+e0,1)-kiso) < TOL_BIG);
    CHECK(abs(tendonCurve.calcDerivative(1+e0,2)-0   ) < TOL_BIG);
    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
      areCurveDerivativesCloseToNumericDerivatives(
       tendonCurve,
       tendonCurveSample,TOL_DX_BIG);

    CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(tendonCurve, tendonCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(tendonCurve,
                                                 tendonCurveSample,
                                                 TOL_BIG);

    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
    bool curveIsMonotonic = isCurveMontonic(tendonCurveSample);
    CHECK(curveIsMonotonic);

    if(FLAG_PLOT_CURVES){
     tendonCurve.printCurveToCSVFile(FILE_PATH,
                 "tendonCurve",
                 1.0-(e0/10),
                 1+e0);
    }
    //cout << "    passed" << endl;

}

TEST(activeForceLengthCurve)
{
    //cout << endl;
    //cout << endl;
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER ACTIVE FORCE LENGTH CURVE TESTING     "<<endl;
    double lce0 = 0.4;
    double lce1 = 0.75;
    double lce2 = 1;
    double lce3 = 1.6;
    double shoulderVal  = 0.05;
    double plateauSlope = 0.75;//0.75;
    double curviness    = 0.75;
    SmoothSegmentedFunction fiberfalCurve = SmoothSegmentedFunction();

    MuscleFunctionFactory::
     createFiberActiveForceLengthCurve(lce0, lce1, lce2, lce3, 
          shoulderVal, plateauSlope, curviness,
          "test_fiberActiveForceLengthCurve", fiberfalCurve);
    //fiberfalCurve.printMuscleCurveToCSVFile(FILE_PATH);

    RigidBodyDynamics::Math::MatrixNd fiberfalCurveSample 
        = fiberfalCurve.calcSampledCurve(6,0,lce3);

    //0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    CHECK(abs(fiberfalCurve.calcValue(lce0) - shoulderVal) < TOL_SMALL);  
    CHECK(abs(fiberfalCurve.calcDerivative(lce0,1))     < TOL_BIG);
    CHECK(abs(fiberfalCurve.calcDerivative(lce0,2))     < TOL_BIG);
   
    //lce2 isn't the location of the end of a quintic Bezier curve
    //so I can't actually do any testing on this point.
    //SimTK_TEST_EQ_TOL(fiberfalCurve.calcValue(lce0),shoulderVal,TOL_SMALL);  
    //SimTK_TEST_EQ_TOL(
    //fiberfalCurve.calcDerivative(lce2,1),plateauSlope,TOL_BIG);
    //SimTK_TEST_EQ_TOL(fiberfalCurve.calcDerivative(lce2,2),0.0,TOL_BIG);

    CHECK(abs(fiberfalCurve.calcValue(lce2) - 1.0)    <  TOL_SMALL);  
    CHECK(abs(fiberfalCurve.calcDerivative(lce2,1))   <  TOL_BIG);
    CHECK(abs(fiberfalCurve.calcDerivative(lce2,2))   <  TOL_BIG);
    CHECK(abs(fiberfalCurve.calcValue(lce3)-shoulderVal) <  TOL_SMALL);
    CHECK(abs(fiberfalCurve.calcDerivative(lce3,1))   <  TOL_BIG);
    CHECK(abs(fiberfalCurve.calcDerivative(lce3,2))   <  TOL_BIG);

    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberfalCurve,
      fiberfalCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberfalCurve,fiberfalCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(fiberfalCurve,
                                                 fiberfalCurveSample,
                                                 TOL_BIG);
    CHECK(curveIsContinuous);

    //fiberfalCurve.MuscleCurveToCSVFile("C:/mjhmilla/Stanford/dev");

    if(FLAG_PLOT_CURVES){
     fiberfalCurve.printCurveToCSVFile(FILE_PATH,
                  "fiberFalCurve",
                  0.3,
                  1.8);
    }


}


TEST(ForceVelocityCurve)
{
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER FORCE VELOCITY CURVE TESTING       "<<endl;

    double fmaxE     = 1.8;
    double dydxC     = 0.1;
    double dydxNearC    = 0.15;
    double dydxE     = 0.1;
    double dydxNearE    = 0.1+0.0001;
    double dydxIso   = 5;
    double concCurviness= 0.1;
    double eccCurviness = 0.75;

    SmoothSegmentedFunction fiberFVCurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::
     createFiberForceVelocityCurve(fmaxE, 
            dydxC,
            dydxNearC,
            dydxIso,
            dydxE,
            dydxNearE,
            concCurviness,
            eccCurviness,
            "test_fiberForceVelocityCurve",
            fiberFVCurve);
    //fiberFVCurve.printMuscleCurveToCSVFile(FILE_PATH);

    RigidBodyDynamics::Math::MatrixNd fiberFVCurveSample 
        = fiberFVCurve.calcSampledCurve(6,-1.0,1.0);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    assert(abs(fiberFVCurve.calcValue(-1)      ) < TOL_SMALL);  
    assert(abs(fiberFVCurve.calcDerivative(-1,1)-dydxC  ) < TOL_BIG  );
    assert(abs(fiberFVCurve.calcDerivative(-1,2)     ) < TOL_BIG  );
    assert(abs(fiberFVCurve.calcValue(0)    -1.0  ) < TOL_SMALL);  
    assert(abs(fiberFVCurve.calcDerivative(0,1)-dydxIso ) < TOL_BIG  );
    assert(abs(fiberFVCurve.calcDerivative(0,2)      ) < TOL_BIG  );
    assert(abs(fiberFVCurve.calcValue(1)    -fmaxE   ) < TOL_SMALL);  
    assert(abs(fiberFVCurve.calcDerivative(1,1)-dydxE   ) < TOL_BIG  );
    assert(abs(fiberFVCurve.calcDerivative(1,2)      ) < TOL_BIG  );

    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberFVCurve,
      fiberFVCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);

//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberFVCurve,fiberFVCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(
                                fiberFVCurve,
                                fiberFVCurveSample,
                                TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate

    isCurveMontonic(fiberFVCurveSample);
    CHECK(curveIsContinuous);

    if(FLAG_PLOT_CURVES){
     fiberFVCurve.printCurveToCSVFile(FILE_PATH,
                 "fiberFvCurve",
                 -1.1,
                  1.1);
    }

    //cout << "    passed" << endl;

}

TEST(ForceVelocityInverseCurve)
{
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER FORCE VELOCITY INVERSE CURVE TESTING     "<<endl;
    double fmaxE     = 1.8;
    double dydxC     = 0.1;
    double dydxNearC    = 0.15;
    double dydxE     = 0.1;
    double dydxNearE    = 0.1+0.0001;
    double dydxIso   = 5;
    double concCurviness= 0.1;
    double eccCurviness = 0.75;

    SmoothSegmentedFunction fiberFVInvCurve = SmoothSegmentedFunction();    
    MuscleFunctionFactory::
      createFiberForceVelocityInverseCurve( 
        fmaxE, 
        dydxC, 
        dydxNearC, 
        dydxIso, 
        dydxE, 
        dydxNearE,
        concCurviness,  
        eccCurviness,
        "test_fiberForceVelocityInverseCurve",
        fiberFVInvCurve);

    SmoothSegmentedFunction fiberFVCurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::
     createFiberForceVelocityCurve(
      fmaxE, 
      dydxC,
      dydxNearC,
      dydxIso,
      dydxE,
      dydxNearE,
      concCurviness,
      eccCurviness,
      "test_fiberForceVelocityCurve",
      fiberFVCurve);    
    //fiberFVInvCurve.printMuscleCurveToFile(FILE_PATH);
    

    RigidBodyDynamics::Math::MatrixNd fiberFVCurveSample
     = fiberFVCurve.calcSampledCurve(6, -1.0, 1.0);

    RigidBodyDynamics::Math::MatrixNd fiberFVInvCurveSample 
        = fiberFVInvCurve.calcSampledCurve(6,0,fmaxE);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    CHECK(abs( fiberFVInvCurve.calcValue(0)    +1    ) < TOL_SMALL);
    CHECK(abs( fiberFVInvCurve.calcDerivative(0,1)-1/dydxC    ) < TOL_BIG);
    CHECK(abs( fiberFVInvCurve.calcDerivative(0,2)      ) < TOL_BIG);               
    CHECK(abs( fiberFVInvCurve.calcValue(1)          ) < TOL_SMALL);  
    CHECK(abs( fiberFVInvCurve.calcDerivative(1,1)-1/dydxIso  ) < TOL_BIG);
    CHECK(abs( fiberFVInvCurve.calcDerivative(1,2)      ) < TOL_BIG);                
    CHECK(abs( fiberFVInvCurve.calcValue(fmaxE)   -1    ) < TOL_SMALL);  
    CHECK(abs( fiberFVInvCurve.calcDerivative(fmaxE,1)-1/dydxE) < TOL_BIG);
    CHECK(abs( fiberFVInvCurve.calcDerivative(fmaxE,2)     ) <  TOL_BIG);

    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberFVInvCurve,
      fiberFVInvCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);

//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberFVInvCurve,fiberFVInvCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(
                                fiberFVInvCurve,
                                fiberFVInvCurveSample,
                                TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate

    bool curveIsMonotonic = isCurveMontonic(fiberFVInvCurveSample);
    CHECK(curveIsMonotonic);

//5. Testing the inverse of the curve - is it really an inverse?
    //cout << endl;
    //cout << "   TEST: Inverse correctness:fv(fvinv(fv)) = fv" << endl;
    double fv = 0;
    double dlce = 0;
    double fvCalc = 0;
    double fvErr = 0;
    double fvErrMax = 0;
    for(int i = 0; i < fiberFVInvCurveSample.rows(); i++){
     fv   = fiberFVCurveSample(i,0);
     dlce    = fiberFVInvCurve.calcValue(fv);
     fvCalc  = fiberFVCurve.calcValue(dlce);
     fvErr   = abs(fv-fvCalc);
     if(fvErrMax < fvErr)
      fvErrMax = fvErr;

     CHECK( fvErr < TOL_BIG);
    }

    if(FLAG_PLOT_CURVES){
     fiberFVInvCurve.printCurveToCSVFile(FILE_PATH,
                  "fiberFvInvCurve",
                  -0.1,
                  fmaxE+0.1);
    }

    //printf("   passed with a maximum error of %fe-12",fvErrMax*1e12);

}

TEST(passiveForceLengthCurve)
{
    double e0f   = 0.6;
    double kisof    = 8.389863790885878;
    double cf    = 0.65;
    double klow  = 0.5*(1.0/e0f);

    SmoothSegmentedFunction fiberFLCurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::
             createFiberForceLengthCurve(
              0.0, e0f,klow,kisof,cf,
             "test_fiberForceLength",
             fiberFLCurve);

    RigidBodyDynamics::Math::MatrixNd fiberFLCurveSample 
        = fiberFLCurve.calcSampledCurve(6,1.0,1.0+e0f);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    RigidBodyDynamics::Math::VectorNd fiberFLCurveDomain 
    = fiberFLCurve.getCurveDomain();

    CHECK(abs(fiberFLCurveDomain[0] -1)     < TOL_SMALL);
    CHECK(abs(fiberFLCurveDomain[1] - (1+e0f)) < TOL_SMALL);
    CHECK(abs(fiberFLCurve.calcValue(1.0)  )   < TOL_SMALL);  
    CHECK(abs(fiberFLCurve.calcDerivative(1.0,1)) < TOL_BIG);
    CHECK(abs(fiberFLCurve.calcDerivative(1.0,2)) < TOL_BIG);
    CHECK(abs(fiberFLCurve.calcValue(1+e0f) -1.0)      < TOL_SMALL);  
    CHECK(abs(fiberFLCurve.calcDerivative(1+e0f,1)-kisof) < TOL_BIG);
    CHECK(abs(fiberFLCurve.calcDerivative(1+e0f,2))    < TOL_BIG);
    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberFLCurve,
      fiberFLCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberFLCurve,fiberFLCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(
                              fiberFLCurve,
                              fiberFLCurveSample,
                              TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate

    bool curveIsMonotonic = isCurveMontonic(fiberFLCurveSample);
    CHECK(curveIsMonotonic);

    if(FLAG_PLOT_CURVES){
     fiberFLCurve.printCurveToCSVFile(FILE_PATH,
                 "fiberFLCurve",
                 1.0-0.1,
                 1.0+e0f+0.1);
    }
}

TEST(compressiveForceLengthCurve)
{
///////////////////////////////////////
//FIBER COMPRESSIVE FORCE LENGTH
///////////////////////////////////////
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER COMPRESSIVE FORCE LENGTH CURVE TESTING   "<<endl;


    double lmax = 0.6;
    double kce  = -8.389863790885878;
    double cce  = 0.5;//0.0;

    SmoothSegmentedFunction fiberCECurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::
    createFiberCompressiveForceLengthCurve(
      lmax,
      kce,
      cce,
      "test_fiberCompressiveForceLengthCurve",
      fiberCECurve);

    //fiberCECurve.printMuscleCurveToFile("C:/mjhmilla/Stanford/dev"
    //    "/OpenSim_LOCALPROJECTS/MuscleLibrary_Bench_20120210/build");
    RigidBodyDynamics::Math::MatrixNd fiberCECurveSample 
        = fiberCECurve.calcSampledCurve(6,0,lmax);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    RigidBodyDynamics::Math::VectorNd fiberCECurveDomain 
     = fiberCECurve.getCurveDomain();
    CHECK(abs(fiberCECurveDomain[0])          < TOL_SMALL);
    CHECK(abs(fiberCECurveDomain[1]- lmax)       < TOL_SMALL);
    CHECK(abs(fiberCECurve.calcValue(lmax))      < TOL_SMALL);  
    CHECK(abs(fiberCECurve.calcDerivative(lmax,1))  < TOL_BIG);
    CHECK(abs(fiberCECurve.calcDerivative(lmax,2))  < TOL_BIG);
    CHECK(abs(fiberCECurve.calcValue(0) - 1.0)      < TOL_SMALL);  
    CHECK(abs(fiberCECurve.calcDerivative(0,1)-kce)    < TOL_BIG);
    CHECK(abs(fiberCECurve.calcDerivative(0,2))     < TOL_BIG);
    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberCECurve,
      fiberCECurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);

//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberCECurve,fiberCECurveSample);

//3. Test numerically to see if the curve is C2 continuous
    bool curveIsContinuous = isCurveC2Continuous(
                              fiberCECurve,
                              fiberCECurveSample,
                              TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate

    bool curveIsMonotonic = isCurveMontonic(fiberCECurveSample);
    CHECK(curveIsMonotonic);

//5. Testing Exceptions
    //cout << endl;
    //cout << "   Exception Testing" << endl;

    if(FLAG_PLOT_CURVES){
     fiberCECurve.printCurveToCSVFile(FILE_PATH,
                 "fiberCECurve",
                 -0.1,
                 lmax + 0.1);
    }

    //cout << "    passed" << endl;

}

TEST(compressivePhiCurve)
{
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER COMPRESSIVE FORCE PHI CURVE TESTING   "<<endl;

    double phi0 = (M_PI/2)*(1.0/2.0);
    double phi1 = M_PI/2;
    double kphi  = 8.389863790885878;
    double cphi  = 0.0;  

    SmoothSegmentedFunction fiberCEPhiCurve = SmoothSegmentedFunction();
    MuscleFunctionFactory::
     createFiberCompressiveForcePennationCurve(phi0,kphi,cphi,
         "test_fiberCompressiveForcePennationCurve", fiberCEPhiCurve);    


    RigidBodyDynamics::Math::MatrixNd fiberCEPhiCurveSample 
        = fiberCEPhiCurve.calcSampledCurve(6,phi0,phi1);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    CHECK(abs(fiberCEPhiCurve.calcValue(phi0))     < TOL_SMALL);  
    CHECK(abs(fiberCEPhiCurve.calcDerivative(phi0,1)) < TOL_BIG);
    CHECK(abs(fiberCEPhiCurve.calcDerivative(phi0,2)) < TOL_BIG);
    CHECK(abs(fiberCEPhiCurve.calcValue(phi1)) -1     < TOL_SMALL);
    CHECK(abs(fiberCEPhiCurve.calcDerivative(phi1,1)-kphi)  < TOL_BIG);
    CHECK(abs(fiberCEPhiCurve.calcDerivative(phi1,2))    < TOL_BIG);
    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberCEPhiCurve,
      fiberCEPhiCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberCEPhiCurve,fiberCEPhiCurveSample);

//3. Test numerically to see if the curve is C2 continuous
    //cout << "Attention: Removed test for the Cos Phi Compressive Curve"<<endl;
    bool curveIsContinuous = isCurveC2Continuous(
                                fiberCEPhiCurve,
                                fiberCEPhiCurveSample,
                                TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate
    bool curveIsMonotonic = isCurveMontonic(fiberCEPhiCurveSample);
    CHECK(curveIsMonotonic);


    if(FLAG_PLOT_CURVES){
     fiberCEPhiCurve.printCurveToCSVFile(FILE_PATH,
                 "fiberCEPhiCurve",
                 phi0-0.1,
                 phi1+0.1);
    }

    //cout << "    passed" << endl;

}

TEST(compressiveCosPhiCurve)
{
    //cout <<"**************************************************"<<endl;
    //cout <<"FIBER COMPRESSIVE FORCE COSPHI CURVE TESTING   "<<endl;

    double cosPhi0 = cos( (80.0/90.0)*M_PI/2);
    double kcosPhi  = -1.2/(cosPhi0);
    double ccosPhi  = 0.5;
    SmoothSegmentedFunction fiberCECosPhiCurve = SmoothSegmentedFunction();

    MuscleFunctionFactory::
        createFiberCompressiveForceCosPennationCurve(
           cosPhi0,kcosPhi,ccosPhi,
           "test_fiberCompressiveForceCosPennationCurve",
           fiberCECosPhiCurve);

    RigidBodyDynamics::Math::MatrixNd fiberCECosPhiCurveSample 
        = fiberCECosPhiCurve.calcSampledCurve(6,0,cosPhi0);

//0. Test that each curve fulfills its contract.
    //cout << "   Keypoint Testing" << endl;

    CHECK(abs(fiberCECosPhiCurve.calcValue(cosPhi0)    )  < TOL_SMALL);  
    CHECK(abs(fiberCECosPhiCurve.calcDerivative(cosPhi0,1)   )  < TOL_BIG);
    CHECK(abs(fiberCECosPhiCurve.calcDerivative(cosPhi0,2)   )  < TOL_BIG);
    CHECK(abs(fiberCECosPhiCurve.calcValue(0)    - 1.0    )  < TOL_SMALL);  
    CHECK(abs(fiberCECosPhiCurve.calcDerivative(0,1) -kcosPhi)  < TOL_BIG);
    CHECK(abs(fiberCECosPhiCurve.calcDerivative(0,2)      )  < TOL_BIG);
    //cout << "   passed" << endl;
    //cout << endl;
//1. Test each derivative sample for correctness against a numerically
//   computed version
    bool areCurveDerivativesGood = 
     areCurveDerivativesCloseToNumericDerivatives(
      fiberCECosPhiCurve,
      fiberCECosPhiCurveSample,
      TOL_DX);
    CHECK(areCurveDerivativesGood);
//2. Test each integral, where computed for correctness.
    //testMuscleCurveIntegral(fiberCECosPhiCurve,fiberCECosPhiCurveSample);

//3. Test numerically to see if the curve is C2 continuous

    bool curveIsContinuous = isCurveC2Continuous(
                                fiberCECosPhiCurve,
                                fiberCECosPhiCurveSample,
                                TOL_BIG);
    CHECK(curveIsContinuous);
//4. Test for montonicity where appropriate

    bool curveIsMonotonic = isCurveMontonic(fiberCECosPhiCurveSample);
    CHECK(curveIsMonotonic);


    //cout << "    passed" << endl;
    if(FLAG_PLOT_CURVES){
     fiberCECosPhiCurve.printCurveToCSVFile(FILE_PATH,
                 "fiberCECosPhiCurve",
                 -0.1,
                 cosPhi0+0.1);
    }

}