# ifndef TRAJ_GEN
# define TRAJ_GEN

//////////////////////////////////////////////////////////////////////////////////
// TODO : Preventing T imposed pins (t,d same, X different)
// TODO : check this works well with angular elements
//////////////////////////////////////////////////////////////////////////////////

#include <vector>
#include <set>
#include <algorithm>
#include <numeric>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <qpOASES.hpp>
#include <Eigen/LU>

using namespace Eigen;
using namespace std;

/**
 * Trajectory generation modules
 */
namespace trajgen {

    /////////////
    // Typedef //
    /////////////

    typedef unsigned int uint;
    template<typename T,size_t Size> using Vector = Eigen::Matrix<T, Size, 1>;
    template<typename T> using VectorX = Eigen::Matrix<T, -1, 1>;
    typedef uint d_order; // derivative order
    typedef uint p_order; // polynomial order
    template<typename T> using time_knots = vector<T> ;
    template<typename T> using spMatrixRow = SparseMatrix<T,RowMajor>;
    template<typename T> using PolyCoeff =  Eigen::Matrix<T, -1, 1>;
    template<typename T> using MatrixRow = Matrix<T,-1,-1,RowMajor>;

    ///////////////
    // Sub utils //
    ///////////////

    /**
     * @brief Insert inMat to targetMat.block(startRow,startCol,rowSize,colSize).
     * @details As the block insertion for sparse matrix is not implemented in Eigen3, I wrote.
     * @param targetMat Insertion target (big)
     * @param inMat Inserted matrix (small)
     * @param startRow
     * @param startCol
     */
    template <typename T>
    void sparseBlockCopy(SparseMatrix<T,RowMajor> *targetMat, SparseMatrix<T,RowMajor> inMat, long startRow, long startCol) {
        for (int k = 0; k < inMat.rows(); ++k)
            for (typename SparseMatrix<T,RowMajor>::InnerIterator it(inMat, k); it; ++it) { // we use sparsity
                long insertR = it.row() + startRow;
                long insertC = it.col() + startCol;
                targetMat->coeffRef(insertR, insertC) = it.value();
            }
    }
    /**
     * @brief Insert inMat to targetMat.block(startRow,startCol,rowSize,colSize). As the block insertion for sparse matrix is not implemented in Eigen3, I wrote.
     * @param targetMat Insertion target (big)
     * @param inMat Inserted matrix (small)
     * @param startRow
     * @param startCol
     */
    template <typename T>
    void sparseBlockCopy(SparseMatrix<T,RowMajor> *targetMat, SparseMatrix<T,ColMajor> inMat, long startRow, long startCol) {
        for (int k = 0; k < inMat.cols(); ++k)
            for (typename SparseMatrix<T,ColMajor>::InnerIterator it(inMat, k); it; ++it) { // we use sparsity
                long insertR = it.row() + startRow;
                long insertC = it.col() + startCol;
                targetMat->coeffRef(insertR, insertC) = it.value();
            }
    }
    /**
     * @brief Insert inMat to targetMat.block(startRow,startCol,rowSize,colSize). As the block insertion for sparse matrix is not implemented in Eigen3, I wrote.
     * @param targetMat Insertion target (big)
     * @param inMat Inserted matrix (small)
     * @param startRow
     * @param startCol
     */
    template <typename T>
    void sparseBlockCopy(SparseMatrix<T,RowMajor> *targetMat, Matrix<T,-1,-1,RowMajor> inMat, long startRow, long startCol) {
        for (int k = 0; k < inMat.rows(); ++k)
            for (typename Matrix<T,-1,-1,RowMajor>::InnerIterator it(inMat, k); it; ++it) { // we use sparsity
                long insertR = it.row() + startRow;
                long insertC = it.col() + startCol;
                targetMat->coeffRef(insertR, insertC) = it.value();
            }
    }

    /**
     * @brief extract the diff element (setA - setB).
     * @tparam T
     * @param setA
     * @param setB
     * @return
     */
    template <typename T> vector<T> setDiff(const set<T> & setA, const set<T> & setB){
        vector<T> output_it(setA.size());
        auto it =std::set_difference(setA.begin(),setA.end(),
                setB.begin(),setB.end(),output_it.begin());

        output_it.resize(it-output_it.begin());
        return output_it;
    }
    /**
     * @brief Perform interpolation for the data pair (xData,yData) given a query value x
     * @tparam T
     * @param xData
     * @param yData
     * @param x
     * @param extrapolate if true, extrapolation
     * @return
     */
    template <typename T>
    T interpolate(const VectorX<T>& xData, const VectorX<T>& yData,const T & x,bool extrapolate){
        int size = xData.size();
        int i = 0;                                                                  // find left end of interval for interpolation
        if ( x >= xData(size - 2) )                                                 // special case: beyond right end
        {
            i = size - 2;
        }
        else
        {
            while ( x > xData(i+1) ) i++;
        }
        T xL = xData(i), yL = yData(i), xR = xData(i+1), yR = yData(i+1);      // points on either side (unless beyond ends)
        if ( !extrapolate )                                                         // if beyond ends of array and not extrapolating
        {
            if ( x < xL ) yR = yL;
            if ( x > xR ) yL = yR;
        }

        T dydx = ( yR - yL ) / ( xR - xL );                                    // gradient
        return yL + dydx * ( x - xL );                                              // linear interpolation
    }

    /////////////////////////////////////////////////
    // PIN for equality or inequality constriants  //
    /////////////////////////////////////////////////

