/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/basda-mocca/src/device/mpia/mocon/Profile.cc 15131 2018-02-16 20:48:59Z mathar $
*/

#include <mocon/Profile.h>
#include <mocon/Exception.h>
#include <Nice/Alg/Alg.h>
#include <Nice/Time/Date.h>
#include <iostream>
#include <boost/math/special_functions/round.hpp>

/** Defining the following macro lets the software merge consecutive segments to avoid zero velocities
* This tries to patch the fact that otherwise the MOCON may (?) create an event that
* this basda-mocca client software regards as the completion of a trajectory.
* @since 2018-02-07
#define TAN_BASDA_MOCCA_MERGE_ZEROVEL_SEGS
*/

#if 0
/************************
* @brief Constructor
*
* @param _trajectory position values in microsteps
* @param _samplesPerSegment The number of points on the trajectory
*                           that should be represented by a single polynomial.
* @param _maxError Maximum error in steps (counts).
*/
Basda::MoConModuleProfile::MoConModuleProfile(std::vector<Nice::PointVar> &_trajectory,
                                              unsigned int _samplesPerSegment,
                                              unsigned int _faultTolerance)
{
    setExternalProfilePositionSequence(_trajectory);
    m_samplesPerSegment = _samplesPerSegment;
    m_faultTolerance = _faultTolerance;

}
#endif /* 0 */

/************************
* @brief Constructor
*/
Basda::MoConModuleProfile::MoConModuleProfile()
{
//    m_trajectory.reserve(0);
    m_maxSamplesPerSegment = 5;
    m_minSamplesPerSegment = 5;
    m_faultTolerance = 0;
    m_deviation = 0;
    m_maxDeviation = 0;
}

/************************
* @brief Destructor
*/
Basda::MoConModuleProfile::~MoConModuleProfile()
{
}

#if 0
void
Basda::MoConModuleProfile::setExternalProfileMicroSteps(unsigned int _microSteps)
{
     m_microSteps = _microSteps;
}

/************************
* @brief Set the external profile (in microsteps and cycles).
*
* @param _trajectory x is the time in cycles; y is the position in
*                    counts or steps
*/
void Basda::MoConModuleProfile::setExternalProfilePositionSequence(std::vector<
        Nice::PointVar> &_trajectory)
{
    m_trajectory.clear();
    m_fitTrajectory.clear();
    m_trajectoryNodes.clear();
    m_fitTrajectoryNodes.clear();

    m_trajectory = _trajectory;
    m_fitTrajectory.reserve(m_trajectory.size()); // allocate the memory
    m_fitTrajectory.resize(m_trajectory.size());  // initialize the vector
}
#endif


/************************
* @brief Set the maximal number of samples per segment.
*
* @param _maxSamplesPerSegment
*/
void Basda::MoConModuleProfile::setExternalProfileMaxSamplesPerSegment(unsigned int _maxSamplesPerSegment)
{
    m_maxSamplesPerSegment = _maxSamplesPerSegment;
}

/************************
* @brief Set the minimal number of samples per segment.
*
* @param _minSamplesPerSegment Minimum acceptable number of samples
*                              for a profile fit
*/
void Basda::MoConModuleProfile::setExternalProfileMinSamplesPerSegment(unsigned int _minSamplesPerSegment)
{
    m_minSamplesPerSegment = _minSamplesPerSegment;
}

/************************
* @brief Set the external profile fault tolerance.
*
* The fitted trajectory can have a maximal deviation of the specified
* fault tolerance.
*
* @param _samplesPerSegment
*/
void Basda::MoConModuleProfile::setExternalProfileFaultTolerance(unsigned int _faultTolerance)
{
    if (_faultTolerance == 0)
    {
        std::string errorMsg =
                "The external profile fault tolerance is invalid. The value have to be greater than zero!";
        E_THROW(::Basda::MoCon::InvalidFaultToleranceException, errorMsg);
    }
    m_faultTolerance = _faultTolerance;
}

