/* -------------------------------------------------------------------------- *
 *           OpenSim:  SmoothSegmentedFunction.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.
  
  Author:
    Matthew Millard
  
  Date:
    Nov 2015

*/
    
//=============================================================================
// INCLUDES
//=============================================================================
#include "SmoothSegmentedFunction.h"
#include <fstream>
#include <ostream>

//=============================================================================
// STATICS
//=============================================================================
//using namespace SimTK;
//using namespace OpenSim;
using namespace std;
using namespace RigidBodyDynamics::Addons::Geometry;

static bool     DEBUG     = false;
static double   UTOL      = std::numeric_limits<double>::epsilon()*1e6;
static double   INTTOL    = std::numeric_limits<double>::epsilon()*1e2;
static double   SQRTEPS   = std::sqrt(numeric_limits<double>::epsilon());
static int      MAXITER   = 20;
static int      NUM_SAMPLE_PTS  = 100;
//=============================================================================
// UTILITY FUNCTIONS
//=============================================================================
/*
 DETAILED COMPUTATIONAL COSTS:
 =========================================================================
     WITHOUT INTEGRAL
     _________________________________________________________________________
            Function  Comp  Div   Mult  Add   Assignments 
     _________________________________________________________________________
    member assign                         M:2, 9
    curve gen:    m,m*100   m     m*100       m     m*100*(4) 
                 +m*100(3)          +m*100*(3)

    Function detail
      Evaluations Function
      m       SimTK::SplineFitter<double>::
              fitForSmoothingParameter(3,x,u,0).getSpline();
      Cost:     ?

      m*100     SegmentedQuinticBezierToolkit::
              calcQuinticBezierCurveVal
      Cost:                   Mult   Add   Assignments
                          21     20  13     

    Total       ~typically  > 2100*m multiplications, additions,
                  > 1000*m assignments
                  > 100*m divisions
     _________________________________________________________________________  
          Comp    Div   Mult    Add     Assignments
    Total:    m+m*100(3)  m*100   m*100*21  m*100*20  m*100*13
                          +m+m*100*3  +m*100*4+9+M:2               
          + m*Cost(SimTK::SplineFitter ...)
     =========================================================================
    ADDITIONAL COST OF COMPUTING THE INTEGRAL CURVE
    
                 Comp Div   Mult  Add    Assign     
     RK45 Fn.Eval   m*100*(156  12    618   390    456)
     RK45 Overhead  m*100*(?  ?     ?   ?     ? )
     Spline cost    m*100*(?  ?     ?   ?     ? )

     Total:       ~typically > 100,000's mult, additions, assignments
                   > 40,000 comparisions 
                   > 3000 divisions

     =========================================================================
    M: Matrix
    V: Vector

    N.B. These costs are dependent on SegmentedQuinticBezierToolkit
*/
SmoothSegmentedFunction::
  SmoothSegmentedFunction(
    const RigidBodyDynamics::Math::MatrixNd& mX, 
    const RigidBodyDynamics::Math::MatrixNd& mY,  
      double x0, double x1, 
      double y0, double y1,
      double dydx0, double dydx1,
    const std::string& name):
    _x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1),
    _name(name)
{
  

  _numBezierSections = mX.cols();
  _mXVec.resize(_numBezierSections);
  _mYVec.resize(_numBezierSections);
  for(int s=0; s < _numBezierSections; s++){
    _mXVec[s] = mX.col(s);
    _mYVec[s] = mY.col(s);
  }
}

//==============================================================================
 SmoothSegmentedFunction::SmoothSegmentedFunction():
    _x0(NAN),_x1(NAN),
    _y0(NAN),_y1(NAN),
    _dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET")
 {
    //_arraySplineUX.resize(0);    
    _mXVec.resize(0);
    _mYVec.resize(0);
    //_splineYintX = SimTK::Spline();
    _numBezierSections = (int)NAN;     
 }