    enum PIN_TYPE {
        FIX_PIN = 0, // equality constraint
        LOOSE_PIN = 1 // inequality constraint
    };
    /**
     * @brief Pin object represent the constraints to be imposed on trajectory.
     * @tparam T datatype (e.g. float or double). The choice of the datatype critical for the accuaracy
     * @tparam dim dimension of operational space (e.g 2D or 3D)
     */
    template<typename T,size_t dim>
    struct Pin{
        T t; // imposed time
        uint d; // imposed order of derivative
        Pin(T t_, d_order d_) : t(t_), d(d_) {};
        virtual PIN_TYPE getType() const = 0;
    };
    /**
     * @brief Equality constraint
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    struct FixPin : public Pin<T,dim> {
        Vector<T,dim> x; // waypoint
        FixPin(T t_, uint d_, Vector<T,dim> x_) : Pin<T,dim>(t_, d_), x(x_) {};
        PIN_TYPE getType() const { return PIN_TYPE::FIX_PIN; }
    };

    /**
     * @brief Inequality constraint
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    struct LoosePin : public Pin<T,dim> {
        Vector<T,dim> xl; // lower bound
        Vector<T,dim> xu; // upper bound
        LoosePin(T t_, uint d_, Vector<T,dim> xl_, Vector<T,dim> xu_) : Pin<T,dim>(t_, d_), xl(xl_), xu(xu_) {};
        PIN_TYPE getType() const { return PIN_TYPE::LOOSE_PIN; }
    };

    /////////////////////////////////////
    // Quadratic programming structure //
    /////////////////////////////////////
    /**
     * @brief Constraint matrix pair (Ax = b or Ax <= b). A is assumed to be sparse while b to be as dense matrix.
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    struct ConstraintMatPair {
        vector<spMatrixRow<T>> ASet;
        vector<MatrixRow<T>> bSet;

        ConstraintMatPair() {
            ASet.resize(dim), bSet.resize(dim);
        }
        /**
         * @brief Intialize the size of matrix AB
         * @param Nc number of constraints (row)
         * @param nVar number of variable of the optimization (column)
         */
        void initialize(uint Nc, uint nVar) {
            for (spMatrixRow<T> &sp : ASet)
                sp = spMatrixRow<T>(Nc, nVar);
            for (MatrixRow<T> &mat : bSet)
                mat = MatrixRow<T>::Zero(Nc, 1);
        }
        ConstraintMatPair(uint Nc, uint nVar) {
            ASet.resize(dim), bSet.resize(dim);
            this->initialize(Nc, nVar);
        }
    };

    /**
     * @brief The basic qp block (p'Qp + Hp). Note that 1/2 is not multiplied!
     * @tparam T
     */
    template<typename T>
    struct QpBlock {
        spMatrixRow<T> Q;
        spMatrixRow<T> H;
        spMatrixRow<T> A;
        spMatrixRow<T> b;
        spMatrixRow<T> Aeq;
        spMatrixRow<T> beq;
        QpBlock(uint nVar, uint neq, uint nineq);
        QpBlock() {};
        VectorX<T> getQpSol(bool & isSolved);
    };

    /**
     * @brief Dimensional collection of qp Blocks.
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    struct QpForm {
        uint nVar,neq,nineq;
        QpBlock<T> *qpBlock;
        QpForm(uint nVar, uint neq, uint nineq) : nVar(nVar),neq(neq),nineq(nineq){
            qpBlock = new QpBlock<T>[dim];
            for (uint d = 0; d < dim; d++)
                qpBlock[d] = QpBlock<T>(nVar, neq, nineq);
        };
        void write();
        vector<VectorX<T>> getQpSolSet(bool & isSolved);
    };

    /////////////////////////////
    // TrajGen as a base class //
    /////////////////////////////
    /**
     * @brief TrajGen is abstract class as a base for polyTrajGen and optimTrajGen
     * @tparam T datatype (e.g. float or double). The choice of the datatype critical for the accuaracy
     * @tparam dim dimension of operational space (e.g. 2D or 3D)
     */
    template<typename T,size_t dim>
    class TrajGen {

    protected:
        uint M; // number of segment
        bool isSolved = false; // solve flag
        time_knots<T> ts; // time knots (do not be confused with time points in case of optimTrajGen)

        vector<FixPin<T,dim>> *fixPinSet;  // equality constraints set of M segment. (In case of optimTrajGen, M = 1)
        vector<LoosePin<T,dim>> *loosePinSet; // inequality constraints set of ()
        set<d_order> *fixPinOrderSet;
        VectorX<T> weight_mask; // high order derivative penalty weights

        // Subroutine functions
        void findSegInterval(T t, uint &m);
        void findSegInterval(T t, uint &m, T &tau);

        virtual ConstraintMatPair<T,dim> fixPinMatSet(const FixPin<T,dim> *pPin,uint blockSize = 1) = 0;
        virtual ConstraintMatPair<T,dim> loosePinMatSet(const LoosePin<T,dim> *pPin,uint blockSize = 1) = 0;
        virtual QpForm<T,dim> getQPSet() = 0; // create qp problem with current information
        uint getTotalNineq();
        virtual uint getTotalNeq(); // different for polyTrajGen and optimTrajGen (in case of optimTrajGen = TrajGen)

    public:
        TrajGen(time_knots<T> ts_);
        TrajGen() {};
        void addPinSet(const vector<Pin<T,dim> *> &pinPtrSet);
        void setDerivativeObj(VectorX<T> weights);
        virtual void addPin(const Pin<T,dim> *pin);
        virtual Vector<T,dim> eval(T t_eval, d_order d) = 0;
        virtual bool solve(bool verbose) = 0;
    };

    ///////////////////////////////////////////
    // PolyTrajGen for piecewise polynomials //
    ///////////////////////////////////////////
    /**
     * @brief Solving method for polynomial
     * @details POLY_COEFF = optimization w.r.t poly coefficient. Fast but unstable for large problem. END_DERIVATIVE = vice versa
     */
    enum ALGORITHM{
        POLY_COEFF = 0,
        END_DERIVATIVE = 1
    };

    /**
     * @brief PolyParameters to define the behavior of polynomial
     */
    struct PolyParam{
        p_order poly_order = 4;
        d_order max_conti_order = 2;
        ALGORITHM algo = ALGORITHM::POLY_COEFF;
        PolyParam() { printf("No default parameters given. Default values are used\n");};
        PolyParam(p_order N,d_order max_conti,ALGORITHM algo): poly_order(N),max_conti_order(max_conti),algo(algo) {};
    };

    /**
     * @brief State of the polynomial segment (number of imposed continuity, added fixed pin).
     */
    struct PolyState{
        uint Nc = 0; // order of continuity
        uint Nf = 0; // number of fixed pins
        uint getN() {return Nc+Nf;};
    };
    /**
     * @brief Piecewise polynomial trajectory. In this method, we define the primitive of trajectory as polynomial.
     * @details Optimization is performed w.r.t the coefficient of polynomials or free end-derivative of each polynomial segment
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    class PolyTrajGen : public TrajGen<T,dim>{

        private:
            PolyParam param;
            PolyState* seg_state_set;
            PolyCoeff<T>** poly_coeff_set; // [dim][M]
            p_order N; // poly order

            // subroutine functions
            T B(p_order n,d_order d);
            VectorX<T> tVec(T t,d_order d);
            spMatrixRow<T> scaleMat(T dt);
            spMatrixRow<T> scaleMatBig();
            spMatrixRow<T> scaleMatBigInv();

            // objective
            spMatrixRow<T> IntDerSquard(d_order d);
            // constraints
            ConstraintMatPair<T,dim> contiMat(uint m, d_order dmax);
            ConstraintMatPair<T,dim> fixPinMatSet(const FixPin<T,dim> * pPin,uint blockSize = 1);
            ConstraintMatPair<T,dim> loosePinMatSet(const LoosePin<T,dim> * pPin,uint blockSize = 1);
            uint getTotalNeq(); // overloaded
            // qp
            QpForm<T,dim> getQPSet();
            // map poly-ceoff to end-derivatives
            MatrixRow<T> coeff2endDerivatives(const spMatrixRow<T> & Aeq);
            QpForm<T,dim> mapQP(const QpForm<T,dim>& QpFormOrig);

    public:
            PolyTrajGen(time_knots<T> ts_,PolyParam param_) ;
            void addPin(const Pin<T,dim>* pin);
            Vector<T,dim> eval(T t_eval, d_order d);
            bool solve(bool verbose);
    };

    ///////////////////////////////
    // Optimal trajectory method //
    ///////////////////////////////
    /**
     * @brief The trajectory is represented as the linear interpolation of discrete points
     * @details In this method, the optimization variables are coordinate of each point
     * @tparam T
     * @tparam dim
     */
    template<typename T,size_t dim>
    class OptimTrajGen : public TrajGen<T,dim>{
        private:
            // member variables
            float pntDensity;
            uint nPnt;
            T dt;
            time_knots<T> tPnts; // time points
            VectorX<T>* trajPnts; // trajectory point. pointer = dim / VectorX<T> is element-wise trajectory

            // subroutine
            ConstraintMatPair<T,dim> fixPinMatSet(const FixPin<T,dim> * pPin,uint blockSize = 1);
            ConstraintMatPair<T,dim> loosePinMatSet(const LoosePin<T,dim> * pPin,uint blockSize = 1);
            uint matchIdx(T eval_t,d_order d = 0); // get the corresponding time point index of a evaluation time
            spMatrixRow<T> getDiff(d_order d);
            QpForm<T,dim> getQPSet();

        public:
            OptimTrajGen(time_knots<T> ts_, float pntDensity);
            void addPin(const Pin<T,dim>* pin);
            bool solve(bool isPrint);
            Vector<T,dim> eval(T t_eval, d_order d);
    };

    ///////////////////////////////////////////////////////////////////////////////////////

           // FUNCTION DEFINITIONS (as we use template, better to include defs here) //

    ///////////////////////////////////////////////////////////////////////////////////////

    /////////////////////////////////////
    // Quadratic programming structure //
    /////////////////////////////////////

    /**
     * write current problem and save it as txt file
     */
    template<typename T,size_t dim> void QpForm<T,dim>::write() {
        ofstream file ("QP");
        file << "Q:" << "\n" << MatrixRow<T>(qpBlock[0].Q) << endl;
        for (uint dd = 0 ; dd < dim ; dd++)
            file << "H[" << dd << "]:" << "\n" << MatrixRow<T>(qpBlock[dd].H) << endl;
        file << "A:" << "\n" << MatrixRow<T>(qpBlock[0].A) << endl;
        for (uint dd = 0 ; dd < dim ; dd++)
            file << "b[" << dd << "]:\n" << MatrixRow<T>(qpBlock[dd].b) << endl;

        file << "Aeq:" << "\n" << MatrixRow<T>(qpBlock[0].Aeq) << endl;
        for (uint dd = 0 ; dd < dim ; dd++)
            file << "beq[" << dd << "]:\n" << MatrixRow<T>(qpBlock[dd].beq) << endl;
        file.close();
    }

    template <typename T,size_t dim> vector<VectorX<T>> QpForm<T,dim>::getQpSolSet(bool & isSolved) {
        vector<VectorX<T>> qpSolSet(dim);
        isSolved = true;
        for (uint dd = 0; dd < dim ; dd++) {
            qpSolSet[dd] = qpBlock[dd].getQpSol(isSolved);
            if (not isSolved)
                break;
        }
        return qpSolSet;
    }

    template<typename T>
    QpBlock<T>::QpBlock(uint nVar, uint neq, uint nineq) {
        Q = spMatrixRow<T>(nVar, nVar);
        H = spMatrixRow<T>(1, nVar);
        A = spMatrixRow<T>(nineq, nVar);
        b = spMatrixRow<T>(nineq, 1);
        Aeq = spMatrixRow<T>(neq, nVar);
        beq = spMatrixRow<T>(neq, 1);
    }
    /**
     * @brief solve the current quadratic programming with qpOASES.
     * @tparam T
     * @param isSolved ref returned with result
     * @return
     */
    template<typename T>
    VectorX<T> QpBlock<T>::getQpSol(bool & isSolved) {
        uint nVar = Q.rows();
        int N_ineq_const = A.rows();
        int N_eq_const = Aeq.rows();
        int N_const = N_ineq_const + N_eq_const;
        // convention follows the qpOASES
        qpOASES::QProblem qp_obj(nVar, N_const, qpOASES::HST_SEMIDEF);
        Matrix<double, -1, -1, RowMajor> Hmat(2 * Q.template cast<double>());
        Matrix<double, 1, -1, RowMajor> gmat(H.template cast<double>());
        Matrix<double, -1, -1, RowMajor> Amat(N_const, nVar);
        Amat << MatrixXd(A.template cast<double>()), MatrixXd(Aeq.template cast<double>());
        Matrix<double, -1, -1, RowMajor> ubAmat(N_const, 1);
        ubAmat << MatrixXd(b.template cast<double>()), MatrixXd(beq.template cast<double>());
        MatrixXd MinusInf(N_ineq_const, 1);
        MinusInf=MinusInf.setConstant(-99999);
        Matrix<double, -1, -1, RowMajor> lbAmat(N_const, 1);
        lbAmat << MinusInf, MatrixXd(beq.template cast<double>());

        qpOASES::real_t *Hqp = Hmat.data();
        qpOASES::real_t *gqp = gmat.data();
        qpOASES::real_t *Aqp = Amat.data();
        qpOASES::real_t *lbAqp = lbAmat.data();
        qpOASES::real_t *ubAqp = ubAmat.data();

        qpOASES::Options options;
        options.printLevel = qpOASES::PL_LOW;
        options.terminationTolerance = 1e-10;
//        options.enableRamping=qpOASES::BT_FALSE;
        qp_obj.setOptions(options);
        qpOASES::int_t nWSR = 4000;
        qp_obj.init(Hqp, gqp, Aqp, NULL, NULL, lbAqp, ubAqp, nWSR);
        if(qp_obj.isInfeasible()){
            cout<<"[QP solver] warning: problem is infeasible. "<<endl;
        }

        cout << "Acutual nWSR: " << nWSR << " / Original: " << 2000 << endl;

        // qpOASES::real_t xOpt[nVar];
        // problem by VS, C2131
        qpOASES::real_t* xOpt = new qpOASES::real_t[nVar];
        qp_obj.getPrimalSolution(xOpt);
        isSolved = qp_obj.isSolved();
        if (isSolved)
            cout<<"[QP solver] Success!  "<<endl;
        else

            cout<<"[QP solver] Failure.  "<<endl;
        return Map<Matrix<double, -1, 1>>(xOpt, nVar, 1).cast<T>();
    }

    /////////////////////////////
    // TrajGen as a base class //
    /////////////////////////////
    template <typename T,size_t dim> TrajGen<T,dim>::TrajGen(time_knots<T> ts_): ts(ts_){
        // pinset per segment
        M = ts.size()-1; // # of segment = length(knots) - 1
        this->fixPinSet = new vector<FixPin<T,dim>>[M];
        this->loosePinSet = new vector<LoosePin<T,dim>>[M];
        fixPinOrderSet = new set<d_order>[M];
        printf ("Initialized %d segments. \n",ts_.size()-1);
    }

    /**
     * @brief Find a segment index (0,1,...,M-1) which contains t (ts[m]<= t < ts[m+1]  || t< ts[1] || ts[M-1]<= t)
     * @tparam dim
     * @param t query time
     * @param m segment index
     */
    template<typename T,size_t dim> void TrajGen<T,dim>::findSegInterval(T t,uint& m){
        auto it = upper_bound(this->ts.begin(),this->ts.end(),t);
        m = std::min(std::max( ((it - this->ts.begin())-1),long(0)),long(this->M-1)); // extrapolation allowed.
    }

    /**
     * Find a segment index (0,1,...,M-1) which contains t (ts[m]<= t < ts[m+1]  || t< ts[1] || ts[M-1]<= t)
     * @tparam dim
     * @param t query time
     * @param m segment index
     * @param tau Normalized time on the segment
     */
    template <typename T,size_t dim> void TrajGen<T,dim>::findSegInterval(T t, uint &m, T &tau) {
        findSegInterval(t,m);
        tau = (t - this->ts[m]) / (this->ts[m+1] - this->ts[m]);
        if (tau<0 || tau > 1)
            printf("tau is outside of [0,1]\n");
    }

    /**
     * Add pin struct to corresponding segment
     * @tparam T
     * @tparam dim
     * @param pin
     */
    template<typename T,size_t dim> void TrajGen<T,dim>::addPin(const Pin<T,dim> *pin){
        uint m;
        this->findSegInterval(pin->t,m); // which segment to add the pin
        if (pin->getType() == PIN_TYPE::FIX_PIN)
            fixPinSet[m].push_back(*(static_cast<const FixPin<T,dim>*>(pin)));
        else
            loosePinSet[m].push_back(*(static_cast<const LoosePin<T,dim>*>(pin)));
    }
    /**
     * This method process the set of pins in one-shot way
     * @tparam T
     * @tparam dim
     * @param pinPtrSet
     */
    template<typename T,size_t dim> void TrajGen<T,dim>::addPinSet(const vector<Pin<T,dim>*>& pinPtrSet){
        for (auto it = pinPtrSet.begin() ; it < pinPtrSet.end() ; it++)
            addPin(*it);
    }
    /**
     * Set the weights of each component of derivative penalty
     * @tparam T
     * @tparam dim
     * @param weights
     */
    template<typename T,size_t dim> void TrajGen<T,dim>::setDerivativeObj(VectorX<T> weights){
        weight_mask = weights;
    }
    /**
     *     get total number of inequality condition.
     */
    template<typename T,size_t dim> uint TrajGen<T,dim>::getTotalNineq() {
        uint nineq = 0;
        for (uint m = 0 ; m < M ; m++)
            nineq+= 2*loosePinSet[m].size();
        return nineq;
    }
    /**
     * get total number of equality condition.
     * @tparam T
     * @tparam dim
     * @return
     */
    template<typename T,size_t dim> uint TrajGen<T,dim>::getTotalNeq() {
        uint neq = 0;
        for (uint m = 0 ; m < M ; m++)
            neq+= fixPinSet[m].size();
        return neq;
    }

    ///////////////////////////////////////////
    // PolyTrajGen for piecewise polynomials //
    ///////////////////////////////////////////

    template<typename T,size_t dim> PolyTrajGen<T,dim>::PolyTrajGen(time_knots<T> ts_,PolyParam param_)  : TrajGen<T,dim>(ts_), param(param_),N(param_.poly_order){

        seg_state_set = new PolyState[this->M];

        // polycoeff segment / dimension
        poly_coeff_set = new PolyCoeff<T>*[dim];
        for (int dd = 0;dd <dim ; dd++)
            poly_coeff_set[dd] = new PolyCoeff<T>[this->M];

        // set the default Nc (except the last segment )
        for (uint m = 0 ; m < (this->M - 1); m++)
            seg_state_set[m].Nc = param_.max_conti_order+1;
    }

    template<typename T,size_t dim> void PolyTrajGen<T,dim>::addPin(const Pin<T,dim>* pin){
        // ensure the pin is imposed on the knots of poly spline
        uint m;
        this->findSegInterval(pin->t,m);
        if (pin->getType() == PIN_TYPE ::FIX_PIN)
            assert(this->ts[m] == pin->t || this->ts[m+1] == pin->t && "For polyTrajGen, fix pins can be imposed only on the knots.");

        // add pin
        if (pin->getType() == PIN_TYPE::FIX_PIN)
            if (seg_state_set[m].Nf > N + 1)
                printf("Fix pin already full for %d th segment. Pin append is ignored. (Nf = %d / max DOF = %d)\n"
                        ,m,seg_state_set[m].Nf,N+1);
            else{
                // add pin
                TrajGen<T,dim>::addPin(pin);
                seg_state_set[m].Nf+= 1;
                if (seg_state_set[m].Nf +seg_state_set[m].Nc > N+1 ) // if this pin addition overflow the total # of dof
                {
                    seg_state_set[m].Nc -= 1;
                    printf ("Fix pin reduced the continuity order (seg : %d, Nf=%d / Nc=%d).\n",seg_state_set[m].Nf,seg_state_set[m].Nc);
                }
                this->fixPinOrderSet[m].insert(pin->d);
            }
        else
            TrajGen<T,dim>::addPin(pin);
    }

    /**
     * The nth order coefficient of time vector ([1 t t^2 ... t^N]) when differentiated d times.
     * @tparam dim
     * @param n : target order
     * @param d : order of derivative
     * @return
     */
    template <typename T,size_t dim> T PolyTrajGen<T,dim>::B(p_order n,d_order d){
        if (d==0)
            return 1;
        else
            if (n<d)
                return 0;
            else{
                VectorXi prodTarget = VectorXi::LinSpaced(d,n-(d-1),n);
                return accumulate(prodTarget.data(),prodTarget.data()+prodTarget.size(),
                        1,std::multiplies<int>());
            }
    }
    /**
     * time vector ([1 t t^2 ... t^N]) when differentiated d times.
     * @tparam dim
     * @param t
     * @param d
     * @return
     */
    template <typename T,size_t dim> VectorX<T> PolyTrajGen<T,dim>::tVec(T t,d_order d){
        VectorX<T> vec(this->N+1); vec.setZero();
        for (int n = d ; n < N+1 ; n ++)
            vec(n) = B(n,d)*pow(t,n-d);
        return vec;
    }
    /**
     * p = scaleMat(dt) * phat
     * @tparam dim
     * @param dt segment duration (dTm)
     * @return
     */
    template <typename T,size_t dim> spMatrixRow<T> PolyTrajGen<T,dim>::scaleMat(T dt){
        spMatrixRow<T> mat(N+1,N+1);

        vector<Triplet<T>> tripletList;
        for (int i = 0 ; i < N+1 ; i++)
            tripletList.emplace_back(i,i,pow(dt,i));
        mat.setFromTriplets(tripletList.begin(),tripletList.end());
        return mat;
    }
    /**
     * diag([scaleMat(dTm)]_{m=[0,1,..,M-1]}). P = scaleMatBig * Phat
     * @tparam T
     * @tparam dim
     * @return
     */
    template <typename T,size_t dim> spMatrixRow<T> PolyTrajGen<T,dim>::scaleMatBig() {
        spMatrixRow<T> mat(this->M*(N+1),this->M*(N+1));
        vector<Triplet<T>> tripletList;
        for (int m = 0 ; m < this->M ; m++)
            for (int i = 0 ; i < N+1 ; i++)
                tripletList.emplace_back( (N+1)*m + i  ,(N+1)*m + i, pow(this->ts[m+1] - this->ts[m],i));
        mat.setFromTriplets(tripletList.begin(),tripletList.end());
        return mat;
    }
    /**
     * Inverse of scaleMatBig.
     * @tparam T
     * @tparam dim
     * @return
     */
    template <typename T,size_t dim> spMatrixRow<T> PolyTrajGen<T,dim>::scaleMatBigInv() {
        spMatrixRow<T> mat(this->M*(N+1),this->M*(N+1));
        vector<Triplet<T>> tripletList;
        for (int m = 0 ; m < this->M ; m++)
            for (int i = 0 ; i < N+1 ; i++)
                tripletList.emplace_back( (N+1)*m + i  ,(N+1)*m + i, pow(this->ts[m+1] - this->ts[m],-i));
        mat.setFromTriplets(tripletList.begin(),tripletList.end());
        return mat;
    }

    /**
     * {x^(d)(t)}^2  = (tVec(t,d)'*Dp)'*(tVec(t,d)'*Dp)
     * @tparam dim
     * @param d
     * @return
     */
    template <typename T,size_t dim> spMatrixRow<T> PolyTrajGen<T,dim>::IntDerSquard(trajgen::d_order d) {
        spMatrixRow<T> Q(N+1,N+1);
        if (d > N) {
            printf("Order of derivative exceeds the polynomial order. Returning zero mat.\n");
        }else{
            for (uint i = d;i<N+1 ; i++)
                for(uint j = d ; j < N+1 ; j ++)
                    Q.coeffRef(i,j) = B(i,d) * B(j,d) / (i+j-2*d+1);
        }
        return Q;
    }
    /**
     * Convert the stacked pin information to the constraints Aeq*p = beq. We can consider multiple pin at once
     * @tparam dim
     * @param pPin pointer to the first element of the array pin
     * @param blockSize how many pins to be considered
     * @return
     */
    template <typename T,size_t dim> ConstraintMatPair<T,dim> PolyTrajGen<T,dim>::fixPinMatSet(const FixPin<T,dim> *pPin,uint blockSize){
        uint nVar = this->M * (N+1);
        uint Nc = blockSize; // only one
        ConstraintMatPair<T,dim> abEq (Nc,nVar);

        for (uint blk = 0; blk < blockSize ; blk++) {
            uint m; T tau;
            this->findSegInterval((pPin+blk)->t, m, tau);
            uint idxStart = m * (N + 1);
            T dt = this->ts[m + 1] - this->ts[m];
            d_order d = (pPin+blk)->d;
            for (uint dd = 0; dd < dim; dd++) {
                sparseBlockCopy<T>(&(abEq.ASet[dd]),tVec(tau,d).transpose()/pow(dt,d),blk,idxStart);
                abEq.bSet[dd](blk) = (pPin + blk)->x(dd);
            }
        }
        return abEq;
    }

    /**
     * Convert the stacked pin information to the constraints A*p < b. We can consider multiple pin at once
     * @tparam T
     * @tparam dim
     * @param pPin
     * @param blockSize how many pins to be considered? total constraints = 2*pin
     * @return
     */
    template <typename T,size_t dim> ConstraintMatPair<T,dim> PolyTrajGen<T,dim>::loosePinMatSet(const LoosePin<T,dim> *pPin,uint blockSize){
        uint nVar = this->M * (N+1);
        uint Nc = 2*blockSize;
        ConstraintMatPair<T,dim> abInEq (Nc,nVar);
       for (uint blk = 0 ; blk < blockSize ; blk ++) {
           uint m; T tau;
           this->findSegInterval((pPin+blk)->t, m, tau);
           uint idxStart = m * (N + 1);
           T dt = this->ts[m + 1] - this->ts[m];
           d_order d = (pPin+blk)->d;
           for (uint dd = 0; dd < dim; dd++) {

               sparseBlockCopy<T>(&(abInEq.ASet[dd]),tVec(tau,d).transpose()/pow(dt,d),2*blk,idxStart);
               sparseBlockCopy<T>(&(abInEq.ASet[dd]),-tVec(tau,d).transpose()/pow(dt,d),2*blk+1,idxStart);

               abInEq.bSet[dd](2*blk) = (pPin+blk)->xu(dd);
               abInEq.bSet[dd](2*blk+1) = -(pPin+blk)->xl(dd);
           }
       }
        return abInEq;
    }

    /**
     * Equality constraint to connect m segment to m+1 segment (m = 0... M-1)
     * @tparam dim
     * @param m
     * @param dmax : we impose  dmax continuity (0,1,...,dmax). This is given from outside of this method.
     * @return
     */
    template <typename T,size_t dim> ConstraintMatPair<T,dim> PolyTrajGen<T,dim>::contiMat(uint m, trajgen::d_order dmax) {

        uint nVar = this->M *(N+1);

        if (m == this->M-1)
            return ConstraintMatPair<T,dim>(0,nVar);

        uint Nc = dmax + 1;
        ConstraintMatPair<T,dim> abEq(Nc,nVar);
        uint idxStart = m*(N+1);
        T dt1 = this->ts[m+1] - this->ts[m];
        T dt2 = this->ts[m+2] - this->ts[m+1];

        for (uint dd = 0; dd < dim ; dd++)
            for (d_order d = 0; d <= dmax; d++) {
                sparseBlockCopy<T>(&(abEq.ASet[dd]),tVec(1, d).transpose() / pow(dt1, d),d,idxStart);
                sparseBlockCopy<T>(&(abEq.ASet[dd]),-tVec(0, d).transpose() / pow(dt2, d),d,idxStart + (N+1));

                abEq.bSet[dd](d) = 0;
            }

        return abEq;
    }

    /**
     * Get the total number of equality constraint (sum (Nc+Nf) for m = 0,,,M-1)
     * @tparam dim
     * @return
     */
    template<typename T,size_t dim> uint PolyTrajGen<T,dim>::getTotalNeq() {
        auto lambda = [&] (uint s1,PolyState s2){return s1+s2.getN();};
        return accumulate(seg_state_set,seg_state_set+this->M,0,lambda);
    };

    /**
     * construct qp problem
     * @tparam dim
     * @return
     */
    template<typename T,size_t dim> QpForm<T,dim> PolyTrajGen<T,dim>::getQPSet() {

        uint nVar = (this->M)*(N+1);
        uint nineq = this->getTotalNineq(); // total line of inequality
        uint neq = this->getTotalNeq();
        QpForm<T,dim> qpForm(nVar,neq,nineq); // initialize all the container

        // 1. Objective function
        spMatrixRow<T> Q(nVar,nVar);
        for (uint d = 1; d <= this->weight_mask.size(); d++) { // dth order derivative
            spMatrixRow<T> Qd(nVar, nVar);
            for (uint m = 0; m < this->M; m++) { // per segment, plug in the Q
                T dt = this->ts[m + 1] - this->ts[m];
                sparseBlockCopy<T>(&Qd,MatrixRow<T>(IntDerSquard(d)/pow(dt,2*d-1)),m*(N+1),m*(N+1));
            }
            Q += this->weight_mask[d - 1] * Qd;
        }

        for (uint dd = 0 ; dd < dim ; dd++)
            qpForm.qpBlock[dd].Q = Q;

        uint Idx1 = 0,Idx2 = 0;
        for (uint m = 0 ; m < this->M ; m++){ // per segment
            // 2. Ineq constraint for this segment
            uint Nineq = this->loosePinSet[m].size();
            ConstraintMatPair<T,dim> Ab = loosePinMatSet((this->loosePinSet[m]).data(),this->loosePinSet[m].size());
            for (uint dd = 0 ; dd < dim ; dd++) { // per dimension
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].A),(Ab.ASet[dd]),Idx1,0);
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].b),(Ab.bSet[dd]),Idx1,0);
            }
            Idx1+= 2*Nineq;

            // 3. Eq constraint for this segment
            ConstraintMatPair<T,dim> AbFix = fixPinMatSet((this->fixPinSet[m]).data(),this->fixPinSet[m].size());
            ConstraintMatPair<T,dim> AbConti = contiMat(m,seg_state_set[m].Nc-1);
            uint Neq = seg_state_set[m].getN(), Nf = seg_state_set[m].Nf, Nc = seg_state_set[m].Nc;
            for (uint dd = 0 ; dd < dim ; dd++) { // inserting A,b
                // (Aeq)m = ([Afix ; Aconti])m : Aeq*p = beq
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].Aeq),(AbFix.ASet[dd]),Idx2,0) ;
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].Aeq),(AbConti.ASet[dd]),Idx2+Nf,0);
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].beq),(AbFix.bSet[dd]),Idx2,0) ;
                sparseBlockCopy<T>(&(qpForm.qpBlock[dd].beq),(AbConti.bSet[dd]),Idx2+Nf,0);

            }
            Idx2 += Neq;
        }
        return qpForm;
    }

    /**
     * Solving routine using qpOASES
     * @tparam dim
     * @return
     */
    template <typename T,size_t dim > bool PolyTrajGen<T,dim>::solve(bool isPrint){

        if (TrajGen<T,dim>::getTotalNeq() == 0 and this->getTotalNineq() == 0) {
            cout << "No constraints exist. Aborting. " << endl;
            return false;
        }

        // Prepare QP
        QpForm<T,dim> qpForm = getQPSet();
        QpForm<T,dim> qpFormOrig = qpForm;
        // If this method is end-derivative, we should map to end-derivative
        if (param.algo == ALGORITHM::END_DERIVATIVE)
            qpForm = mapQP(qpForm);

        qpForm.write();

        // Solve
        bool isSolved; VectorX<T> P;
        vector<VectorX<T>> solSet = qpForm.getQpSolSet(isSolved);
        if (not isSolved){
            this->isSolved = isSolved;
            cout << "Trajectory generation failed. Aborting" << endl;
            return false;
        }
        ofstream file ("QP",std::ofstream::app);
        for (uint dd = 0 ; dd < dim ; dd++) {
            file << "sol[" << dd << "]:\n" << (solSet[dd]) << endl;
            if (param.algo == ALGORITHM::END_DERIVATIVE){
                VectorX<T> df = qpFormOrig.qpBlock[dd].beq;
                VectorX<T> Phat((N+1)*this->M);
                MatrixRow<T> Afp = coeff2endDerivatives(qpFormOrig.qpBlock[0].Aeq);
                Phat << df, solSet[dd]; Phat = Afp.inverse()*Phat;
                P = scaleMatBigInv() * Phat; // polynomial (collection)
            }
            else
                P = scaleMatBigInv() * (solSet[dd]); // polynomial (collection)
            // reshaping
            for (uint m = 0; m < this->M ; m++) {
                poly_coeff_set[dd][m] =  P.segment(m * (N + 1), N + 1);
                if (isPrint)
                    cout << "Poly-coeff of dim: " << dd << " / segment: " << m  << endl << poly_coeff_set[dd][m] <<endl;
            }
        }
        this->isSolved = isSolved;
        return isSolved;
    }
    /**
     * Evalute the value of the dth dertivative trajectory at time t. If sill no solution, returing empty vector.
     * @tparam dim
     * @param t_eval evaluation time
     * @param d order of derivative
     * @return
     */
    template <typename T,size_t dim> Vector<T,dim> PolyTrajGen<T,dim>::eval(T t_eval, d_order d) {
        if (not this->isSolved){
            cout << "Evaluation request failed. Still no valid solution." << endl;
            return Vector<T,dim>();
        }
        if (t_eval < this->ts[0] or t_eval > this->ts[this->M] )
            cout << "Trajectory evaluation time out of bound. Extrapolating.. " << endl;
        uint m ;
        this->findSegInterval(t_eval,m);
        T dtm = this->ts[m+1] - this->ts[m];
        Vector<T,dim> val;
        for (uint dd = 0 ; dd < dim ; dd++)
            val(dd) = tVec(t_eval-this->ts[m],d).transpose()*poly_coeff_set[dd][m];
        return val;
    }

    /**
     * This function is permutating Aeq*p = beq  -> [Aeq ; Ap]*p (Afp * p) = [beq ; bp],
     * where Ap*p = bp is a mapping from poly-coeff to the free end-derivatives.
     * For the convenience, we consider the free end-derivative at the initial end point.
     * @tparam dim
     * @param Aeq only for one dimension (every dims are same)
     * @return [Aeq ; Ap]
     */
    template<typename T,size_t dim> MatrixRow<T> PolyTrajGen<T,dim>::coeff2endDerivatives(const trajgen::spMatrixRow<T> &Aeq) {
        uint nVar = (this->M)*(N+1); uint Nf = Aeq.rows(),Np = nVar - Nf;
        MatrixRow<T> Afp(nVar,nVar); Afp.setZero(); // collection of [Af ; Ap]
        Afp.block(0,0,Nf,nVar) = Aeq;

        vector<FixPin<T,dim>> virtualPinSet; // to generate Ap set (map to free end-derivatives)
        for (uint m = 0 ; m < this->M ; m++){
            // calculate the available dof
            VectorXi totalOrderSet(N+1); totalOrderSet.setLinSpaced(N+1,0,N);
            set<d_order> totalOrderStdSet(totalOrderSet.data(),totalOrderSet.data()+(N+1));
            vector<d_order> availbleOrder = setDiff<d_order>(totalOrderStdSet,this->fixPinOrderSet[m]);
            uint usedDOF = seg_state_set[m].getN();
            availbleOrder.resize(N+1 - usedDOF);
            for (auto freeOrder : availbleOrder )
                virtualPinSet.push_back(FixPin<T,dim>(this->ts[m],freeOrder,Vector<T,dim>()));
        }
        ConstraintMatPair<T,dim> Abp = fixPinMatSet(virtualPinSet.data(),virtualPinSet.size());
        Afp.block(Nf,0,Np,nVar) = Abp.ASet[0];
        return Afp;
    }
    /**
     * This function converts original QP problem into the QP of free end-derivatives
     * @tparam dim
     * @param QpFormOrig
     * @return
     */
    template <typename T,size_t dim> QpForm<T,dim> PolyTrajGen<T,dim>::mapQP(const trajgen::QpForm<T,dim> &QpFormOrig) {
        uint nVar = (this->M)*(N+1);
        uint Nf = QpFormOrig.qpBlock[0].Aeq.rows() , Np = (N+1)*this->M - Nf;
        uint Nineq = QpFormOrig.nineq;

        MatrixRow<T> Afp = coeff2endDerivatives(QpFormOrig.qpBlock[0].Aeq);
        MatrixRow<T> AfpInv = Afp.inverse();

        // cout << AfpInv << endl;
        MatrixRow<T> Qtmp = AfpInv.transpose() * QpFormOrig.qpBlock[0].Q.template selfadjointView<Upper>() * AfpInv;

        // cout << Qtmp << endl;

        MatrixRow<T> Qff = Qtmp.block(0,0,Nf,Nf),
        Qfp = Qtmp.block(0,Nf,Nf,Np),
        Qpf = Qtmp.block(Nf,0,Np,Nf),
        Qpp = Qtmp.block(Nf,Nf,Np,Np);
        QpForm<T,dim> QpNew(Np,0,Nineq);
        for (uint dd = 0 ; dd < dim ; dd++){
            QpNew.qpBlock[dd].Q = Qpp.sparseView();
            MatrixRow <T> Htmp = (QpFormOrig.qpBlock[dd].beq.transpose()*(Qfp+Qpf.transpose()));
            QpNew.qpBlock[dd].H = Htmp.sparseView();
            MatrixRow <T> Atmp = (QpFormOrig.qpBlock[dd].A*AfpInv.block(0,Nf,Nf+Np,Np));
            QpNew.qpBlock[dd].A = Atmp.sparseView();
            MatrixRow<T> Af =  (QpFormOrig.qpBlock[dd].A*AfpInv).block(0,0,Nineq,Nf);
            spMatrixRow<T> AfSparse = Af.sparseView();
            QpNew.qpBlock[dd].b = QpFormOrig.qpBlock[dd].b - spMatrixRow<T>(AfSparse*QpFormOrig.qpBlock[dd].beq);
        }
        return QpNew;
    }

    ///////////////////////////////
    // Optimal trajectory method //
    ///////////////////////////////

    /**
     * This function finds the nearest time index in optimtraj, given a query time
     * @tparam T
     * @tparam dim
     * @param eval_t query time
     * @param d in case of evaluation of dth order derivative, we naturally lose the last d element.
     * @return
     */
    template <typename T, size_t dim> uint OptimTrajGen<T,dim>::matchIdx(T eval_t,d_order d) {
        time_knots <T> tPntDist = this->tPnts;
        for (T & dist : tPntDist)
            dist = abs(dist-eval_t);
        int min_pos = min_element(tPntDist.begin(),tPntDist.end()) - tPntDist.begin();
        return min(min_pos,int(this->tPnts.size()-d));
    }

    template <typename T,size_t dim> OptimTrajGen<T,dim>::OptimTrajGen(
            time_knots<T> ts_,float pntDensity): TrajGen<T, dim>(ts_),pntDensity(pntDensity){
        if (ts_.size() != 2) {
            cout << "[OptimTrajGen]: time was provided more than two. "
                    "We will use only the first and last value by reintializing the base class. "
                 << endl;
            ts_[1] = ts_[ts_.size()-1]; ts_.resize(2);
            this->ts = ts_;
            this-> M = 1;
            this->fixPinSet = new vector<FixPin<T,dim>>[this->M];
            this->loosePinSet = new vector<LoosePin<T,dim>>[this->M];
            this->fixPinOrderSet = new set<d_order>[this->M];
            printf ("Initialized %d segments. \n",ts_.size()-1);
        }

        nPnt = floor((this->ts[1] - this->ts[0])*pntDensity); // total number of points
        dt = (this->ts[1] - this->ts[0])/(nPnt-1); // time interval btw points
        VectorX<T> tPntVec(nPnt); tPntVec.setLinSpaced(nPnt,this->ts[0],this->ts[1]);
        tPnts = time_knots<T>(tPntVec.data(),tPntVec.data() + nPnt);

        trajPnts = new VectorX<T>[dim];
        // initialize the trajectory
        for (uint dd =0 ; dd < dim; dd++)
            trajPnts[dd] = VectorX<T>(nPnt);
    }

    template <typename T,size_t dim> void OptimTrajGen<T,dim>::addPin(const Pin<T, dim> *pin) {
        TrajGen<T,dim>::addPin(pin);
        // do I need else ?
    }
    /**
     * For given set of loosePin, we calculate (A,b) pair s.t A x <= b for the entire dimension
     * @tparam T
     * @tparam dim
     * @param pPin
     * @param blockSize
     * @return
     */
    template<typename T,size_t dim> ConstraintMatPair<T,dim> OptimTrajGen<T,dim>::loosePinMatSet(
            const LoosePin<T,dim> * pPin,uint blockSize ){
        uint Nc = 2*blockSize; // total number of constraint
        ConstraintMatPair<T,dim> abInEq(Nc,nPnt);
        for (uint blk = 0; blk < blockSize ; blk ++ ){
            d_order d = (pPin+blk)->d;
            T t = (pPin+blk)->t;
            uint n = matchIdx(t,d); // which point should be imposed for (blk) th point
            for (uint dd = 0 ; dd < dim; dd++){
                spMatrixRow<T> pickMat(2,nPnt - d);
                pickMat.coeffRef(0,n) = 1; // upper
                pickMat.coeffRef(1,n) = -1; // lower
                spMatrixRow<T> aineq(2,nPnt); aineq = pickMat*getDiff(d);
                sparseBlockCopy<T>(&(abInEq.ASet[dd]),MatrixRow<T>(aineq),2*blk,0);
                abInEq.bSet[dd](2*blk) = (pPin+blk)->xu(dd);
                abInEq.bSet[dd](2*blk+1) = -(pPin+blk)->xl(dd);
            }
        }
        return abInEq;
    };
    /**
     * For given set of loosePin, we calculate (A,b) pair s.t Aeq x = beq for the entire dimension
     * @tparam T
     * @tparam dim
     * @param pPin
     * @param blockSize
     * @return
     */
    template <typename T, size_t dim> ConstraintMatPair<T,dim> OptimTrajGen<T,dim>::fixPinMatSet(
            const trajgen::FixPin<T, dim> *pPin, uint blockSize) {
        uint nVar = nPnt;
        uint Nc = blockSize; // only one
        ConstraintMatPair<T,dim> abEq (Nc,nVar);

        for (uint blk = 0; blk < blockSize ; blk ++){
            d_order d = (pPin+blk)->d;
            T t = (pPin+blk)->t;
            uint n = matchIdx(t,d); // which point should be imposed for (blk) th point
            for (uint dd = 0 ; dd < dim; dd++){
                spMatrixRow<T> pickMat(1,nPnt - d);
                pickMat.coeffRef(0,n) = 1; // upper
                spMatrixRow<T> aeq(1,nPnt); aeq = pickMat*getDiff(d);
                sparseBlockCopy<T>(&(abEq.ASet[dd]),aeq,blk,0);
                abEq.bSet[dd](blk) = (pPin+blk)->x(dd);
            }
        }
        return abEq;
    }

    /**
     * return the matrix Dd s.t x^(d) = Dd x. The number of points will be (nVar-d)
     * @tparam T
     * @tparam dim
     * @param d
     * @return
     */
    template<typename T,size_t dim> spMatrixRow<T> OptimTrajGen<T,dim>::getDiff(trajgen::d_order d) {
        spMatrixRow<T> D0 = spMatrixRow<T>(nPnt,nPnt); D0.setIdentity();
        if (d == 0)
            return D0;
        for (uint j = 1 ; j <= d; j++) {
            spMatrixRow<T> D(nPnt - (j), nPnt - (j - 1));
            // filling this matrix
            for (uint i = 0; i < nPnt - j; i++) {
                D.coeffRef(i,i) = -1; D.coeffRef(i,i+1) = 1;
            }
            D /= dt; D0 = D*D0;
        }
        return D0;
    }
    /**
     * construct qp problem
     * @tparam dim
     * @return
     */
    template <typename T, size_t dim> QpForm<T,dim> OptimTrajGen<T,dim>::getQPSet() {
        uint nVar = nPnt;
        uint nineq = this->getTotalNineq(); // total line of inequality
        uint neq = this->getTotalNeq();
        QpForm<T,dim> qpForm(nVar,neq,nineq); // initialize all the container

        // 1. Objective function
        spMatrixRow <T> Q(nVar,nVar);
        for (uint d = 1; d <= this->weight_mask.size() ; d++){
            spMatrixRow<T> Qd = getDiff(d).transpose()*getDiff(d);
            Q += this->weight_mask[d - 1] * Qd;
        }
        // same for every dimension
        for (uint dd = 0 ; dd < dim ; dd++)
            qpForm.qpBlock[dd].Q = Q;
        // 2. Inequality & equality
        for (uint dd = 0; dd < dim ; dd++){
            ConstraintMatPair<T,dim> Ab = loosePinMatSet(this->loosePinSet[0].data(), // in case of optimTrajGen, seg = 1
                    this->getTotalNineq()/2); // divide by 2 as the argument is number of block
            qpForm.qpBlock[dd].A = Ab.ASet[dd];
            qpForm.qpBlock[dd].b = Ab.bSet[dd].sparseView();

            ConstraintMatPair<T,dim> Abeq = fixPinMatSet(this->fixPinSet[0].data(),
                                                         this->getTotalNeq()); // divide by 2 as the argument is number of block

            qpForm.qpBlock[dd].Aeq = Abeq.ASet[dd];
            qpForm.qpBlock[dd].beq = Abeq.bSet[dd].sparseView();

        }

        return qpForm;
    }

    /**
     * solve the quadratic programming
     * @tparam T
     * @tparam dim
     * @param isPrint
     * @return
     */
    template <typename T, size_t dim> bool OptimTrajGen<T,dim>::solve(bool isPrint) {
        if (this->getTotalNeq() == 0 and this->getTotalNineq() == 0) {
            cout << "No constraints exist. Aborting. " << endl;
            return false;
        }
        QpForm<T, dim> qpForm = getQPSet();
        qpForm.write();

        vector<VectorX<T>> solSet = qpForm.getQpSolSet(this->isSolved);
        if (not this->isSolved) {
            cout << "Trajectory generation failed. Aborting" << endl;
            return false;
        }
        for (uint dd = 0; dd < dim ; dd++)
            trajPnts[dd] = solSet[dd];
        return true;
    }

    /**
     * Evalute the value of the dth dertivative trajectory at time t. If sill no solution, returing empty vector.
     * @tparam dim
     * @param t_eval evaluation time
     * @param d order of derivative
     * @return
     */
     template <typename T, size_t dim > Vector<T,dim> OptimTrajGen<T,dim>::eval(T t_eval, d_order d) {
         if (not this->isSolved){
            cout << "Evaluation request failed. Still no valid solution." << endl;
            return Vector<T,dim>();
         }
        // warning  extra-polation turn on
        if (t_eval < this->ts[0] or t_eval > this->ts[this->M] )
            cout << "Trajectory evaluation time out of bound. Extrapolating.. " << endl;

        Vector<T,dim> val;
        VectorX<T> time_knots_vec = Map<Matrix<T,-1,1>>(tPnts.data(),tPnts.size(),1);
        for (uint dd = 0; dd < dim ; dd++) {
            val(dd) = interpolate<T>(time_knots_vec.segment(0,nPnt-d), getDiff(d)*trajPnts[dd], t_eval,true);
        }
        return val;
     }

} // namespace trajgen
# endif