/*******************************************
* @brief Fits the MoCon profile to the trajectory.
*
* using lfit of numerical recipies in c++, general linear least
* squares, section 15.4
*
* @param  _startDate the start date of the trajectory in a civil date format
*               This value is just needed to store the start date of
*               each segment for debugging purposes.
* @param _startCycle the start MoCon cycle. This value is just needed
*                    to store the start cycle of each segment for
*                    debugging purposes.
* @param[in] _trajectory the trajectory where the x value is the cycle time
*                    and the y value is the MoCon position in steps.
*     The task of this procedure is to re-fit this representation (as a 
*     discrete set of values) to a sequence of low-order polynomials.
* @param[in] _cycleTime the time for one MoCon cycle
*
* @return the number of generated segments
*/
unsigned int Basda::MoConModuleProfile::fitExternalProfile(Nice::Date _startDate,
                                                   Nice::I64 _startCycle,
                                                   std::vector<Nice::PointVar> &_trajectory,
                                                   double _cycleTime)
{
    m_trajectory.clear();
    m_trajectory = _trajectory;


    /* x values (time axis) from a fit
    */
    boost::numeric::ublas::vector<double> cycles;

    /* y values (positions) from a fit
    */
    boost::numeric::ublas::vector<double> position;

    /* weight of the individual points of a fit
    */
    boost::numeric::ublas::vector<double> sigma;

    /* coefficients for the polynomial. De-facto integer, because that's the Mocon interface representation
    */
    boost::numeric::ublas::vector<double> coefficients(Basda::Profile::PolynomOrder);

    /* flags telling which of the coefficients[] should be fitted.
    */
    boost::numeric::ublas::vector<bool> fitThisCoeff(Basda::Profile::PolynomOrder);

    /* Covariance matrix, not actually used but computed.
    * If our Algorithm were too slow: switch from gauss-jordan solving to a faster method
    * (e.g. LLL) See numerical recipes.
    * However, doesn't seem to be an issue!
    */
    boost::numeric::ublas::matrix<double> covariance(Basda::Profile::PolynomOrder,
                                                     Basda::Profile::PolynomOrder);

    /* initial sgement length (may be adjusted later on)
    */
    unsigned int segmentLength = m_maxSamplesPerSegment;
    m_profile.clear();

    /* initialize mimic a motor at rest, at local time zero.
    */
    unsigned int segmentStart = 0;
    double lastDeviation = 0;
    double lastPosition = 0;
    double lastVelocity = 0;
    double lastAcceleration = 0;
    double lastJerk = 0;
    int lastNumberOfCycles = 0;

    Segment segment;                      //the coefficients of a single segment

    /*Procedure:
    *1.  As long as the end is not reached, take the next chunk of samples at
    *    the current position.
    *2.  Fit them.
    *2a. Does the fit result in coefficients that are close to zero:
    *    Fit again - without considering these parameters.
    *2b. If the fit deviates more than tolerable: Fit again, but with a reduced
    *    number of samples, which means: more but shorter segments.
    *3.  Fit acceptable: store the coefficients, increase the index for the
    *    current position and
    *    start over with 1.
    */

    /* Example of the LN trajectories: say 10800 polynomials requested from
    * the TCS, and these were oversampled by a factor of 100 so we have
    * now 1080000 discrete samples along the time axis. Then the next loop
    * will run through segmentStart =0, 100, 200,... with segmentLength=100 and
    * m_trajectory.size() = 1080000, all unitless integers. The fact that
    * these samples were obtained from sampling some piece-wise polynomial is
    * no longer known here; in fact, that fitting here would (in principle, although
    * using discrete motor-integers) never generate jerks (3rd order coefficients).
    */

    /* Until we have reached (almost) the end...
    * Assumes that the full length of the trajectory is a multiple of the segment length.
    * If not, some residual piece of the trajectory would not be covered in
    * the loop but by the handler after the loop.
    */
    while ( segmentStart + segmentLength < m_trajectory.size())
    {
        //...m_segmentLength might have been reduced. So adapt the vector sizes accordingly...
        cycles.resize(segmentLength);
        position.resize(segmentLength);
        sigma.resize(segmentLength);

        //...read in all data of the next segment...
        for (unsigned int i = 0; i < segmentLength; i++)
        {
            cycles[i] = (m_trajectory.at(segmentStart + i)).xp
                    - (m_trajectory.at(segmentStart)).xp;
            position[i] = (m_trajectory.at(segmentStart + i)).yp;
            //...assign a homogeneous weight to all events that are considered in this segment...
            sigma[i] = 1.0;
        }

        /*...coefficients will be fitted to full polynomial order
        */
        for (unsigned int i = 0; i < Basda::Profile::PolynomOrder; i++)
            fitThisCoeff[i] = true;

        /* flag is true if fit is done again with reduced number of parameters
        * (i.e., not using acceleration and/or jerk as fitting variables)
        */
        bool fitAgain;

        /* ...and go for the fit:...
        * this at least once. Do it again, if coefficients are close to zero
        */
        do
        {
            fitAgain = false;

            //first initialize the coefficients that are getting fitted
            for (unsigned int i = 0; i < Basda::Profile::PolynomOrder; i++)
                if (fitThisCoeff[i]) 
                   coefficients[i] = 1.0;

            /* At the nodes the fitted trajectory has to coincide with the specified
            * trajectory. The coincidence is done by assigning the position value
            * at the nodes a higher weight (smaller sigma) for the fit. So at the 
            * end points of the segments it's more like an interpolation than a fit....
            */
            sigma[0] = 0.01;
            sigma[segmentLength - 1] = 0.01;


	    /* chi squared of the fit
	    */
	    double chisq;

            /* ...finally do the polynomial fit that returns coefficients[].
            * Here we use a algorithm that finds a solution based on normal equations
            * if this turns out to be unstable, we might want to consider to switch to
            * an SVD algorithm. However, doesn't seem to be an issue!
            */
            ::Nice::Alg::linearFit(cycles, position, sigma, coefficients,
                                   fitThisCoeff, covariance, chisq, basePol3);

            /*...Is the fitted acceleration large enough to be rounded to the next integer?...
            * the CoefficientTreshold is currently set to 100 in src/interface/Basda/Mocca/Constant.h
            */
            if ((fabs(coefficients[2]) < Basda::Profile::CoefficientTreshold)
                    && fitThisCoeff[2])
            {
                coefficients[2] = 0.0;
                fitThisCoeff[2] = false;
                fitAgain = true;
            }

            //...Is the fitted jerk large enough to be rounded to the next integer?...
            if ((fabs(coefficients[3]) < Basda::Profile::CoefficientTreshold)
                    && fitThisCoeff[3])
            {
                coefficients[3] = 0.0;
                fitThisCoeff[3] = false;
                fitAgain = true;
            }
            //...No?, ok, then ignore acceleration or jerk which are not and do the fit again.
        } while (fitAgain);

        // See if the fit is getting close enough to the data...
        //...first convert the determined coefficients into integers and store them...
        // or (m_trajectory.at(segmentStart+segmentLength)).xp - (m_trajectory.at(segmentStart)).xp;
        segment.numberOfCycles = (int)m_trajectory.at(segmentLength).xp;

#if 0
        segment.position       = boost::math::lround(coefficients[0]);
        segment.velocity       = boost::math::lround(coefficients[1]);
        segment.acceleration   = boost::math::lround(coefficients[2]);
        fitThisCoeff[2] ? boost::math::lround(coefficients[2]) : 0;
#endif

        /* We now have fitted the ideal trajectory in the segment with coefficients[0..3].
        * The previous segment ended at a value lastDeviation away from the continuous
        * curve of the ideal trajectory. Negative lastDeviation means we're now starting
        * the next trajectory with a slightly too small position, positive with a slightly
        * too large position. Because we want avoid sudden jumps in the glued 
        * segments, that is, we want them to be C^0 continuous or steady, the new
        * segment should start at coefficients[0]+lastDeviation, not on coefficients[0]
        * that was proposed by the free fit. So along the next segment we want to swallow most
        * of this offset by changing the coefficients[1..3] such that on the mean we end
        * up in a position relative -lastDeviation away than proposed by the fit, such that
        * at the end of this trajectory we are again exactly on the ideal trajectory.
        * So we distribute this desired offset over velocity and acceleration with a little
        * bit of overshooting, such that the velocity contributes -2*lastDeviation and
        * the accelration +1*lastDeviation to the change.
        */
        segment.position = boost::math::iround(coefficients[0] + lastDeviation);
#if 0
	/* version until Com-3 */
        segment.velocity = boost::math::iround(
                coefficients[1]
                        - 2 * lastDeviation / (double) (segment.numberOfCycles)
                                * (double) Basda::Profile::FixPointEncoding_16_16);
        segment.acceleration =
                (fitThisCoeff[2] ?
                        boost::math::iround(
                                coefficients[2]
                                        + lastDeviation
                                                / (double) (segment
                                                        .numberOfCycles
                                                        * segment.numberOfCycles)
                                                * (double) Basda::Profile::FixPointEncoding_16_16) :
                        boost::math::iround(
                                lastDeviation
                                        / (double) (segment.numberOfCycles
                                                * segment.numberOfCycles)* (double)Basda::Profile::FixPointEncoding_16_16));
#else
        /* first adjust the coefficients[2] to make up for the opposite of the
        * desired offset. The total correction in the next segment over time t is
        * delta(coefficients[2])*(t-t0)^2+delta(coefficients[1])*(t-t0)=-lastDeviation
        * and we chose delta(coefficiens[2])= 1*lastdeviation and delta(coefficients[1]) = -2*lastDeviation.
        * as feedback. First calculate the desired contribution of the coefficients[2] up
        * to the integer arithmetics of the MoCon. If that turns out to be zero, because
        * the trajectory is almost flat, we only insert delta(coefficients[1])= -lastDeviation
        * to avoid overshooting and oscillation of the velocity around its mean term. RJM 2018-02-16
        */
        segment.acceleration =
                (fitThisCoeff[2] ?
                        boost::math::iround(
                                coefficients[2]
                                        + lastDeviation
                                                / (double) (segment
                                                        .numberOfCycles
                                                        * segment.numberOfCycles)
                                                * (double) Basda::Profile::FixPointEncoding_16_16) :
                        boost::math::iround(
                                lastDeviation
                                        / (double) (segment.numberOfCycles
                                                * segment.numberOfCycles)* (double)Basda::Profile::FixPointEncoding_16_16));
        const short int velFactorDeviation = segment.acceleration ? -2 : -1 ;
        segment.velocity = boost::math::iround(
                coefficients[1] + velFactorDeviation * lastDeviation / (double) (segment.numberOfCycles)
                                * (double) Basda::Profile::FixPointEncoding_16_16);
#endif

        //fitThisCoeff[3] ? boost::math::lround(coefficients[3]) : 0;
        segment.jerk = boost::math::iround(coefficients[3]);

        if (m_profile.empty())
        {
            segment.startCycle = _startCycle;
            segment.startDate = _startDate;
        }
        else
        {
            segment.startCycle = m_profile.at(m_profile.size() - 1).startCycle
                    + m_profile.at(m_profile.size() - 1).numberOfCycles;
            segment.startDate = _startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime * (double)(segment.startCycle - _startCycle)));
        }

        //...now, calculate for each point in the trajectory the corresponding position value from the fit
        //    and find out how much the fit deviates from the requested trajectory...
        calcMaxDeviation(segmentStart, segment, lastPosition, lastVelocity, lastAcceleration, lastJerk, lastNumberOfCycles);

        //...if a maximum acceptable error has been specified and cannot be met
        //   reduce the segment length and fit again...
        // the maxError has to be bigger than the minError on order to respect the max deviation
        if ( m_faultTolerance > 0 && ((unsigned int) fabs(m_maxDeviation) >= m_faultTolerance) )
        {
            segmentLength /= 2;
            if (segmentLength < m_minSamplesPerSegment)
            {
                std::stringstream error;
                error << "The segment length (" << segmentLength
                        << ") will be smaller than the allowed minimum segment length ("
                        << m_minSamplesPerSegment
                        << ") to decrease the maximal error deviation. ("
                        << m_maxDeviation << "). Velocity is " << segment.velocity ;
//                E_LOG(error.str());
                E_THROW(::Basda::MoCon::SegmentLengthUnderrunException, error.str());
            }
        }