//==============================================================================
void SmoothSegmentedFunction::
  updSmoothSegmentedFunction(
    const RigidBodyDynamics::Math::MatrixNd& mX, 
    const RigidBodyDynamics::Math::MatrixNd& mY,  
      double x0, double x1, 
      double y0, double y1,
      double dydx0, double dydx1,
      const std::string& name)
{

  if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ){
    cerr<<"SmoothSegmentedFunction::updSmoothSegmentedFunction "
         << _name.c_str()
        <<": matrices mX and mY must have 6 rows, and the same"
        <<" number of columns."
        << endl;
    assert(0);
    abort();
  }

  _x0   =x0;
  _x1   =x1;
  _y0   =y0;
  _y1   =y1;
  _dydx0=dydx0;
  _dydx1=dydx1;
    
  if(mX.cols() != _mXVec.size()){
    _mXVec.resize(mX.cols());
    _mYVec.resize(mY.cols());
  }

  _numBezierSections = mX.cols();
  for(int s=0; s < mX.cols(); s++){
    _mXVec[s] = mX.col(s);
    _mYVec[s] = mY.col(s);
  }

  _name = name;
}
//==============================================================================
void SmoothSegmentedFunction::shift(double xShift, double yShift)
{
  _x0 += xShift;
  _x1 += xShift;
  _y0 += yShift;
  _y1 += yShift;

  for(int i=0; i<_mXVec.size();++i){
    for(int j=0; j<_mXVec.at(i).rows();++j){
      _mXVec.at(i)[j] += xShift;
      _mYVec.at(i)[j] += yShift;
    }
  }

}

void SmoothSegmentedFunction::scale(double xScale, double yScale)
{

  if( abs( xScale ) <= SQRTEPS){
    cerr<<"SmoothSegmentedFunction::scale "
         << _name.c_str()
        <<": xScale must be greater than sqrt(eps). Setting xScale to such"
        <<" a small value will cause the slope of the curve to approach "
        <<" infinity, or become undefined."
        << endl;
    assert(0);
    abort();
  }

  _x0 *= xScale;
  _x1 *= xScale;
  _y0 *= yScale;
  _y1 *= yScale;
  _dydx0 *= yScale/xScale;
  _dydx1 *= yScale/xScale;

  for(int i=0; i<_mXVec.size();++i){
    for(int j=0; j<_mXVec.at(i).rows();++j){
      _mXVec.at(i)[j] *= xScale;
      _mYVec.at(i)[j] *= yScale;
    }
  }
  
}


//==============================================================================


 /*Detailed Computational Costs
 ________________________________________________________________________
  If x is in the Bezier Curve
              Name   Comp.   Div.  Mult.   Add.  Assign.
_______________________________________________________________________
    SegmentedQuinticBezierToolkit::
            calcIndex   3*m+2           1*m   3   
              *calcU   15    2    82    42    60  
    calcQuinticBezierCurveVal           21    20    13
              total  15+3*m+2  2    103   62+1*m  76

    *Approximate. Uses iteration
________________________________________________________________________
If x is in the linear region

            Name   Comp.   Div.  Mult.   Add.  Assign.
                  1         1    2   1
________________________________________________________________________
 
 */

double SmoothSegmentedFunction::calcValue(double x) const
{
  double yVal = 0;
  if(x >= _x0 && x <= _x1 )
  {
    int idx  = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec);
    double u = SegmentedQuinticBezierToolkit::
         calcU(x,_mXVec[idx], UTOL, MAXITER);
    yVal = SegmentedQuinticBezierToolkit::
         calcQuinticBezierCurveVal(u,_mYVec[idx]);
  }else{
    if(x < _x0){
      yVal = _y0 + _dydx0*(x-_x0);      
    }else{
      yVal = _y1 + _dydx1*(x-_x1);          
    }  
  }

  return yVal;
}


double SmoothSegmentedFunction::calcInverseValue(double y,
                                                 double xGuess) const
{

  double xVal = 0;

  int idx = -1;
  double yLeft  = 0.;
  double yRight = 0;
  double xLeft  = 0.;
  double xRight = 0;
  double xDist  = 0;
  double xDistBest  = numeric_limits<double>::infinity();

  for(unsigned int i=0; i < _numBezierSections; ++i){

    yLeft = y - _mYVec[i][0];
    yRight=     _mYVec[i][5] - y;

    xLeft = xGuess - _mXVec[i][0];
    xRight=        _mXVec[i][5] - xGuess;
    xDist = fabs(xLeft)+fabs(xRight);

    //If the y value is in the spline interval and the
    //x interval is closer to the guess, update the interval
    if(yLeft*yRight >= 0 && xDist < xDistBest){
      idx = i;
      xDistBest = xDist;
    }

  }

  //y value is in the linear region
  if(idx == -1){
    if( (y-_y1)*_dydx1 >= 0
        && fabs(_dydx1) > numeric_limits< double >::epsilon() ) {
      xVal = (y-_y1)/_dydx1 + _x1;
    }else if( (_y0-y)*_dydx0 >= 0
       && fabs(_dydx0) > numeric_limits< double >::epsilon() ){
      xVal = (y-_y0)/_dydx0 + _x0;
    }else{
      xVal = numeric_limits<double>::signaling_NaN();
    }

  }else{
    //y is in an interval
    double u = SegmentedQuinticBezierToolkit::
         calcU(y,_mYVec[idx], UTOL, MAXITER);
    xVal = SegmentedQuinticBezierToolkit::
         calcQuinticBezierCurveVal(u,_mXVec[idx]);
  }

  return xVal;

}