/*******************************************************************************
 * Copyright (C) 2016 Advanced Micro Devices, Inc. All rights reserved.
 ******************************************************************************/

#include <cmath>
#include <cstddef>
#include <functional>
#include <iostream>
#include <numeric>
#include <sstream>

#include "./rider.h"
#include "rocfft.h"
#include <boost/program_options.hpp>

#include "./misc.h"

namespace po = boost::program_options;

// Increment the index (column-major) for looping over arbitrary dimensional loops with
// dimensions length.
template <class T1, class T2>
bool increment_colmajor(std::vector<T1>& index, const std::vector<T2>& length)
{
    for(int idim = 0; idim < length.size(); ++idim)
    {
        if(index[idim] < length[idim])
        {
            if(++index[idim] == length[idim])
            {
                index[idim] = 0;
                continue;
            }
            break;
        }
    }
    // End the loop when we get back to the start:
    return !std::all_of(index.begin(), index.end(), [](int i) { return i == 0; });
}

// Perform a transform using rocFFT.  We assume that all input is valid at this point.
// ntrial is the number of trials; if this is 0, then we just do a correctness check.
template <typename T>
int transform(const std::vector<size_t> length,
              const std::vector<size_t> istride,
              const std::vector<size_t> ostride,
              const size_t              idist,
              const size_t              odist,
              size_t                    nbatch,
              const std::vector<size_t> ioffset,
              const std::vector<size_t> ooffset,
              rocfft_array_type         itype,
              rocfft_array_type         otype,
              rocfft_result_placement   place,
              rocfft_precision          precision,
              rocfft_transform_type     transformType,
              double                    scale,
              int                       deviceId,
              const int                 ntrial)
{
    HIP_V_THROW(hipSetDevice(deviceId), " hipSetDevice failed");

    const unsigned dim = length.size();

    unsigned number_of_input_buffers = 0;
    switch(itype)
    {
    case rocfft_array_type_complex_planar:
    case rocfft_array_type_hermitian_planar:
        number_of_input_buffers = 2;
        break;
    default:
        number_of_input_buffers = 1;
    }

    unsigned number_of_output_buffers = 0;
    switch(otype)
    {
    case rocfft_array_type_complex_planar:
    case rocfft_array_type_hermitian_planar:
        number_of_output_buffers = 2;
        break;
    default:
        number_of_output_buffers = 1;
    }

    size_t isize = idist * nbatch;
    switch(itype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_hermitian_interleaved:
        isize *= sizeof(std::complex<T>);
        break;
    default:
        isize *= sizeof(T);
    }

    size_t osize = odist * nbatch;
    switch(otype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_hermitian_interleaved:
        osize *= sizeof(std::complex<T>);
        break;
    default:
        osize *= sizeof(T);
    }

    std::vector<void*> ibuffer;
    std::vector<void*> obuffer;

    for(unsigned i = 0; i < number_of_input_buffers; i++)
    {
        ibuffer.push_back(NULL);
        HIP_V_THROW(hipMalloc(&ibuffer[i], isize), "hipMalloc failed");
    }

    for(unsigned i = 0; i < number_of_output_buffers; i++)
    {
        obuffer.push_back(NULL);
        if(place != rocfft_placement_inplace)
            HIP_V_THROW(hipMalloc(&obuffer[i], osize), "hipMalloc failed");
    }

    // Fill the input buffers
    switch(itype)
    {
    case rocfft_array_type_complex_interleaved:
    {
        std::vector<std::complex<T>> input(idist * nbatch);

        // Input data is constant; output should be delta.
        std::fill(input.begin(), input.end(), 0.0);
        for(size_t b = 0; b < nbatch; b++)
        {
            std::vector<int> index(length.size());
            std::fill(index.begin(), index.end(), 0);
            do
            {
                int i = std::inner_product(index.begin(), index.end(), istride.begin(), b * idist);
                input[i] = 1.0;

            } while(increment_colmajor(index, length));
        }

        HIP_V_THROW(hipMemcpy(ibuffer[0], input.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
    }
    break;
    case rocfft_array_type_complex_planar:
    {
        std::vector<T> real(idist * nbatch);
        std::vector<T> imag(idist * nbatch);

        // Input data is constant; output should be delta.
        std::fill(real.begin(), real.end(), 0.0);
        std::fill(imag.begin(), imag.end(), 0.0);
        for(size_t b = 0; b < nbatch; b++)
        {
            std::vector<int> index(length.size());
            std::fill(index.begin(), index.end(), 0);
            do
            {
                int i = std::inner_product(index.begin(), index.end(), istride.begin(), b * idist);
                real[i] = 1.0;
                imag[i] = 0.0;

            } while(increment_colmajor(index, length));
        }

        HIP_V_THROW(hipMemcpy(ibuffer[0], real.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
        HIP_V_THROW(hipMemcpy(ibuffer[1], imag.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
    }
    break;
    case rocfft_array_type_hermitian_interleaved:
    {
        std::vector<std::complex<T>> input(idist * nbatch);

        // Input data is the delta function; output should be constant.
        T delta = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
        std::fill(input.begin(), input.end(), 0.0);
        for(size_t b = 0; b < nbatch; b++)
        {
            size_t p3 = b * idist;
            input[p3] = delta;
        }
        HIP_V_THROW(hipMemcpy(ibuffer[0], input.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
    }
    break;
    case rocfft_array_type_hermitian_planar:
    {
        std::vector<T> real(isize);
        std::vector<T> imag(isize);

        // Input data is the delta function; output should be constant.
        std::fill(real.begin(), real.end(), 0.0);
        std::fill(imag.begin(), imag.end(), 0.0);
        T delta = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
        for(size_t b = 0; b < nbatch; b++)
        {
            size_t p3 = b * idist;
            real[p3]  = delta;
        }
        HIP_V_THROW(hipMemcpy(ibuffer[0], real.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
        HIP_V_THROW(hipMemcpy(ibuffer[1], imag.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
    }
    break;
    case rocfft_array_type_real:
    {
        std::vector<T> input(idist * nbatch);

        // Input data is constant; output should be delta.
        std::fill(input.begin(), input.end(), 0.0);
        for(size_t b = 0; b < nbatch; b++)
        {
            std::vector<int> index(length.size());
            std::fill(index.begin(), index.end(), 0);
            do
            {
                const int i
                    = std::inner_product(index.begin(), index.end(), istride.begin(), b * idist);
                input[i] = 1.0;

            } while(increment_colmajor(index, length));
        }
        HIP_V_THROW(hipMemcpy(ibuffer[0], input.data(), isize, hipMemcpyHostToDevice),
                    "hipMemcpy failed");
    }
    break;
    default:
    {
        throw std::runtime_error("Input layout format not yet supported");
    }
    break;
    }

    LIB_V_THROW(rocfft_setup(), " rocfft_setup failed");

    rocfft_plan_description desc = NULL;
    LIB_V_THROW(rocfft_plan_description_create(&desc), "rocfft_plan_description_create failed");

    LIB_V_THROW(rocfft_plan_description_set_data_layout(desc,
                                                        itype,
                                                        otype,
                                                        ioffset.data(),
                                                        ooffset.data(),
                                                        istride.size(),
                                                        istride.data(),
                                                        idist,
                                                        ostride.size(),
                                                        ostride.data(),
                                                        odist),
                "rocfft_plan_description_data_layout failed");

    // NB: not yet implemented in rocFFT.
    // if(scale != 1.0)
    // {
    //     if(precision == rocfft_precision_single)
    //         LIB_V_THROW(
    //             rocfft_plan_description_set_scale_float( desc, (float)scale ),
    //             "rocfft_plan_description_set_scale_float failed" );
    //     else
    //         LIB_V_THROW(
    //             rocfft_plan_description_set_scale_double( desc, scale ),
    //             "rocfft_plan_description_set_scale_double failed" );
    // }

    rocfft_plan plan = NULL;
    LIB_V_THROW(
        rocfft_plan_create(
            &plan, place, transformType, precision, length.size(), length.data(), nbatch, desc),
        "rocfft_plan_create failed");

    LIB_V_THROW(rocfft_plan_get_print(plan), "rocfft_plan_get_print failed");

    // Get the buffersize
    size_t workBufferSize = 0;
    LIB_V_THROW(rocfft_plan_get_work_buffer_size(plan, &workBufferSize),
                "rocfft_plan_get_work_buffer_size failed");

    // Allocate the work buffer
    void* workBuffer = NULL;
    if(workBufferSize)
    {
        HIP_V_THROW(hipMalloc(&workBuffer, workBufferSize), "Creating intmediate Buffer failed");
    }

    void** BuffersOut = (place == rocfft_placement_inplace) ? NULL : obuffer.data();

    rocfft_execution_info info = NULL;
    LIB_V_THROW(rocfft_execution_info_create(&info), "rocfft_execution_info_create failed");
    if(workBuffer != NULL)
    {
        LIB_V_THROW(rocfft_execution_info_set_work_buffer(info, workBuffer, workBufferSize),
                    "rocfft_execution_info_set_work_buffer failed");
    }

    // Execute once for basic functional test
    LIB_V_THROW(rocfft_execute(plan,
                               ibuffer.data(),
                               (place == rocfft_placement_inplace) ? NULL : obuffer.data(),
                               info),
                "rocfft_execute failed");

    HIP_V_THROW(hipDeviceSynchronize(), "hipDeviceSynchronize failed");

    std::vector<float> gpu_time(ntrial);

    if(ntrial > 0)
    {
        hipEvent_t start, stop;
        HIP_V_THROW(hipEventCreate(&start), "hipEventCreate failed");
        HIP_V_THROW(hipEventCreate(&stop), "hipEventCreate failed");

        for(int itrial = 0; itrial < ntrial; ++itrial)
        {
            HIP_V_THROW(hipEventRecord(start), "hipEventRecord failed");

            LIB_V_THROW(rocfft_execute(plan, ibuffer.data(), BuffersOut, info),
                        "rocfft_execute failed");

            HIP_V_THROW(hipEventRecord(stop), "hipEventRecord failed");
            HIP_V_THROW(hipEventSynchronize(stop), "hipEventSynchronize failed");

            hipEventElapsedTime(&gpu_time[itrial], start, stop);
            gpu_time[itrial];
        }

        HIP_V_THROW(hipDeviceSynchronize(), "hipDeviceSynchronize failed");

        std::cout << "\nExecution gpu time:";
        for(const auto& i : gpu_time)
        {
            std::cout << " " << i;
        }
        std::cout << " ms" << std::endl;

        std::cout << "Execution gflops:  ";
        const double totsize
            = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
        const double k
            = ((itype == rocfft_array_type_real) || (otype == rocfft_array_type_real)) ? 2.5 : 5.0;
        const double opscount = (double)nbatch * k * totsize * log(totsize) / log(2.0);
        for(const auto& i : gpu_time)
        {
            std::cout << " " << opscount / (1e6 * i);
        }
        std::cout << std::endl;
    }

    if(workBuffer)
        HIP_V_THROW(hipFree(workBuffer), "hipFree failed");

    LIB_V_THROW(rocfft_plan_description_destroy(desc), "rocfft_plan_description_destroy failed");

    LIB_V_THROW(rocfft_execution_info_destroy(info), "rocfft_execution_info_destroy failed");

    bool   checkflag = false;
    double err_ratio = 1E-6;

    // Read and check output data
    // This check is not valid if the FFT is executed multiple times.
    if(ntrial == 0)
    {
        switch(otype)
        {
        case rocfft_array_type_hermitian_interleaved:
        case rocfft_array_type_complex_interleaved:
        {
            std::vector<std::complex<T>> output(odist * nbatch);

            HIP_V_THROW(hipMemcpy(output.data(),
                                  (place == rocfft_placement_inplace) ? ibuffer[0] : BuffersOut[0],
                                  osize,
                                  hipMemcpyDeviceToHost),
                        "hipMemcpy failed");

            // check output data: we should get a peak at the index, and zero otherwise.
            T delta = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
            for(size_t b = 0; b < nbatch; b++)
            {
                std::vector<int> index(length.size());
                std::fill(index.begin(), index.end(), 0);
                do
                {
                    int i = std::inner_product(
                        index.begin(), index.end(), ostride.begin(), b * odist);
                    if(i == b * odist)
                    {
                        // DC mode
                        if(fabs(output[i] - std::complex<T>(delta, 0)) / delta > err_ratio)
                        {
                            checkflag = true;
                            break;
                        }
                    }
                    else
                    {
                        if(fabs(output[i]) / delta > err_ratio)
                        {
                            checkflag = true;
                            break;
                        }
                    }
                } while(increment_colmajor(index, length));
            }
        }
        break;
        case rocfft_array_type_hermitian_planar:
        case rocfft_array_type_complex_planar:
        {
            std::vector<T> real(osize);
            HIP_V_THROW(hipMemcpy(real.data(),
                                  (place == rocfft_placement_inplace) ? ibuffer[0] : BuffersOut[0],
                                  osize,
                                  hipMemcpyDeviceToHost),
                        "hipMemcpy failed");

            std::vector<T> imag(osize);
            HIP_V_THROW(hipMemcpy(imag.data(),
                                  (place == rocfft_placement_inplace) ? ibuffer[1] : BuffersOut[1],
                                  osize,
                                  hipMemcpyDeviceToHost),
                        "hipMemcpy failed");

            T delta = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());
            for(size_t b = 0; b < nbatch; b++)
            {
                std::vector<int> index(length.size());
                std::fill(index.begin(), index.end(), 0);
                do
                {
                    int i = std::inner_product(
                        index.begin(), index.end(), ostride.begin(), b * odist);
                    if(i == b * odist)
                    {
                        // DC mode
                        if(fabs(std::complex<T>(real[i], imag[i]) - std::complex<T>(delta, 0))
                               / delta
                           > err_ratio)
                        {
                            checkflag = true;
                            break;
                        }
                    }
                    else
                    {
                        if(fabs(real[i]) / delta > err_ratio || fabs(imag[i]) / delta > err_ratio)
                        {
                            checkflag = true;
                            break;
                        }
                    }
                } while(increment_colmajor(index, length));
            }
        }
        break;
        case rocfft_array_type_real:
        {
            std::vector<T> output(odist * nbatch);

            HIP_V_THROW(hipMemcpy(output.data(),
                                  (place == rocfft_placement_inplace) ? ibuffer[0] : BuffersOut[0],
                                  osize,
                                  hipMemcpyDeviceToHost),
                        "hipMemcpy failed");

            // NB: we compare with delta, because the inverse transform isn't normalized.
            T delta = std::accumulate(length.begin(), length.end(), 1, std::multiplies<size_t>());

            std::vector<int> index(length.size());
            std::fill(index.begin(), index.end(), 0);
            for(size_t b = 0; b < nbatch; b++)
            {
                do
                {
                    const int i = std::inner_product(
                        index.begin(), index.end(), ostride.begin(), b * odist);
                    if(fabs(output[i] - delta) > err_ratio)
                    {
                        checkflag = true;
                        break;
                    }
                } while(increment_colmajor(index, length));
            }
        }
        break;
        default:
        {
            throw std::runtime_error("Input layout format not yet supported");
        }
        break;
        }

        if(checkflag)
        {
            std::cout << "\n\n\t\tRider Test *****FAIL*****" << std::endl;
        }
        else
        {
            std::cout << "\n\n\t\tRider Test *****PASS*****" << std::endl;
        }
    }

    LIB_V_THROW(rocfft_plan_destroy(plan), "rocfft_plan_destroy failed");
    LIB_V_THROW(rocfft_cleanup(), "rocfft_cleanup failed");

    for(auto& buf : ibuffer)
    {
        if(buf != NULL)
        {
            HIP_V_THROW(hipFree(buf), "hipFree failed");
            buf = NULL;
        }
    }
    for(auto& buf : obuffer)
    {
        if(buf != NULL)
        {
            HIP_V_THROW(hipFree(buf), "hipFree failed");
            buf = NULL;
        }
    }

    return checkflag ? -1 : 0;
}

// This is used with the program_options class so that the user can type an integer on the
// command line and we store into an enum varaible
template <class _Elem, class _Traits>
std::basic_istream<_Elem, _Traits>& operator>>(std::basic_istream<_Elem, _Traits>& stream,
                                               rocfft_array_type&                  atype)
{
    unsigned tmp;
    stream >> tmp;
    atype = rocfft_array_type(tmp);
    return stream;
}

// similarly for transform type
template <class _Elem, class _Traits>
std::basic_istream<_Elem, _Traits>& operator>>(std::basic_istream<_Elem, _Traits>& stream,
                                               rocfft_transform_type&              ttype)
{
    unsigned tmp;
    stream >> tmp;
    ttype = rocfft_transform_type(tmp);
    return stream;
}

// Given a length vector, set the rest of the strides.
// The optinal argument stride1 is useful for setting contiguous-ish data for in-place
// real/complex transforms.
template <class T>
std::vector<T> contiguous_stride(const std::vector<T>& length, const int stride1 = 0)
{
    std::vector<T> stride;
    stride.push_back(1);
    if(stride1 == 0)
    {
        for(auto i = 1; i < length.size(); ++i)
        {
            stride.push_back(stride[i - 1] * length[i - 1]);
        }
    }
    else
    {
        if(length.size() > 1)
        {
            stride.push_back(stride1);
        }
        for(auto i = 2; i < length.size(); ++i)
        {
            stride.push_back(stride[i - 1] * length[i - 1]);
        }
    }
    return stride;
}

int main(int argc, char* argv[])
{
    // This helps with mixing output of both wide and narrow characters to the screen
    std::ios::sync_with_stdio(false);

    // hip Device number for running tests:
    int deviceId;

    // Transform type parameters:
    rocfft_transform_type transformType;
    rocfft_array_type     itype;
    rocfft_array_type     otype;

    // Number of performance trial samples
    int ntrial;

    // Number of batches:
    size_t nbatch = 1;

    // Scale for transform
    double scale = 1.0;

    // Transform length:
    std::vector<size_t> length;

    // Transform input and output strides:
    std::vector<size_t> istride;
    std::vector<size_t> ostride;

    // Offset to start of buffer (or buffers, for planar format):
    std::vector<size_t> ioffset;
    std::vector<size_t> ooffset;

    // Input and output distances:
    size_t idist;
    size_t odist;

    // clang-format doesn't handle boost program options very well:
    // clang-format off

    // Declare the supported options.
    po::options_description desc("rocfft rider command line options");
    desc.add_options()("help,h", "produces this help message")
        ("version,v", "Print queryable version information from the rocfft library")
        ("device", po::value<int>(&deviceId)->default_value(0),
         "Select a specific device id")
        ("ntrial,N", po::value<int>(&ntrial)->default_value(1),
         "Trial size for the problem")
        ("notInPlace,o", "Not in-place FFT transform (default: in-place)")
        ("double", "Double precision transform (default: single)")
        ("transformType,t", po::value<rocfft_transform_type>(&transformType)
         ->default_value(rocfft_transform_type_complex_forward),
         "Type of transform:\n0) complex forward\n1) complex inverse\n2) real "
         "forward\n3) real inverse")
        ( "idist", po::value<size_t>(&idist)->default_value(0),
          "input distance between successive members when batch size > 1")
        ( "odist", po::value<size_t>(&odist)->default_value(0),
          "output distance between successive members when batch size > 1")
        ("scale", po::value<double>(&scale)->default_value(1.0), "Specify the scaling factor ")
        ( "batchSize,b", po::value<size_t>(&nbatch)->default_value(1),
          "If this value is greater than one, arrays will be used ")
        ( "itype", po::value<rocfft_array_type>(&itype)
          ->default_value(rocfft_array_type_complex_interleaved),
          "Array type of input data:\n0) interleaved\n1) planar\n2) real\n3) "
          "hermitian interleaved\n4) hermitian planar")
        ( "otype", po::value<rocfft_array_type>(&otype)
          ->default_value(rocfft_array_type_complex_interleaved),
          "Array type of output data:\n0) interleaved\n1) planar\n2) real\n3) "
          "hermitian interleaved\n4) hermitian planar")
        ("length",  po::value<std::vector<size_t>>(&length)->multitoken(), "Lengths.")
        ("istride", po::value<std::vector<size_t>>(&istride)->multitoken(), "Input strides.")
        ("ostride", po::value<std::vector<size_t>>(&ostride)->multitoken(), "Output strides.")
        ("ioffset", po::value<std::vector<size_t>>(&ioffset)->multitoken(), "Input offsets.")
        ("ooffset", po::value<std::vector<size_t>>(&ooffset)->multitoken(), "Output offsets.");

    // clang-format on

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 0;
    }

    if(vm.count("version"))
    {
        char v[256];
        rocfft_get_version_string(v, 256);
        std::cout << "version " << v << std::endl;
        return 0;
    }

    if(!vm.count("length"))
    {
        std::cout << "Please specify transform length!" << std::endl;
        std::cout << desc << std::endl;
        return 0;
    }

    const rocfft_result_placement place
        = vm.count("notInPlace") ? rocfft_placement_notinplace : rocfft_placement_inplace;
    const rocfft_precision precision
        = vm.count("double") ? rocfft_precision_double : rocfft_precision_single;

    if(vm.count("notInPlace"))
    {
        std::cout << "out-of-place\n";
    }
    else
    {
        std::cout << "in-place\n";
    }

    if(vm.count("ntrial"))
    {
        std::cout << "Running profile with " << ntrial << " samples\n";
    }

    if(vm.count("length"))
    {
        std::cout << "length:";
        for(auto& i : length)
            std::cout << " " << i;
        std::cout << "\n";
    }

    if(vm.count("istride"))
    {
        std::cout << "istride:";
        for(auto& i : istride)
            std::cout << " " << i;
        std::cout << "\n";
    }

    if(vm.count("ostride"))
    {
        std::cout << "ostride:";
        for(auto& i : ostride)
            std::cout << " " << i;
        std::cout << "\n";
    }

    if(vm.count("idist"))
    {
        std::cout << "idist: " << idist << "\n";
    }
    if(vm.count("odist"))
    {
        std::cout << "odist: " << odist << "\n";
    }

    if(vm.count("ioffset"))
    {
        std::cout << "ioffset:";
        for(auto& i : ioffset)
            std::cout << " " << i;
        std::cout << "\n";
    }
    if(vm.count("ooffset"))
    {
        std::cout << "ooffset:";
        for(auto& i : ooffset)
            std::cout << " " << i;
        std::cout << "\n";
    }

    std::cout << std::flush;

    switch(itype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_complex_planar:
    case rocfft_array_type_hermitian_interleaved:
    case rocfft_array_type_hermitian_planar:
    case rocfft_array_type_real:
        break;
    default:
        throw std::runtime_error("Invalid Input array type format");
    }

    switch(otype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_complex_planar:
    case rocfft_array_type_hermitian_interleaved:
    case rocfft_array_type_hermitian_planar:
    case rocfft_array_type_real:
        break;
    default:
        throw std::runtime_error("Invalid Input array type format");
    }

    // Check that format choices are supported
    if(transformType != rocfft_transform_type_real_forward
       && transformType != rocfft_transform_type_real_inverse)
    {
        if(place == rocfft_placement_inplace && itype != otype)
        {
            throw std::runtime_error(
                "In-place transforms must have identical input and output types");
        }
    }
    bool okformat = true;
    switch(itype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_complex_planar:
        okformat = (otype == rocfft_array_type_complex_interleaved
                    || otype == rocfft_array_type_complex_planar);
        break;
    case rocfft_array_type_hermitian_interleaved:
    case rocfft_array_type_hermitian_planar:
        okformat = otype == rocfft_array_type_real;
        break;
    case rocfft_array_type_real:
        okformat = (otype == rocfft_array_type_hermitian_interleaved
                    || otype == rocfft_array_type_hermitian_planar);
        break;
    default:
        throw std::runtime_error("Invalid Input array type format");
    }
    switch(otype)
    {
    case rocfft_array_type_complex_interleaved:
    case rocfft_array_type_complex_planar:
    case rocfft_array_type_hermitian_interleaved:
    case rocfft_array_type_hermitian_planar:
    case rocfft_array_type_real:
        break;
    default:
        okformat = false;
    }
    if(!okformat)
    {
        throw std::runtime_error("Invalid combination of Input/Output array type formats");
    }

    if(!istride.empty() && istride.size() != length.size())
    {
        throw std::runtime_error("Transform dimension doesn't match input stride length");
    }

    if(!ostride.empty() && ostride.size() != length.size())
    {
        throw std::runtime_error("Transform dimension doesn't match output stride length");
    }

    if((transformType == rocfft_transform_type_complex_forward)
       || (transformType == rocfft_transform_type_complex_inverse))
    {
        // Complex-to-complex transform

        // User-specified strides must match for in-place transforms:
        if(place == rocfft_placement_inplace && !istride.empty() && !ostride.empty()
           && istride != ostride)
        {
            throw std::runtime_error("In-place transforms require istride == ostride");
        }

        // If the user only specified istride, use that for ostride for in-place
        // transforms.
        if(place == rocfft_placement_inplace && !istride.empty() && ostride.empty())
        {
            ostride = istride;
        }

        // If the strides are empty, we use contiguous data.
        if(istride.empty())
        {
            istride = contiguous_stride(length);
        }
        if(ostride.empty())
        {
            ostride = contiguous_stride(length);
        }
    }
    else
    {
        // Real/complex transform
        const bool forward = itype == rocfft_array_type_real;
        const bool inplace = place == rocfft_placement_inplace;

        // Length of complex data
        auto clength = length;
        clength[0]   = length[0] / 2 + 1;

        if(inplace)
        {
            // Fastest index must be contiguous.
            if(!istride.empty() && istride[0] != 1)
            {
                throw std::runtime_error(
                    "In-place real/complex transforms require contiguous input data.");
            }
            if(!ostride.empty() && ostride[0] != 1)
            {
                throw std::runtime_error(
                    "In-place real/complex transforms require contiguous output data.");
            }
            if(!istride.empty() && !ostride.empty())
            {
                for(int i = 1; i < length.size(); ++i)
                {
                    if(forward && istride[i] != 2 * ostride[i])
                    {
                        throw std::runtime_error(
                            "In-place real-to-complex transforms strides are inconsistent.");
                    }
                    if(!forward && 2 * istride[i] != ostride[i])
                    {
                        throw std::runtime_error(
                            "In-place complex-to-real transforms strides are inconsistent.");
                    }
                }
            }
        }

        if(istride.empty())
        {
            if(forward)
            {
                // real data
                istride = contiguous_stride(length, inplace ? clength[0] * 2 : 0);
            }
            else
            {
                // complex data
                istride = contiguous_stride(clength);
            }
        }

        if(ostride.empty())
        {
            if(forward)
            {
                // complex data
                ostride = contiguous_stride(clength);
            }
            else
            {
                // real data
                ostride = contiguous_stride(length, inplace ? clength[0] * 2 : 0);
            }
        }
    }

    if(idist == 0)
    {

        if(transformType == rocfft_transform_type_real_inverse && length.size() == 1)
        {
            idist = length[0] / 2 + 1;
        }
        else
        {
            idist = length[length.size() - 1] * istride[istride.size() - 1];
        }

        // in-place 1D transforms need extra dist.
        if(transformType == rocfft_transform_type_real_forward && length.size() == 1
           && place == rocfft_placement_inplace)
        {
            idist += 2;
        }
    }

    if(odist == 0)
    {
        if(transformType == rocfft_transform_type_real_forward && length.size() == 1)
        {
            odist = length[0] / 2 + 1;
        }
        else
        {
            odist = length[length.size() - 1] * ostride[ostride.size() - 1];
        }

        // in-place 1D transforms need extra dist.
        if(transformType == rocfft_transform_type_real_inverse && length.size() == 1
           && place == rocfft_placement_inplace)
        {
            odist += 2;
        }
    }

    // Final validation:
    if(istride.size() != length.size())
    {
        throw std::runtime_error("Setup failed; inconsistent istride and length.");
    }
    if(ostride.size() != length.size())
    {
        throw std::runtime_error("Setup failed; inconsistent ostride and length.");
    }

    int tret = 0;
    try
    {
        if(precision == rocfft_precision_single)
        {
            tret = transform<float>(length,
                                    istride,
                                    ostride,
                                    idist,
                                    odist,
                                    nbatch,
                                    ioffset,
                                    ooffset,
                                    itype,
                                    otype,
                                    place,
                                    precision,
                                    transformType,
                                    scale,
                                    deviceId,
                                    ntrial);
        }
        else
        {
            tret = transform<double>(length,
                                     istride,
                                     ostride,
                                     idist,
                                     odist,
                                     nbatch,
                                     ioffset,
                                     ooffset,
                                     itype,
                                     otype,
                                     place,
                                     precision,
                                     transformType,
                                     scale,
                                     deviceId,
                                     ntrial);
        }
    }
    catch(std::exception& e)
    {
        std::cerr << "rocfft error condition reported:" << std::endl << e.what() << std::endl;
        return 1;
    }

    return tret;
}