#if 0
        // if the velocity is zero (MoCon will send a completion event whenever
        // the velocity is zero, but the trajectory should continue ..
        else if ( segment.velocity == 0)
        {
            /* hack RJM 2018-01-26 if the velocity is temporarily zero (LN parallactic
            * angle for example), let the motor move at least to there instead of throwing an error...
            * Todo: can't we just take a parabolic orbit and let the motor controller run
            * through the sign reversal of the velocity??
            */
            if (segmentLength < m_minSamplesPerSegment)
            {
                std::stringstream error;
                error << "Velocity is temporarily " << segment.velocity << ". Motor may stop prematurely" ;
		E_LOG(error.str());
                // E_THROW(::Basda::MoCon::SegmentLengthUnderrunException, error.str()); // code COM-3, refusing traj
            }
        }
#endif
#ifdef TAN_BASDA_MOCCA_MERGE_ZEROVEL_SEGS
        else if ( segment.velocity == 0)
        {
            segmentLength *= 2;
            /* try to merge this segment with the next one to bypass a transient Motion-Complete event in 
            * device/mpia/mocon/NDevice.cc. RJM 2018-02-07
            */
            std::stringstream error;
            const Nice::Date segStrt = _startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime * (double)(segmentStart)));
            error << "Scheduled velocity is temporarily " << segment.velocity << " at " << segStrt
                        << ". Segment " << segmentStart << " of " << m_trajectory.size()
                        << ". Length " << segmentLength
                        << ". Accel " << segment.acceleration
                        << ". Jerk " << segment.jerk
			<<". Motor may stop prematurely there?" ;
            W_LOG(error.str());
        }