double SmoothSegmentedFunction::calcValue(
    const RigidBodyDynamics::Math::VectorNd& ax) const
{
  
  if( !(ax.size() == 1) ){
    cerr<<"SmoothSegmentedFunction::calcValue " << _name.c_str() 
        <<": Argument x must have only 1 element, as this function is "
        << "designed only for 1D functions, but a function with "
        << ax.size() << " elements was entered"
        << endl;
    assert(0);
    abort();
    
  }

  return calcValue(ax[0]); 
}

/*Detailed Computational Costs
________________________________________________________________________
If x is in the Bezier Curve, and dy/dx is being evaluated
            Name   Comp.   Div.  Mult.   Add.  Assign.
_______________________________________________________________________
Overhead:
  SegmentedQuinticBezierToolkit::
          calcIndex   3*m+2           1*m   3   
            *calcU   15    2    82    42   60  
  Derivative Evaluation:
  **calcQuinticBezierCurveDYDX          21    20    13
            dy/du           20    19    11
            dx/du           20    19    11
            dy/dx       1

            total  17+3*m   3     143   m+100   98

*Approximate. Uses iteration
**Higher order derivatives cost more
________________________________________________________________________
If x is in the linear region

            Name   Comp.   Div.  Mult.   Add.  Assign.
                  1              1
________________________________________________________________________
  */

double SmoothSegmentedFunction::calcDerivative(double x, int order) const
{
  //return calcDerivative( SimTK::Array_<int>(order,0),
    //           RigidBodyDynamics::Math::VectorNd(1,x));
  double yVal = 0;

  //QUINTIC SPLINE

  
  if(order==0){
        yVal = calcValue(x);
  }else{
      if(x >= _x0 && x <= _x1){    
        int idx  = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec);
        double u = SegmentedQuinticBezierToolkit::
                calcU(x,_mXVec[idx],UTOL,MAXITER);
        yVal = SegmentedQuinticBezierToolkit::
              calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx], 
              _mYVec[idx], order);
/*
              std::cout << _mX(3, idx) << std::endl;
              std::cout << _mX(idx) << std::endl;*/
      }else{
          if(order == 1){
            if(x < _x0){
              yVal = _dydx0;
            }else{
              yVal = _dydx1;}
          }else{
            yVal = 0;}   
        }
    }

  return yVal;
}



double SmoothSegmentedFunction::
  calcDerivative( const std::vector<int>& derivComponents,
          const RigidBodyDynamics::Math::VectorNd& ax) const
{
  /*
  for(int i=0; i < (signed)derivComponents.size(); i++){
    SimTK_ERRCHK2_ALWAYS( derivComponents[i] == 0,
    "SmoothSegmentedFunction::calcDerivative",
    "%s: derivComponents can only be populated with 0's because "
    "SmoothSegmentedFunction is only valid for a 1D function, but "
    "derivComponents had a value of %i in it",
    _name.c_str(), derivComponents[i]);
  }
  SimTK_ERRCHK2_ALWAYS( derivComponents.size() <= 6,
    "SmoothSegmentedFunction::calcDerivative",
    "%s: calcDerivative is only valid up to a 6th order derivative"
    " but derivComponents had a size of %i",
    _name.c_str(), derivComponents.size());

  SimTK_ERRCHK2_ALWAYS( ax.size() == 1,
    "SmoothSegmentedFunction::calcValue",
    "%s: Argument x must have only 1 element, as this function is "
    "designed only for 1D functions, but ax had a size of %i",
    _name.c_str(), ax.size());
  */

  for(int i=0; i < (signed)derivComponents.size(); i++){
    if( !(derivComponents[i] == 0)){
      cerr << "SmoothSegmentedFunction::calcDerivative " 
        << _name.c_str()
        <<": derivComponents can only be populated with 0's because "
        << "SmoothSegmentedFunction is only valid for a 1D function,"
        << " but derivComponents had a value of " 
        << derivComponents[i] << " in it"
        << endl;
      assert(0);
      abort();

    }
  }

  if( !(derivComponents.size() <= 6)){
    cerr  << "SmoothSegmentedFunction::calcDerivative " << _name.c_str()
      << ": calcDerivative is only valid up to a 6th order derivative"
      << " but derivComponents had a size of "
      << derivComponents.size()
      << endl;
    assert(0);
    abort();
  }

  if( !(ax.size() == 1) ){
    cerr  << "SmoothSegmentedFunction::calcValue " << _name.c_str()
          << ": Argument x must have only 1 element, as this function is "
          << "designed only for 1D functions, but ax had a size of "
          << ax.size()
          << endl;
    assert(0);
    abort();
  }


  return calcDerivative(ax[0], derivComponents.size());
}

