#pragma once

#include <assert.h>
#include <iostream>

//! @brief Base class for all waypoint managers.
template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
class GenericPathSlicer
{
public:
    typedef Container<ElementType> ContainerType;
    GenericPathSlicer();

    const ContainerType &path  () const;

    // Waypoint editing.
    void            setPath     (const ContainerType &path);
    void            push_back   (const ElementType &wp);
    void            push_front  (const ElementType &wp);
    void            clear       ();
    void            insert      (int i, const ElementType &wp);
    uint32_t size();
    ElementType     &at         (unsigned int i);


    // Slicing.
    void slice      (ContainerType &slice);
    void next       (ContainerType &slice);
    void previous   (ContainerType &slice);
    void reset      (ContainerType &slice);


    // Slicing parameters.
    //! @brief Sets the overlap.
    //!
    //! @param overlap The number of overlapping vertices
    //! between on and the next slice.
    void setOverlap            (uint32_t overlap);
    //! @brief Sets the number of vertices per slice.
    //!
    //! @param N The number of vertices per slice.
    void setN   (std::uint32_t N);
    //! @brief Sets the start index.
    //!
    //! @param idxStart The start index.
    void setStartIndex         (int idxStart);

    //! @return Returns the overlap.
    uint32_t      overlap       ();
    //! @return Returns the number of vertices per slice N.
    uint32_t      N             ();
    //! @return Returns the start index.
    int           startIndex    ();
    //! @return Returns the end index.
    int           endIndex    ();
    //! @return Returns the start index of the next slice.
    int           nextIndex    ();


private:
    void _updateIdx();

    ContainerType _path;
    long          _idxStart;
    long          _idxEnd;
    long          _idxNext;
    long          _idxPrevious;
    uint32_t      _overlap;
    uint32_t      _N;
    bool          _idxValid;
    bool          _atEnd;


};


template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
GenericPathSlicer<ElementType, Container>::GenericPathSlicer():
    _idxValid(false)
  , _atEnd(false)
{}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
const Container<ElementType> &GenericPathSlicer<ElementType, Container>::path() const {
    return _path;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setPath(const ContainerType &waypoints) {
    _idxValid = false;
    _path = waypoints;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::push_back(const ElementType &wp) {
    _idxValid = false;
    _path.push_back(wp);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::push_front(const ElementType &wp) {
    _idxValid = false;
    _path.push_front(wp);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::clear(){
    _idxValid = false;
    _path.clear();
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::insert(int i, const ElementType &wp){
    _idxValid = false;
    _path.insert(i, wp);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
std::uint32_t GenericPathSlicer<ElementType, Container>::size()
{
    return std::uint32_t(_path.size());
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
ElementType &GenericPathSlicer<ElementType, Container>::at(unsigned int i)
{
    return _path.at(i);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setOverlap(std::uint32_t overlap)
{
    _idxValid = false;
    _overlap = overlap;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setN(uint32_t N)
{
    _idxValid = false;
    _N = N > 0 ? N : 1;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::setStartIndex(int idxStart)
{
    _idxValid = false;
    _idxStart = idxStart;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::overlap()
{
    return _overlap;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
uint32_t GenericPathSlicer<ElementType, Container>::N()
{
    return _N;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::startIndex()
{
    if (!_idxValid)
        _updateIdx();
    return _idxStart;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::endIndex()
{
    if (!_idxValid)
        _updateIdx();
    return _idxEnd;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
int GenericPathSlicer<ElementType, Container>::nextIndex()
{
    if (!_idxValid)
        _updateIdx();
    return _idxNext;
}


template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::_updateIdx()
{
    _idxValid = true;
    _atEnd    = false;
    std::uint64_t size = _path.size();
    if ( _idxStart >= size-1 ) {
        _idxStart   = size-1;
        _idxEnd     = _idxStart;
        _idxNext    = _idxStart;
        _atEnd      = true;
        return;
    }

    _idxStart < 0 ? 0 : _idxStart;

    _idxEnd = _idxStart + _N - 1;
    _idxEnd = _idxEnd < size ? _idxEnd : size-1;

    _idxNext = _idxEnd + 1 - _overlap;
    _idxNext = _idxNext < 0    ? 0        : _idxNext;
    _idxNext = _idxNext < size ? _idxNext : size-1;

    _idxPrevious = _idxStart - 1 + _overlap;
    _idxPrevious = _idxPrevious < 0    ? 0        : _idxPrevious;
    _idxPrevious = _idxPrevious < size ? _idxPrevious : size-1;
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::slice(Container<ElementType> &c){

    if ( !_idxValid)
        _updateIdx();

    (void)c;
    assert(false); //ToDo

    // extract waypoints

}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::next(Container<ElementType> &c){
    setStartIndex(_idxNext);
    slice(c);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::previous(Container<ElementType> &c){
    setStartIndex(_idxPrevious);
    slice(c);
}

template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
void GenericPathSlicer<ElementType, Container>::reset(Container<ElementType> &c){
    setStartIndex(0);
    slice(c);
}