#endif
        else
        {
            if ( segment.velocity == 0)
            {
                std::stringstream error;
                const Nice::Date segStrt = _startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime * (double)(segmentStart)));
                error << "Scheduled velocity is temporarily " << segment.velocity << " at " << segStrt
                        << ". Segment " << segmentStart << " of " << m_trajectory.size()
                        << ". Accel " << segment.acceleration
                        << ". Jerk " << segment.jerk
			<<". Motor may stop prematurely there." ;
		E_LOG(error.str());
            }

            /* remember how far this segment will deviate at its end from the ideal trajectory
            */
            lastDeviation = m_deviation;

            /* with this log one can use
            *    cd $INSROOT/var/log
            *    fgrep profDump laos.sx.hws.drot.derotation-svr.log | awk '{printf("%d %d %d %d %.1f\n",$15,$16,$17,$18,$19)}' > profDump.log
            *    gnuplot
            *    >> plot "profDump.log" using 1:2, "profDump.log" using 1:3, "profDump.log" using 1:4, "profDump.log" using 1:5
            I_LOG("profDump " << m_profile.size() << " " << segment.position << " " << segment.velocity << " " << segment.acceleration << " " << m_trajectory[segmentStart].yp << " ") ;
            */
            // Now store the fitting result of the segment...
            m_profile.push_back(segment);

            /*
            * store the data to check the algorithm with gnuplot
	    * m_trajectoryNodes.push_back(_trajectory.at(segmentStart));
            * m_fitTrajectoryNodes.push_back(m_fitTrajectory.at(segmentStart));
            */

            segmentStart += segmentLength;
            // the lastPosition is used for the terminating segment node
            if (( segmentStart + m_maxSamplesPerSegment >= m_trajectory.size())
                    && ( m_trajectory.size() - segmentStart > m_minSamplesPerSegment))
                segmentLength = (unsigned int)m_trajectory.size() - segmentStart - 1;
            else
                /* if we had temporarily shrunk the segment length above to decrease the
                * deviation between fit and samples, increase it to the original size
                * such that we don't keep the small segment length for the rest of the data....
                */
                segmentLength = m_maxSamplesPerSegment;
        }
    } /* end of loop that checks segmentstart plus length less than sample size */

    segment.numberOfCycles = lastNumberOfCycles;
    segment.position = boost::math::iround(lastPosition);
    segment.velocity = boost::math::iround(
            coefficients[1]
                    - 2 * lastDeviation / (double) (segment.numberOfCycles)
                            * (double) Basda::Profile::FixPointEncoding_16_16);
    segment.acceleration = (fitThisCoeff[2] ?
            boost::math::iround(
                    coefficients[2]
                            + lastDeviation
                                    / (double) (segment
                                            .numberOfCycles
                                            * segment.numberOfCycles)
                                    * (double) Basda::Profile::FixPointEncoding_16_16) :
            boost::math::iround(
                    lastDeviation
                            / (double) (segment.numberOfCycles
                                    * segment.numberOfCycles)* (double)Basda::Profile::FixPointEncoding_16_16));
    segment.jerk = boost::math::iround(coefficients[3]);
    m_profile.push_back(segment);

    return (unsigned int)m_profile.size();
} /* fitExternalProfile */



Basda::MoConModuleProfile::Segment Basda::MoConModuleProfile::shinsGetStartingRampSegment(const std::vector<Nice::PointVar> & _positionSequence, unsigned int durationInCycles)
{
    U9_LOG("_positionSequence.size() : " << _positionSequence.size() << "; durationInCycles : " << durationInCycles);
    Segment return_seg;
    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;
    double start_velocity = (_positionSequence.at(1).yp - _positionSequence.at(0).yp)/(_positionSequence.at(1).xp - _positionSequence.at(0).xp); // steps/cycle
    return_seg.position = _positionSequence.at(0).yp;

    U9_LOG(_positionSequence.at(1).yp << " ~ " << _positionSequence.at(0).yp << " ~ " << _positionSequence.at(1).xp << " ~ " << _positionSequence.at(0).xp);
    double endingPosition, endingVelocity, endingAcceleration;
    int encAcc = 1, acc_cycles = 100;
    int encVel = (int)(encode16*start_velocity);
    U9_LOG("Ecoded Velocity = " << encVel);
    if(encVel != 0)
    {
        int sign = encVel >= 0 ? 1 : -1;
        int absVel = abs(encVel);
        if(absVel < 128)
        {
            encAcc = sign;
        }
        if(absVel < 65536)
        {
            encAcc = sign*(absVel >> 7);
        }
        else
        {
            encAcc = sign*512;
        }
        acc_cycles = encVel/encAcc;
    }
    return_seg.acceleration = encAcc;
    return_seg.numberOfCycles = acc_cycles;
    return_seg.velocity = 0;
    return_seg.jerk = 0;
    segmentEvolve(return_seg, endingPosition, endingVelocity, endingAcceleration);
    return_seg.position -= (endingPosition - return_seg.position);
    return return_seg;
}