/*Detailed Computational Costs
________________________________________________________________________
If x is in the Bezier Curve, and dy/dx is being evaluated
            Name   Comp.   Div.  Mult.   Add.  Assign.
_______________________________________________________________________
      *spline.calcValue   7         2     3     1

*Approximate cost of evaluating a cubic spline with 100 knots, where
the bisection method is used to find the correct index
________________________________________________________________________
If x is in the linear region

        Name        Comp.   Div.  Mult.   Add.  Assign.
        *spline.calcValue  1         2     3     1    
        integral eval    2         4     5     1
        total        3         6     8     2

*Approximate cost of evaluating a cubic spline at its last knot point
________________________________________________________________________

*/
/*
double SmoothSegmentedFunction::calcIntegral(double x) const
{
  SimTK_ERRCHK1_ALWAYS(_computeIntegral,
    "SmoothSegmentedFunction::calcIntegral",
    "%s: This curve was not constructed with its integral because"
    "computeIntegral was false",_name.c_str());

  double yVal = 0;  
  if(x >= _x0 && x <= _x1){
    yVal = _splineYintX.calcValue(RigidBodyDynamics::Math::VectorNd(1,x));
  }else{
    //LINEAR EXTRAPOLATION     
    if(x < _x0){
      RigidBodyDynamics::Math::VectorNd tmp(1);
      tmp(0) = _x0;
      double ic = _splineYintX.calcValue(tmp);
      if(_intx0x1){//Integrating left to right
        yVal = _y0*(x-_x0) 
          + _dydx0*(x-_x0)*(x-_x0)*0.5 
          + ic;
      }else{//Integrating right to left
        yVal = -_y0*(x-_x0) 
          - _dydx0*(x-_x0)*(x-_x0)*0.5 
          + ic;
      }      
    }else{
      RigidBodyDynamics::Math::VectorNd tmp(1);
      tmp(0) = _x1;
      double ic = _splineYintX.calcValue(tmp);
      if(_intx0x1){
        yVal = _y1*(x-_x1) 
          + _dydx1*(x-_x1)*(x-_x1)*0.5 
          + ic;
      }else{
        yVal = -_y1*(x-_x1) 
          - _dydx1*(x-_x1)*(x-_x1)*0.5 
          + ic;
      }
    }
  } 
  
  return yVal;
}
*/
/*
bool SmoothSegmentedFunction::isIntegralAvailable() const
{
  return _computeIntegral;
}
*/

/*
bool SmoothSegmentedFunction::isIntegralComputedLeftToRight() const
{
  return _intx0x1;
}
*/

int SmoothSegmentedFunction::getArgumentSize() const
{
  return 1;
}

int SmoothSegmentedFunction::getMaxDerivativeOrder() const
{
  return 6;
}

std::string SmoothSegmentedFunction::getName() const
{
  return _name;
}

void SmoothSegmentedFunction::setName(const std::string &name) 
{
  _name = name;
}

RigidBodyDynamics::Math::VectorNd 
  SmoothSegmentedFunction::getCurveDomain() const
{
  RigidBodyDynamics::Math::VectorNd domain(2);
  
  domain[0] = 0;
  domain[1] = 0;
  if (!_mXVec.empty()) {
    domain[0] = _mXVec[0][0];
    domain[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1];
  }
  return domain;
}

///////////////////////////////////////////////////////////////////////////////
// Utility functions
///////////////////////////////////////////////////////////////////////////////

