/*! @file
 *  @brief This file keeps the methods to program the MoCon to follow an external profile.
 *
 * @author Frank Kittmann
 * @version 09.06.23
 * @date 2009.06.23
 */

#pragma once

/* $Header: https://svn.mpia.de/gulli/TwiceAsNice/branch/unstable/TwiceAsNice/basda-mocca/src/device/mpia/mocon/Profile.h 15119 2018-02-13 18:44:27Z mathar $
*/


#include <boost/numeric/ublas/vector.hpp>
#include <Nice/Map/Point.h>
#include <Nice/Time/Date.h>
#include <Basda/Mocca/Constant.h>

namespace Basda
{
    /**
     * @brief External MoCon profile: Representation of a trajectory as list of
     *        parameterized segments.
     *
     * A MoCon profile uses degree 3 polynomials to describe each segment of a
     * motor trajectory.
     * The Coefficients of the polynomials represent \b time, \b position,
     * \b velocity, \b acceleration, and \b jerk.
     *
     * The time defines when the next trajectory point will be loaded. The
     * position of the next trajectory point have to be the same
     * as the current position point. The next acceleration, velocity and jerk
     * values define the next movement.
     *
     * The position of the first trajectory point have to be the start position.
     * If it is not the start position the MoCon will do a jump which can
     * introduce errors.
     *
     * The position value of the next trajectory point
     * must be the same as the current position value. Which means the position
     * value of the next trajectory point defines the current
     *
     * This class
     *  - transfers a specified trajectory into a MoCon Profile. The specified
     *    trajectory is a vector of positions which are equally spaced in time.
     *
     *
     *
     * @todo soon: provides a method to change the current trajectory
     * @todo soon: deals with the UTC start time for the profile. Needed for
     *             absolute time trajectories.
     */
    class MoConModuleProfile
    {
        public:
            MoConModuleProfile();

//            MoConModuleProfile(std::vector<Nice::PointVar> &_trajectory,
//                               unsigned int _samplesPerSegment,
//                               unsigned int _maxError);
            ~MoConModuleProfile();

//            void setExternalProfilePositionSequence(std::vector<Nice::PointVar> &_trajectory);

            void setExternalProfileMaxSamplesPerSegment(unsigned int _maxSamplesPerSegment);

            void setExternalProfileMinSamplesPerSegment(unsigned int _minSamplesPerSegment);

            void setExternalProfileFaultTolerance(unsigned int _maxError);

            unsigned int fitExternalProfile(Nice::Date _date,
                                    Nice::I64 _startCycle,
                                    std::vector<Nice::PointVar> &_trajectory,
                                    double _cycleTime);
            unsigned int shinsFitExternalProfile(Nice::Date _date,
                                                 Nice::I64 _startCycle,
                                                 std::vector<Nice::PointVar> &_trajectory,
                                                 double _cycleTime);
            static int shinsGetStartingRamp(const std::vector<Nice::PointVar> & _positionSequence);


            unsigned int getExternalProfileSize();

            /**
             * @brief Single segment of a MoCon profile
             */
            struct Segment
            {
                    int position;        //!< position in counts
                    int velocity;        //!< velocity in counts/cycle
                    int acceleration;    //!< acceleration in counts/cycle^2
                    int jerk;            //!< jerk in counts/cycle^3
                    int numberOfCycles;  //!< number of cycles (duration)
                    Nice::I64 startCycle; //!< absolute start cycle of this segment
                    Nice::Date startDate; //!< start date of this segment
            };

            static void segmentEvolve(Segment _segment, double &_position, double &_velocity, double &_acceleration);
            static Segment shinsGetStartingRampSegment(const std::vector<Nice::PointVar> & _positionSequence, unsigned int durationInCycles=3255);
            static bool shinsGetEndingRampSegments(const Basda::MoConModuleProfile::Segment & lastSegment, Basda::MoConModuleProfile::Segment & dec_seg, Basda::MoConModuleProfile::Segment & stop_seg);

            void getExternalProfile(int _index,
                                    int &_position,
                                    int &_velocity,
                                    int &_acceleration,
                                    int &_jerk,
                                    int &_numberOfCycles);
            std::vector<Segment> getExternalProfile();

//            void dumpExternalProfile();

        private:
            static void basePol3(const double _x,
                                 boost::numeric::ublas::vector<double> &_afunc);

            /**
             * @brief Type of a full MoCon trajectory (in steps or counts and cycles) that is to be transfered to a mocon profile.
             *
             * x is the time in cycles
             * y is the position in counts or steps
             */
            std::vector<Nice::PointVar> m_trajectory;
//
//            std::vector<Nice::PointVar> m_fitTrajectory; //!<Trajectory with the fitted values
//            std::vector<Nice::PointVar> m_trajectoryNodes; //!<Trajectory containing only the nodes
//            std::vector<Nice::PointVar> m_fitTrajectoryNodes; //!<Trajectory containing only the nodes
//            std::vector<Nice::PointVar> m_correctedFitTrajectory;//!<Trajectory with the corrected fitted values

            /**
             * @brief The MoCon profile vector consist of segments.
             */
            std::vector<Segment> m_profile;

            /**
             * @brief Fault Tolerance in steps (counts).
             *
             * This is the maximum acceptable deviation of the fitted trajectory
             * from the input trajectory.
             * The deviation is determined for each specified point in the input
             * trajectory.
             */
            unsigned int m_faultTolerance;

            /**
             * @brief The number of points on the trajectory that should be
             *        represented by a single polynomial.
             *
             * Each n*samplesPerSegment point will be a knot on the piecewise
             * polynomial The sampling will be reduced,
             * if the fitted polynomial of the corresponding segment cannot be
             * fitted accurately enough.
             */
            unsigned int m_maxSamplesPerSegment;
            unsigned int m_minSamplesPerSegment;

            /* @brief This member stores the deviation of the segment end.
             *
             * The next segment will correct the deviation of the previous segment
             * by adding that value.
             */
            double m_deviation;
            double m_maxDeviation;

            void calcMaxDeviation(unsigned int _segmentStart,
                                    Segment _segment,
                                    double &_position,
                                    double &_velocity,
                                    double &_acceleration,
                                    double &_jerk,
                                    int &_numberOfCycles);
    };

    /**
     * @brief Definition of the smart pointer of this class.
     */
    typedef IceUtil::Handle<Basda::MoConModuleProfile> MoConModuleProfilePtr;
}