bool Basda::MoConModuleProfile::shinsGetEndingRampSegments(const Basda::MoConModuleProfile::Segment & lastSegment, Basda::MoConModuleProfile::Segment & dec_seg, Basda::MoConModuleProfile::Segment & stop_seg)
{
    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;
    double position, velocity, acceleration;
    segmentEvolve(lastSegment, position, velocity, acceleration);

    int encVel = (int)(encode16*velocity);

    if(encVel != 0)
    {
        int sign = encVel >= 0 ? 1 : -1;
        int encAcc, dec_cycles;
        int absVel = abs(encVel);
        if(absVel < 128)
        {
            encAcc = -sign;
        }
        else if(absVel < 65536)
        {
            encAcc = -sign*(absVel >> 7);
        }
        else
        {
            encAcc = -sign*512;
        }
        dec_cycles = -encVel/encAcc;

        dec_seg.position = (int)position;
        dec_seg.velocity = encVel;
        dec_seg.acceleration = encAcc;
        dec_seg.jerk = 0;
        dec_seg.numberOfCycles = dec_cycles;
        dec_seg.startCycle = lastSegment.startCycle + lastSegment.numberOfCycles;
        dec_seg.startDate = Nice::Date();

        segmentEvolve(dec_seg, position, velocity, acceleration);
    }

    stop_seg.position = (int)position;
    stop_seg.velocity = 0;
    stop_seg.acceleration = 0;
    stop_seg.jerk = 0;
    stop_seg.numberOfCycles = 0;
    stop_seg.startCycle = dec_seg.startCycle + dec_seg.numberOfCycles;
    stop_seg.startDate = Nice::Date();

    if(encVel == 0)return false;
    return true;
}


int Basda::MoConModuleProfile::shinsGetStartingRamp(const std::vector<Nice::PointVar> & _positionSequence)
{
     return (int)(shinsGetStartingRampSegment(_positionSequence).position - _positionSequence[0].yp);
}