/*Detailed Computational Costs

_______________________________________________________________________
            Name   Comp.   Div.  Mult.   Add.  Assign.
_______________________________________________________________________

        *overhead  (17+3*m   2  82    42+m  63)*7
                119+21*m  14  574   294+7m  441

        calcValue           21    20    13
  calcDerivative: dy/dx        1  40    38    22      
          : d2y/dx2        2  78    73    23
          : d3y/dx3        4  118   105   58
          : d4y/dx4        5  168   137   71
          : d5y/dx5        7  236   170   88
          : d6y/dx6        9  334   209   106

        **calcIntegral  7         2     3     1       

      total per point   126+21*m  42   1571  1049  823
      total per elbow   126k+21k*m  42k  1571k   1049k   823k
           
  *Approximate. Overhead associated with finding the correct Bezier
          spline section, and evaluating u(x). 
          Assumes 2 Newton iterations in calcU

  **Approximate. Includes estimated cost of evaluating a cubic spline
          with 100 knots
*/
RigidBodyDynamics::Math::MatrixNd 
  SmoothSegmentedFunction::calcSampledCurve(int maxOrder,
                          double domainMin,
                          double domainMax) const{

  int pts = 1; //Number of points between each of the spline points used
          //to fit u(x), and also the integral spline
  if( !(maxOrder <= getMaxDerivativeOrder()) ){
    cerr  << "SmoothSegmentedFunction::calcSampledCurve "
          << "Derivative order past the maximum computed order requested"
          << endl;
    assert(0);
    abort();
  }

  double x0,x1,delta;
  //y,dy,d1y,d2y,d3y,d4y,d5y,d6y,iy
   RigidBodyDynamics::Math::VectorNd 
    midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1));
   RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS);

   //Generate a sample of X values inside of the curve that is denser where 
   //the curve is more curvy.
   double u;
   int idx = 0;
    for(int s=0; s < _numBezierSections; s++){
    //Sample the local set for u and x
    for(int i=0;i<NUM_SAMPLE_PTS;i++){
        u = ( (double)i )/( (double)(NUM_SAMPLE_PTS-1) );
        x[i] = SegmentedQuinticBezierToolkit::
          calcQuinticBezierCurveVal(u,_mXVec[s]);  
        if(_numBezierSections > 1){
           //Skip the last point of a set that has another set of points
           //after it. Why? The last point and the starting point of the
           //next set are identical in value.
          if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){
            midX[idx] = x[i];
            idx++;
          }
        }else{
          midX[idx] = x[i];        
          idx++;
        }
      }
    }     

    
  RigidBodyDynamics::Math::VectorNd xsmpl(pts*(midX.size()-1)+2*10*pts);
  
  RigidBodyDynamics::Math::MatrixNd results;

  /*
  if(_computeIntegral){
    results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2+1);
  }else{

  }
  */
  results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2);

  //Array initialization is so ugly ...
  std::vector<int> d1y(1),d2y(2),d3y(3),d4y(4),d5y(5),d6y(6);
  d1y[0]=0;
  d2y[0] = 0;
  d2y[1] = 0;
  for(int i=0;i<3;i++)
    d3y[i]=0;
  for(int i=0;i<4;i++)
    d4y[i]=0;
  for(int i=0;i<5;i++)
    d5y[i]=0;
  for(int i=0;i<6;i++)
    d6y[i]=0;

  //generate some sample points in the extrapolated region
  idx = 0;
  x0  = _x0 - 0.1*(_x1-_x0);
  if(domainMin < x0)
    x0 = domainMin;

  x1  = _x0;
  delta = (0.1)*(x1-x0)/(pts);

  for(int j=0; j<pts*10; j++){
    xsmpl[idx] = x0 + delta*j;
    idx++;
  }


  //generate some points in the mid region
  for(int i=0; i< midX.size()-1;i++){  
    x0 = midX[i];
    x1 = midX[i+1];
    delta = (x1-x0)/pts;
    for(int j=0;j<pts;j++){
      xsmpl[idx] = x0 + delta*j;
      idx++;   
    }    
  }

  //generate some points in the extrapolated region
  x0 = _x1;
  x1 = _x1 + 0.1*(_x1-_x0);
  if(domainMax > x1)
    x1 = domainMax;

  delta = (1.0/9.0)*(x1-x0)/(pts);

  for(int j=0;j<pts*10;j++){
    xsmpl[idx] = x0 + delta*j;
    idx++;
  }

  //Populate the results matrix at the sample points
  RigidBodyDynamics::Math::VectorNd ax(1);
  for(int i=0; i < xsmpl.size(); i++){
    ax[0] = xsmpl[i];
    results(i,0) = ax[0];
    if(i==48){
       double here = 1.0;
    }
    results(i,1) = calcValue(ax);
    if(maxOrder>=1)
    results(i,2) = calcDerivative(d1y,ax);

    if(maxOrder>=2)
    results(i,3) = calcDerivative(d2y,ax);
    
    if(maxOrder>=3)
    results(i,4) = calcDerivative(d3y,ax);
    
    if(maxOrder>=4)
    results(i,5) = calcDerivative(d4y,ax);
    
    if(maxOrder>=5)
    results(i,6) = calcDerivative(d5y,ax);
    
    if(maxOrder>=6)
    results(i,7) = calcDerivative(d6y,ax);

    /*
    if(_computeIntegral){
      results(i,maxOrder+2) = calcIntegral(ax(0));
    }
    */
  }
   return results;
}