/*******************************************
* @brief Fits the MoCon profile to the trajectory using ad hoc algorithm.
*
* @param  _startDate the start date of the trajectory in a civil date format
*               This value is just needed to store the start date of
*               each segment for debugging purposes.
* @param _startCycle the start MoCon cycle. This value is just needed
*                    to store the start cycle of each segment for
*                    debugging purposes.
* @param[in] _trajectory the trajectory where the x value is the cycle time
*                    and the y value is the MoCon position in steps.
*     The task of this procedure is to re-fit this representation (as a 
*     discrete set of values) to a sequence of low-order polynomials.
* @param[in] _cycleTime the time for one MoCon cycle
*
* @return the number of generated segments including eventually one acceleration segment and eventually two deceleration segments*/
unsigned int Basda::MoConModuleProfile::shinsFitExternalProfile(Nice::Date _startDate,
                                                                Nice::I64 _startCycle,
                                                                std::vector<Nice::PointVar> &_trajectory,
                                                                double _cycleTime)
{
    // static int fit_cycle = 0;
    // fit_cycle++;
    // if(fit_cycle % 3 == 0)
    // {
    //     std::stringstream error;
    //     error << "Profile is momentarily interdicted. m_profile.size() = " << m_profile.size();
    //     E_LOG(error.str());
    //     throw(std::runtime_error(error.str()));
    // }

    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;

    m_trajectory.clear();
    m_trajectory = _trajectory;

    std::vector<double> shins_cycles;
    std::vector<double> shins_positions;
    unsigned int segmentLength = m_maxSamplesPerSegment;

    m_profile.clear();

    /* initialize mimic a motor at rest, at local time zero. */
    unsigned int segmentStart = 0;
    double endingPosition = 0;
    double endingVelocity = 0;
    double endingAcceleration = 0;
    double lastJerk = 0;
    int lastNumberOfCycles = 0;
    std::string sep = "\t";

    Segment shins_segment;
    std::vector<Segment> shins_profile;

    int AA, BB, CC, DD;
    double maxdelta, shins_last_position = 0, shins_last_velocity = 0;

    shins_segment = shinsGetStartingRampSegment(m_trajectory);
    shins_segment.startCycle = _startCycle - shins_segment.numberOfCycles;
    shins_segment.startDate = _startDate - Nice::Time::microSeconds(Nice::I64(_cycleTime*shins_segment.numberOfCycles));
    shins_profile.push_back(shins_segment);


    U2_LOG("index\tposition\tvelocity\tacceleration\tjerk\tnumberOfCycles\tstartCycle\tmaxdelta\tmax_deviation\tdeviation\tseg_length\tstartDate");
    U2_LOG("str : " << shins_profile.size() - 1<< sep << shins_segment.position << sep << shins_segment.velocity << sep << shins_segment.acceleration << sep << shins_segment.jerk << sep << shins_segment.numberOfCycles << sep << shins_segment.startCycle << sep << 0 << sep << 0 << sep << 0 << sep << 0 << sep << shins_segment.startDate);
    while(segmentStart + segmentLength <= m_trajectory.size())
    {
        //...m_segmentLength might have been reduced. So adapt the vector sizes accordingly...
        shins_cycles.resize(segmentLength + 1);
        shins_positions.resize(segmentLength + 1);

        //...read in all data of the next segment...
        for(unsigned int i = 0; i < segmentLength; i++)
        {
            shins_cycles[i] = (m_trajectory.at(segmentStart + i)).xp - (m_trajectory.at(segmentStart)).xp;
            shins_positions[i] = (m_trajectory.at(segmentStart + i)).yp;
        }
        shins_cycles[segmentLength] = m_trajectory.at(segmentStart + segmentLength).xp - m_trajectory.at(segmentStart).xp;
        shins_positions[segmentLength] = m_trajectory.at(segmentStart + segmentLength).yp;

        ::Nice::Alg::shinsBestFitF(shins_cycles, shins_positions, AA, BB, CC, DD, maxdelta);
        shins_segment.position = AA;
        shins_segment.velocity = BB;
        shins_segment.acceleration = CC;
        shins_segment.jerk = DD;
        shins_segment.numberOfCycles = (int)m_trajectory.at(segmentLength).xp;
        {
            const Segment & previous_seg = shins_profile.at(shins_profile.size() - 1);
            shins_segment.startCycle = previous_seg.startCycle + previous_seg.numberOfCycles;
            shins_segment.startDate = previous_seg.startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime*previous_seg.numberOfCycles));
        }

        //...now, calculate for each point in the trajectory the corresponding position value from the fit
        //    and find out how much the fit deviates from the requested trajectory...
        Nice::Date t0 = Nice::Date::now();
        calcMaxDeviation(segmentStart, shins_segment, endingPosition, endingVelocity, endingAcceleration, lastJerk, lastNumberOfCycles);
        shins_last_position = endingPosition;
        shins_last_velocity = endingVelocity;
        Nice::Date t1 = Nice::Date::now();

        bool fitAccepted = true;
        if(m_faultTolerance > 0 && ((unsigned int)fabs(m_maxDeviation) >= m_faultTolerance) )
        {
            int new_segment_length = segmentLength/2;
            if(segmentLength > m_minSamplesPerSegment && new_segment_length < m_minSamplesPerSegment)new_segment_length = m_minSamplesPerSegment;
            if(segmentLength == m_minSamplesPerSegment)
            {
                std::stringstream error;
                error << "Fault Tolerance of " << m_maxDeviation << " for segment " << m_profile.size() + 1;
                W_LOG(error.str());
            }
            else
            {
                fitAccepted = false;
                segmentLength = new_segment_length;
            }
        }
        if(fitAccepted)
        {
            U2_LOG("seg : " << shins_profile.size() << sep << shins_segment.position << sep << shins_segment.velocity << sep << shins_segment.acceleration << sep << shins_segment.jerk << sep << shins_segment.numberOfCycles << sep << shins_segment.startCycle << sep << maxdelta << sep << m_maxDeviation << sep << m_deviation << sep << segmentLength << sep <<  segmentStart << sep << shins_segment.startDate);
            shins_profile.push_back(shins_segment);

            segmentStart += segmentLength;
            if(segmentStart + m_maxSamplesPerSegment < m_trajectory.size())
            {
                segmentLength = m_maxSamplesPerSegment;
            }
            else if((segmentStart + m_maxSamplesPerSegment >= m_trajectory.size()) && (m_trajectory.size() - segmentStart >= m_minSamplesPerSegment))
            {
                segmentLength = (unsigned int)m_trajectory.size() - segmentStart - 1;
            }
            else
            {
                break;
            }
        }
    } /* end of loop that checks segmentstart plus length less than sample size */

    Segment dec_seg, stop_seg;
    bool result = shinsGetEndingRampSegments(shins_segment, dec_seg, stop_seg);
    dec_seg.startDate = shins_segment.startDate + Nice::Time::microSeconds(_cycleTime*shins_segment.numberOfCycles);
    stop_seg.startDate = dec_seg.startDate + Nice::Time::microSeconds(_cycleTime*dec_seg.numberOfCycles);
    if(result)
    {
//        U2_LOG("_end__segment _-> start_velocity : " << " ~ " << (int)(encode16*endingVelocity) << "/" << (60000000*endingVelocity/_cycleTime)*360/40960000 << "; shins_last_position : " << (int)endingPosition << "; lastNumberOfCycles : " << lastNumberOfCycles);
        U2_LOG("end : " << shins_profile.size() << sep << dec_seg.position << sep << dec_seg.velocity << sep << dec_seg.acceleration << sep << dec_seg.jerk << sep << dec_seg.numberOfCycles << sep << dec_seg.startCycle << sep << 0 << sep << 0 << sep << 0 << sep << 0 << sep << dec_seg.startDate);
        shins_profile.push_back(dec_seg);
    }
    U2_LOG("stp : " << shins_profile.size() << sep << stop_seg.position << sep << stop_seg.velocity << sep << stop_seg.acceleration << sep << stop_seg.jerk << sep << stop_seg.numberOfCycles << sep << stop_seg.startCycle << sep << 0 << sep << 0 << sep << 0 << sep << 0 << sep << stop_seg.startDate);
    shins_profile.push_back(stop_seg);

    // shins_segment.position = shins_last_position;
    // shins_segment.numberOfCycles = (int)(2000000/_cycleTime); // deceleration ramp of 2 seconds
    // shins_segment.velocity = encode16*shins_last_velocity;
    // shins_segment.acceleration = -boost::math::iround(encode16*shins_last_velocity/(shins_segment.numberOfCycles - 1));
    // shins_segment.jerk = 0;
    // if(shins_segment.acceleration != 0 && abs(shins_segment.acceleration) < shins_segment.numberOfCycles)
    // {
    //     shins_segment.numberOfCycles = -boost::math::iround(encode16*shins_last_velocity/shins_segment.acceleration) + 1;
    // }
    // {
    //     const Segment & previous_seg = shins_profile.at(shins_profile.size() - 1);
    //     shins_segment.startCycle = previous_seg.startCycle + previous_seg.numberOfCycles;
    //     shins_segment.startDate = previous_seg.startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime*previous_seg.numberOfCycles));
    // }
    // shins_profile.push_back(shins_segment);
    // calcMaxDeviation(segmentStart, shins_segment, endingPosition, endingVelocity, endingAcceleration, lastJerk, lastNumberOfCycles);
    // U2_LOG("_end__segment _-> start_velocity : " << " ~ " << (int)(encode16*endingVelocity) << "/" << (60000000*endingVelocity/_cycleTime)*360/40960000 << "; shins_last_position : " << (int)endingPosition << "; lastNumberOfCycles : " << lastNumberOfCycles);
    // U2_LOG("_end__segment _: " << shins_profile.size() << " ~ " << shins_segment.position << " ~ " << shins_segment.velocity  << " ~ " << shins_segment.acceleration << " ~ " << shins_segment.jerk << " ~ " << shins_segment.position - (int)shins_last_position);
    // shins_segment.position = shins_last_position;
    // shins_segment.numberOfCycles = 0;
    // shins_segment.velocity = 0;
    // shins_segment.acceleration = 0;
    // shins_segment.jerk = 0;
    // {
    //     const Segment & previous_seg = shins_profile.at(shins_profile.size() - 1);
    //     shins_segment.startCycle = previous_seg.startCycle + previous_seg.numberOfCycles;
    //     shins_segment.startDate = previous_seg.startDate + Nice::Time::microSeconds(Nice::I64(_cycleTime*previous_seg.numberOfCycles));
    // }
    // shins_profile.push_back(shins_segment);

//    if(fit_cycle % 3 == 1)
    {
        m_profile = shins_profile;
    }
    // std::vector<Segment>::iterator itSeg;
    // U2_LOG("position\tvelocity\tacceleration\tjerk\tnumberOfCycles\tstartCycle\tstartDate");
    // for(itSeg = m_profile.begin(); itSeg < m_profile.end(); itSeg++)
    // {
    //     U2_LOG(itSeg->position << "\t" << itSeg->velocity << "\t" << itSeg->acceleration << "\t" << itSeg->jerk << "\t" << itSeg->numberOfCycles << "\t" << itSeg->startCycle << "\t" << itSeg->startDate);
    // }

    return (unsigned int)m_profile.size();
} /* shinsFitExternalProfile */


/**
* @brief Basis functions for a degree 3 polynomial
*
* Function provided to the LNMath function lfit within ProfiFit()
*
* @param[in] _x Value onthe abscissa
* @param[out] _afunc returns a constant 1 for index 0, _x scaled by a factor 2^16 for index 1 and so on.
* @see Nice::Alg::linearFit
*/
void Basda::MoConModuleProfile::basePol3(const double _x,
                                         boost::numeric::ublas::vector<double> &_afunc)
{
    _afunc(0) = 1.; // position
    _afunc(1) = _x / Basda::Profile::FixPointEncoding_16_16; // velocity
    _afunc(2) = 0.5 * ::Nice::Alg::SQR(_x) / Basda::Profile::FixPointEncoding_16_16; // acceleration
    _afunc(3) = 1. / 6. * ::Nice::Alg::SQR(_x) * _x / Basda::Profile::FixPointEncoding_0_32; // jerk
}


/**
* @brief calculates the biggest deviation between the input and
*        fitted trajectory.
* Calculates also m_deviation, the deviation between fit and requested position at the end of the segment.
*/
void Basda::MoConModuleProfile::calcMaxDeviation(unsigned int _currentSample,
                                                   Segment _segment,
                                                   double &_position,
                                                   double &_velocity,
                                                   double &_acceleration,
                                                   double &_jerk,
                                                   int &_numberOfCycles)
{
    m_deviation = 0;
    m_maxDeviation = 0;
    unsigned int currentCycle = 0;
    if(_currentSample >= 0 && _currentSample < m_trajectory.size())
    {
        currentCycle = (unsigned int)(m_trajectory.at(_currentSample)).xp;
    }

    _position = (double) _segment.position;
    _velocity = (double) _segment.velocity
            / (double) Basda::Profile::FixPointEncoding_16_16;
    _acceleration = (double) _segment.acceleration
            / (double) Basda::Profile::FixPointEncoding_16_16;
    _jerk = (double) _segment.jerk / (double) Basda::Profile::FixPointEncoding_0_32;
    _numberOfCycles = _segment.numberOfCycles;

    int j;
    for (j = 0; j < _numberOfCycles + 1; j++)
    {
        if (_currentSample < m_trajectory.size() && currentCycle == (m_trajectory.at(_currentSample)).xp)
        {
//            Nice::PointVar point;
//            point.xp = currentCycle;
//            point.yp = (int) position;
//            m_fitTrajectory.at(_currentSample) = point;

            m_deviation = _position - (m_trajectory.at(_currentSample)).yp;
            if (fabs(m_deviation) > fabs(m_maxDeviation)) 
                 m_maxDeviation = m_deviation;
            _currentSample++;
        }
        _position += _velocity + _acceleration / 2 + _jerk / 6;
        _velocity += _acceleration + _jerk / 2;
        _acceleration += _jerk;

        currentCycle++;
    }
}

void Basda::MoConModuleProfile::segmentEvolve(Segment _segment, double &_position, double &_velocity, double &_acceleration)
{
    const double encode16 = 65536.;
    const double encode32 = 65536.*65536;
    _position = _segment.position;
    _velocity = _segment.velocity/encode16;
    _acceleration = _segment.acceleration/encode16;
    double jerk = _segment.jerk /encode32;
    int numberOfCycles = _segment.numberOfCycles;

    for (int j = 0; j < numberOfCycles + 1; j++)
    {
        _position += _velocity + _acceleration / 2 + jerk / 6;
        _velocity += _acceleration + jerk / 2;
        _acceleration += jerk;
    }
}

/********************************
* @brief Returns the size of the external profile to allocate the
*        memory at the motor controller.
*
* @return the size of the external profile
*/
unsigned int Basda::MoConModuleProfile::getExternalProfileSize()
{
    return (unsigned int)m_profile.size();
}