void SmoothSegmentedFunction::getXControlPoints(
    RigidBodyDynamics::Math::MatrixNd& mat) const
{
  mat.resize(_mXVec.size(), _mXVec.at(0).size());

  for(int i=0; i<_mXVec.size();++i){
    for(int j=0; j<_mXVec.size();++j){
      mat(i,j) = _mXVec.at(i)[j];
    }
  }

}
void SmoothSegmentedFunction::getYControlPoints(
    RigidBodyDynamics::Math::MatrixNd& mat) const
{
mat.resize(_mYVec.size(), _mYVec.at(0).size());

for(int i=0; i<_mYVec.size();++i){
  for(int j=0; j<_mYVec.size();++j){
    mat(i,j) = _mYVec.at(i)[j];
  }
}

}


/*Detailed Computational Costs

_______________________________________________________________________
            Name   Comp.   Div.  Mult.   Add.  Assign.
_______________________________________________________________________

         *overhead   (17+3*m   2  82    42+m  63)*3
                51+9m    6  246   126+3m   189

        calcValue             21    20    13
  calcDerivative  : dy/dx        1  40    38    22      
          : d2y/dx2        2  78    73    23

        **calcIntegral  7        2     3     1       

      total per point    58+9m   9  387  260+3m   248
      total per elbow  5.8k+900m   900  38.7k  26k+300m 24.8k
           
  *Approximate. Overhead associated with finding the correct Bezier
          spline section, and evaluating u(x). 
          Assumes 2 Newton iterations in calcU

  **Approximate. Includes estimated cost of evaluating a cubic spline
          with 100 knots
*/
void SmoothSegmentedFunction::printCurveToCSVFile(
                const std::string& path,
                const std::string& fileNameWithoutExtension,
                double domainMin,
                double domainMax) const
{
  //Only compute up to the 2nd derivative
  RigidBodyDynamics::Math::MatrixNd results = 
    calcSampledCurve(2,domainMin,domainMax);
  std::vector<std::string> colNames(results.cols());
  colNames[0] = "x";
  colNames[1] = "y";
  colNames[2] = "dy/dx";
  colNames[3] = "d2y/dx2";
  /*
  if(results.cols() == 5){
    colNames[4] = "int_y(x)";
  }
  */
  std::string fname = fileNameWithoutExtension;
  fname.append(".csv");
  printMatrixToFile(results,colNames,path,fname);
}
/*
This function will print cvs file of the column vector col0 and the matrix data
*/
void SmoothSegmentedFunction::
  printMatrixToFile(  RigidBodyDynamics::Math::MatrixNd& data, 
            std::vector<std::string>& colNames,
            const std::string& path, 
            const std::string& filename) const
{
  
  ofstream datafile;
  std::string fullpath = path;
  
  if(fullpath.length() > 0)
    fullpath.append("/");
  
  fullpath.append(filename);

  datafile.open(fullpath.c_str(),std::ios::out);

  if(!datafile){
    datafile.close();
    cerr << "SmoothSegmentedFunction::printMatrixToFile "
         << _name.c_str() << ": Failed to open the file path: "
         << fullpath.c_str()
         << endl;
    assert(0);
    abort();    
  }


  for(int i = 0; i < (signed)colNames.size(); i++){
    if(i < (signed)colNames.size()-1)
      datafile << colNames[i] << ",";
    else
      datafile << colNames[i] << "\n";
  }

  for(int i = 0; i < data.rows(); i++){     
    for(int j = 0; j < data.cols(); j++){
      if(j<data.cols()-1)
        datafile << data(i,j) << ",";
      else
        datafile << data(i,j) << "\n";
    }   
  }
  datafile.close();
}