/*****************************************
* @brief Returns a segment of a profile at index.
*
* @param _index the index of the segment
* @param _position return value with the new position
* @param _velocity return value with the new velocity
* @param _acceleration return value with the new acceleration
* @param _jerk return value with the new jerk
* @param _numberOfCycles return value with the new numberOfCycles
*/
void Basda::MoConModuleProfile::getExternalProfile(int _index,
                                                   int &_position,
                                                   int &_velocity,
                                                   int &_acceleration,
                                                   int &_jerk,
                                                   int &_numberOfCycles)
{
    try
    {
        Segment segment = m_profile.at(_index);
        _position = segment.position;
        _velocity = segment.velocity;
        _acceleration = segment.acceleration;
        _jerk = segment.jerk;
        _numberOfCycles = segment.numberOfCycles;
    }
    catch (std::out_of_range &e)
    {
        E_THROW(::Basda::MoCon::VectorRangeCheckException, "Vector out of range. The size of the External Profile vector is " << m_profile.size() << ". The current index is " << _index << ".");
    }
}

std::vector<Basda::MoConModuleProfile::Segment> Basda::MoConModuleProfile::getExternalProfile()
{
    return m_profile;
}

#if 0
/**
* @brief Dump the external profile in a file.
*/
void Basda::MoConModuleProfile::dumpExternalProfile()
{
    std::string date = Nice::Date::now().dateString();
    std::string time = Nice::Date::now().timeString(0);
    std::ofstream trajectoryFile(
            (date + std::string("_") + time + std::string("_m_trajectory.dat"))
                    .c_str());      // original curve
    std::ofstream trajectoryNodesFile(
            (date + std::string("_") + time
                    + std::string("_m_trajectoryNodes.dat")).c_str()); // Stuetzstellen
    std::ofstream fitTrajectoryFile(
            (date + std::string("_") + time
                    + std::string("_m_fitTrajectory.dat")).c_str()); // fitted Stuetzstellen + samples per Segment (nur Positionswerte)
    std::ofstream fitTrajectoryNodeFile(
            (date + std::string("_") + time
                    + std::string("_m_fitTrajectoryNode.dat")).c_str()); // fitted Stuetzstellen + samples per Segment (nur Positionswerte)
    std::ofstream correctedFitTrajectoryFile(
            (date + std::string("_") + time
                    + std::string("_m_correctedFitTrajectory.dat")).c_str()); // fitted Stuetzstellen + samples per Segment (nur Positionswerte)
    std::ofstream deviationFile(
            (date + std::string("_") + time + std::string("_deviation.dat"))
                    .c_str());
    std::ofstream deviationNodeFile(
            (date + std::string("_") + time + std::string("_deviationNode.dat"))
                    .c_str());
    std::ofstream stepSizeFile(
            (date + std::string("_") + time + std::string("_stepSize.dat"))
                    .c_str());
    std::ofstream proFile(
            (date + std::string("_") + time + std::string("_profile.dat")).c_str());

    trajectoryFile.flags(std::ios_base::fixed);
    trajectoryNodesFile.flags(std::ios_base::fixed);
    correctedFitTrajectoryFile.flags(std::ios_base::fixed);
    fitTrajectoryFile.flags(std::ios_base::fixed);
    deviationFile.flags(std::ios_base::fixed);
    deviationNodeFile.flags(std::ios_base::fixed);
    stepSizeFile.flags(std::ios_base::fixed);
    proFile.flags(std::ios_base::fixed);

    unsigned int i;
    try
    {
        for (i = 0; i < m_trajectory.size(); i++)
            trajectoryFile << (m_trajectory.at(i)).xp << "\t"
                           << (m_trajectory.at(i)).yp << std::endl;
    }
    catch (std::out_of_range &e)
    {
        E_THROW(::Basda::MoCon::VectorRangeCheckException, "Vector out of range.");
    }

    try
    {
        for (i = 0; i < m_fitTrajectory.size(); i++)
        {
            fitTrajectoryFile << (m_fitTrajectory.at(i)).xp << "\t"
                              << (m_fitTrajectory.at(i)).yp << std::endl;

            deviationFile
                    << (m_fitTrajectory.at(i)).xp << "\t"
                    << (m_fitTrajectory.at(i)).yp - (m_trajectory.at(i).yp)
                    << std::endl;
            if (i > 0)
                stepSizeFile
                        << (m_fitTrajectory.at(i)).xp
                        << "\t"
                        << abs((m_fitTrajectory.at(i)).yp
                                - (m_fitTrajectory.at(i - 1)).yp)
                        << std::endl;
        }
    }
    catch (std::out_of_range &e)
    {
        E_THROW(::Basda::MoCon::VectorRangeCheckException, "Vector out of range.");
    }

    try
    {
        for (i = 0; i < m_trajectoryNodes.size(); i++)
        {
            trajectoryNodesFile << (m_trajectoryNodes.at(i)).xp << "\t"
                                << (m_trajectoryNodes.at(i)).yp << std::endl;
            fitTrajectoryNodeFile << (m_fitTrajectoryNodes.at(i)).xp << "\t"
                                  << (m_fitTrajectoryNodes.at(i)).yp
                                  << std::endl;
            deviationNodeFile
                    << (m_fitTrajectoryNodes.at(i)).xp
                    << "\t"
                    << (m_fitTrajectoryNodes.at(i)).yp
                            - (m_trajectoryNodes.at(i)).yp
                    << std::endl;
        }
    }
    catch (std::out_of_range &e)
    {
        E_THROW(::Basda::MoCon::VectorRangeCheckException, "Vector out of range.");
    }

    try
    {
        Segment segment;
        double cycle = 0;
        for (i = 0; i < m_profile.size(); i++)
        {
            segment = m_profile.at(i);
            proFile << i << "\t" << cycle << "\t" << segment.position << "\t"
                    << segment.velocity << "\t" << segment.acceleration << "\t"
                    << segment.jerk << "\t" << segment.numberOfCycles << "\t"
                    << std::endl;
            cycle += segment.numberOfCycles;
        }
    }
    catch (std::out_of_range &e)
    {
        E_THROW(::Basda::MoCon::VectorRangeCheckException, "Vector out of range.");
    }
}

#endif /* 0 */
