/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#include "CubicSplineFactory.hpp"

#include <rw/core/Ptr.hpp>
#include <rw/math/EAA.hpp>
#include <rw/math/Q.hpp>
#include <rw/math/Quaternion.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/Vector2D.hpp>
#include <rw/math/Vector3D.hpp>
#include <rw/math/VectorND.hpp>
#include <rw/trajectory/CubicSplineInterpolator.hpp>
#include <rw/trajectory/SQUADInterpolator.hpp>

#include <Eigen/Sparse>
#include <math.h>

#if EIGEN_VERSION_AT_LEAST(3, 1, 0)
#include <Eigen/SparseCholesky>
#endif

using namespace rw::trajectory;

using namespace rw::math;
using namespace rw::core;

// ###########################################################################
// #                                Q Functions                              #
// ###########################################################################

InterpolatorTrajectory< Q >::Ptr CubicSplineFactory::makeNaturalSpline (QPath::Ptr qpath,
                                                                        double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < qpath->size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeNaturalSpline (*qpath, times);
}

InterpolatorTrajectory< rw::math::Q >::Ptr
CubicSplineFactory::makeNaturalSpline (TimedQPath::Ptr tqpath)
{
    QPath path;
    std::vector< double > times;
    for (const TimedQ& tq : *tqpath) {
        path.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }

    return makeNaturalSpline (path, times);
}

InterpolatorTrajectory< rw::math::Q >::Ptr
CubicSplineFactory::makeNaturalSpline (const QPath& qpath, const std::vector< double >& times)
{
    typedef float T;

    if (qpath.size () < 2)
        RW_THROW ("Path must be longer than 1!");
    if (qpath.size () != times.size ())
        RW_THROW ("Length of path and times need to be equal");

    size_t dim = (qpath)[0].size ();    // the number of dimensions of the points
    size_t N   = qpath.size () - 1;     // we have N+1 points, which yields N splines

    typedef Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector;
    // typedef Eigen::Matrix<T, Eigen::Dynamic, 1, 1> Matrix;

    Vector B (N + 1);    // make room for boundary conditions

    Vector Y (N + 1);    // the points that the spline should intersect

    Vector a (dim * (N + 1)), b (dim * N), c (dim * N), d (dim * N);

    Vector H (N);    // duration from point i to i+1
    for (size_t i = 0; i < N; i++) {
        // T timeI0 = (T)((*tqpath)[i]).getTime();
        // T timeI1 = (T)((*tqpath)[i+1]).getTime();
        // H[i] = timeI1-timeI0;
        H[i] = (float) (times[i + 1] - times[i]);
    }

#if EIGEN_VERSION_AT_LEAST(3, 1, 0)
    Eigen::SparseMatrix< T > A ((int) N + 1, (int) N + 1);
    // D[0] = 2*H[0];
    A.insert (0, 0) = 2 * H[0];
    A.insert (0, 1) = H[0];
    A.insert (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        // D[i] = 2*(H[i-1]+H[i]);
        int ei                = (int) i;
        A.insert (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A.insert (ei, ei + 1) = H[i];
        A.insert (ei + 1, ei) = H[i];
    }
    // D[N] = 2*H[N-1];
    A.insert ((int) N, (int) N) = 2 * H[N - 1];

    Eigen::SimplicialLLT< Eigen::SparseMatrix< T > > solver;
    solver.compute (A);

#else
    Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > A =
        Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic >::Zero ((int) N + 1, (int) N + 1);
    A (0, 0) = 2 * H[0];
    A (0, 1) = H[0];
    A (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        // D[i] = 2*(H[i-1]+H[i]);
        int ei         = (int) i;
        A (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A (ei, ei + 1) = H[i];
        A (ei + 1, ei) = H[i];
    }
    // D[N] = 2*H[N-1];
    A ((int) N, (int) N) = 2 * H[N - 1];
    Eigen::LLT< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > > solver;
    solver.compute (A);

#endif
    for (size_t j = 0; j < (size_t) dim; j++) {
        for (size_t i = 0; i < (size_t) N + 1; i++) {
            Y[i] = (T) (qpath[i])[j];
        }

        B[0] = (T) (3.0 * (Y[1] - Y[0]) / H[0]);
        for (size_t i = 1; i < N; i++) {
            B[i] = (T) (3.0 * ((Y[i + 1] - Y[i]) / H[i] - (Y[i] - Y[i - 1]) / H[i - 1]));
        }
        B[N] = (T) (-3.0 * (Y[N] - Y[N - 1]) / H[N - 1]);

        B = solver.solve (B);

        for (size_t i = 0; i < (size_t) N + 1; i++) {
            a[i * dim + j] = Y[i];
        }

        for (size_t i = 0; i < (size_t) N; i++) {
            c[j + i * dim] = B[i];
            b[j + i * dim] = (Y[i + 1] - Y[i]) / H[i] - H[i] * (B[i + 1] + 2 * B[i]) / (T) 3.0;
            d[j + i * dim] = (B[i + 1] - B[i]) / ((T) 3.0 * H[i]);    //   +B[i]+B[i+1];
        }
    }

    // ************** now create the actual trajectory from the calcualted parameters
    InterpolatorTrajectory< Q >::Ptr traj = ownedPtr (new InterpolatorTrajectory< Q > (times[0]));

    Q ba (dim), bb (dim), bc (dim), bd (dim);
    for (size_t i = 0; i < N; i++) {
        for (size_t j = 0; j < dim; j++) {
            ba[j] = a[j + i * dim];
            bb[j] = b[j + i * dim];
            bc[j] = c[j + i * dim];
            bd[j] = d[j + i * dim];
        }
        Interpolator< Q >* iptr = new CubicSplineInterpolator< Q > (ba, bb, bc, bd, H[i]);
        traj->add (ownedPtr (iptr));
    }

    return traj;
}

InterpolatorTrajectory< Q >::Ptr CubicSplineFactory::makeClampedSpline (QPath::Ptr qpath,
                                                                        const rw::math::Q& dqStart,
                                                                        const rw::math::Q& dqEnd,
                                                                        double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < qpath->size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeClampedSpline (*qpath, times, dqStart, dqEnd);
}

InterpolatorTrajectory< rw::math::Q >::Ptr
CubicSplineFactory::makeClampedSpline (TimedQPath::Ptr tqpath, const rw::math::Q& dqStart,
                                       const rw::math::Q& dqEnd)

{
    QPath path;
    std::vector< double > times;
    for (const TimedQ& tq : *tqpath) {
        path.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }

    return makeClampedSpline (path, times, dqStart, dqEnd);
}

// ###########################################################################
// #                             Template Functions                          #
// ###########################################################################

template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< T >& path, double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < path.size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeNaturalSpline (path, times);
}

// ### explicit template specifications
template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Vector3D<> >& path, double timeStep);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Transform3DVector<> >& path, double timeStep);

template InterpolatorTrajectory< Q >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Q >& path, double timeStep);

template InterpolatorTrajectory< Quaternion< double > >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Quaternion< double > >& path, double timeStep);

// ########### NEXT FUNCTION ###############

template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< T > >& path)
{
    Path< T > pathN;
    std::vector< double > times;
    for (const Timed< T >& tq : path) {
        pathN.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }

    return makeNaturalSpline (pathN, times);
}

// ### explicit template specifications
template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< Vector3D<> > >& path);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< Transform3DVector<> > >& path);

template InterpolatorTrajectory< Q >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< Q > >& path);

template InterpolatorTrajectory< Quaternion< double > >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< Quaternion< double > > >& path);

// ########### NEXT FUNCTION ###############

template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< T >& path, const std::vector< double >& times)
{
    if (path.size () < 2) {
        RW_THROW ("Path must be longer than 1!");
    }
    if (path.size () != times.size ()) {
        RW_THROW ("Length of path and times need to be equal");
    }

    size_t dim = path[0].size ();     // the number of dimensions of the points
    size_t N   = path.size () - 1;    // we have N+1 points, which yields N splines
    Eigen::VectorXd B (N + 1);        // make room for boundary conditions

    Eigen::VectorXd Y (N + 1);    // the points that the spline should intersect

    Eigen::VectorXd a (dim * (N + 1));
    Eigen::VectorXd b (dim * N);
    Eigen::VectorXd c (dim * N);
    Eigen::VectorXd d (dim * N);

    Eigen::VectorXd H (N);    // duration from point i to i+1
    for (size_t i = 0; i < N; i++) {
        H[i] = (double) (times[i + 1] - times[i]);
    }
#if EIGEN_VERSION_AT_LEAST(3, 1, 0)
    Eigen::SparseMatrix< double > A ((int) N + 1, (int) N + 1);
    A.insert (0, 0) = 2 * H[0];
    A.insert (0, 1) = H[0];
    A.insert (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        int ei                = (int) i;
        A.insert (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A.insert (ei, ei + 1) = H[i];
        A.insert (ei + 1, ei) = H[i];
    }
    // D[N] = 2*H[N-1];
    A.insert ((int) N, (int) N) = 2 * H[N - 1];

    Eigen::SimplicialLLT< Eigen::SparseMatrix< double > > solver;
    solver.compute (A);

#else
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > A =
        Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero ((int) N + 1, (int) N + 1);
    A (0, 0) = 2 * H[0];
    A (0, 1) = H[0];
    A (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        int ei         = (int) i;
        A (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A (ei, ei + 1) = H[i];
        A (ei + 1, ei) = H[i];
    }
    A ((int) N, (int) N) = 2 * H[N - 1];
    Eigen::LLT< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > > solver;
    solver.compute (A);
#endif
    for (size_t j = 0; j < (size_t) dim; j++) {
        for (size_t i = 0; i < (size_t) N + 1; i++) {
            Y[i] = (double) (path[i])[j];
        }

        B[0] = (double) (3.0 * (Y[1] - Y[0]) / H[0]);
        for (size_t i = 1; i < N; i++) {
            B[i] = (double) (3.0 * ((Y[i + 1] - Y[i]) / H[i] - (Y[i] - Y[i - 1]) / H[i - 1]));
        }
        B[N] = (double) (-3.0 * (Y[N] - Y[N - 1]) / H[N - 1]);

        B = solver.solve (B);

        for (size_t i = 0; i < (size_t) N + 1; i++) {
            a[i * dim + j] = Y[i];
        }

        for (size_t i = 0; i < (size_t) N; i++) {
            c[j + i * dim] = B[i];
            b[j + i * dim] = (Y[i + 1] - Y[i]) / H[i] - H[i] * (B[i + 1] + 2 * B[i]) / (double) 3.0;
            d[j + i * dim] = (B[i + 1] - B[i]) / ((double) 3.0 * H[i]);    //   +B[i]+B[i+1];
        }
    }
    // ************** now create the actual trajectory from the calcualted parameters
    typename InterpolatorTrajectory< T >::Ptr traj =
        ownedPtr (new InterpolatorTrajectory< T > (times[0]));

    T ba, bb, bc, bd;
    for (size_t i = 0; i < N; i++) {
        for (size_t j = 0; j < dim; j++) {
            ba[j] = a[j + i * dim];
            bb[j] = b[j + i * dim];
            bc[j] = c[j + i * dim];
            bd[j] = d[j + i * dim];
        }
        Interpolator< T >* iptr = new CubicSplineInterpolator< T > (ba, bb, bc, bd, H[i]);
        traj->add (ownedPtr (iptr));
    }
    return traj;
}

// ### explicit template specifications
template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Vector3D<> >& path,
                                       const std::vector< double >& times);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Transform3DVector<> >& path,
                                       const std::vector< double >& times);

template InterpolatorTrajectory< Quaternion<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Quaternion<> >& path,
                                       const std::vector< double >& times);

// ########### NEXT FUNCTION ###############
template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< T >& path, const T& dStart, const T& dEnd,
                                       double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < path.size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeClampedSpline (path, times, dStart, dEnd);
}
namespace rw { namespace trajectory {
template<>
typename InterpolatorTrajectory< Transform3D<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Transform3D<> >& path,
                                       const Transform3D<>& dStart, const Transform3D<>& dEnd,
                                       double timeStep)
{
    RW_THROW ("Clamped Cubic Spline not yet implemented for Transform3D");
    return NULL;
}
}}
template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Vector3D<> >& path, const Vector3D<>& dStart,
                                       const Vector3D<>& dEnd, double timeStep);

template InterpolatorTrajectory< Q >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Q >& path, const Q& dStart, const Q& dEnd,
                                       double timeStep);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Transform3DVector<> >& path,
                                       const Transform3DVector<>& dStart,
                                       const Transform3DVector<>& dEnd, double timeStep);

template InterpolatorTrajectory< Quaternion<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Quaternion<> >& path, const Quaternion<>& dStart,
                                       const Quaternion<>& dEnd, double timeStep);

// ########### NEXT FUNCTION ###############

template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Timed< T > >& tpath, const T& dStart,
                                       const T& dEnd)
{
    Path< T > path;
    std::vector< double > times;
    for (const Timed< T >& tq : tpath) {
        path.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }

    return makeClampedSpline (path, times, dStart, dEnd);
}

template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Timed< Vector3D<> > >& tpath,
                                       const Vector3D<>& dStart, const Vector3D<>& dEnd);

template InterpolatorTrajectory< Q >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Timed< Q > >& path, const Q& dStart,
                                       const Q& dEnd);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Timed< Transform3DVector<> > >& tpath,
                                       const Transform3DVector<>& dStart,
                                       const Transform3DVector<>& dEnd);

template InterpolatorTrajectory< Quaternion<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Timed< Quaternion<> > >& tpath,
                                       const Quaternion<>& dStart, const Quaternion<>& dEnd);

// ########### NEXT FUNCTION ###############
template< typename T >
typename InterpolatorTrajectory< T >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< T >& path, const std::vector< double >& times,
                                       const T& dStart, const T& dEnd)
{
    if (path.size () < 2)
        RW_THROW ("Path must be longer than 1!");

    if (path.size () != times.size ())
        RW_THROW ("Length of path and times need to match");

    size_t dim = path[0].size ();     // the number of dimensions of the points
    size_t N   = path.size () - 1;    // we have N+1 points, which yields N splines
    Eigen::VectorXd B (N + 1);        // make room for boundary conditions

    Eigen::VectorXd Y (N + 1);    // the points that the spline should intersect

    Eigen::VectorXd a (dim * (N + 1));
    Eigen::VectorXd b (dim * N);
    Eigen::VectorXd c (dim * N);
    Eigen::VectorXd d (dim * N);
    Eigen::VectorXd H (N);    // duration from point i to i+1

    for (size_t i = 0; i < N; i++) {
        H[i] = (float) (times[i + 1] - times[i]);
    }
#if EIGEN_VERSION_AT_LEAST(3, 1, 0)
    Eigen::SparseMatrix< double > A ((int) N + 1, (int) N + 1);

    A.insert (0, 0) = 2 * H[0];
    A.insert (0, 1) = H[0];
    A.insert (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        int ei                = (int) i;
        A.insert (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A.insert (ei + 1, ei) = H[i];
        A.insert (ei, ei + 1) = H[i];
    }
    A.insert ((int) N, (int) N) = 2 * H[N - 1];

    Eigen::SimplicialLLT< Eigen::SparseMatrix< double > > solver;
    solver.compute (A);

#else
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > A =
        Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero ((int) N + 1, (int) N + 1);

    A (0, 0) = 2 * H[0];
    A (0, 1) = H[0];
    A (1, 0) = H[0];
    for (size_t i = 1; i < N; i++) {
        int ei         = (int) i;
        A (ei, ei)     = 2 * (H[i - 1] + H[i]);
        A (ei + 1, ei) = H[i];
        A (ei, ei + 1) = H[i];
    }
    A ((int) N, (int) N) = 2 * H[N - 1];

    Eigen::LLT< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > > solver;
    solver.compute (A);

#endif

    for (size_t j = 0; j < (size_t) dim; j++) {
        for (size_t i = 0; i < (size_t) N + 1; i++) {
            Y[i] = (double) (path[i])[j];
        }

        B[0] = (double) (3.0 * (Y[1] - Y[0]) / H[0] - 3 * dStart[j]);
        for (size_t i = 1; i < (std::size_t) B.size () - 1; i++) {
            B[i] = (double) (3.0 * ((Y[i + 1] - Y[i]) / H[i] - (Y[i] - Y[i - 1]) / H[i - 1]));
        }
        B[N] = (double) (3.0 * dEnd[j] - 3.0 * (Y[N] - Y[N - 1]) / H[N - 1]);

        B = solver.solve (B);

        for (size_t i = 0; i < N + 1; i++) {
            a[i * dim + j] = Y[i];
        }

        for (size_t i = 0; i < (size_t) N; i++) {
            c[j + i * dim] = B[i];
            b[j + i * dim] =
                (double) ((Y[i + 1] - Y[i]) / H[i] - H[i] * (B[i + 1] + 2 * B[i]) / 3.0);
            d[j + i * dim] = (double) ((B[i + 1] - B[i]) / (3.0 * H[i]));    //   +B[i]+B[i+1];
        }
    }

    // ************** now create the actual trajectory from the calcualted parameters
    typename InterpolatorTrajectory< T >::Ptr traj =
        ownedPtr (new InterpolatorTrajectory< T > (times[0]));

    T ba=path[0], bb=path[0], bc=path[0], bd=path[0];
    for (size_t i = 0; i < N; i++) {

        for (size_t j = 0; j < dim; j++) {
            ba[j] = a[j + i * dim];
            bb[j] = b[j + i * dim];
            bc[j] = c[j + i * dim];
            bd[j] = d[j + i * dim];
        }
        Interpolator< T >* iptr = new CubicSplineInterpolator< T > (ba, bb, bc, bd, H[i]);
        traj->add (ownedPtr (iptr));
    }

    return traj;
}

template InterpolatorTrajectory< Vector3D<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Vector3D<> >& path,
                                       const std::vector< double >& times, const Vector3D<>& dStart,
                                       const Vector3D<>& dEnd);

template InterpolatorTrajectory< Q >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Q >& path,
                                       const std::vector< double >& times, const Q& dStart,
                                       const Q& dEnd);

template InterpolatorTrajectory< Transform3DVector<> >::Ptr CubicSplineFactory::makeClampedSpline (
    const Path< Transform3DVector<> >& path, const std::vector< double >& times,
    const Transform3DVector<>& dStart, const Transform3DVector<>& dEnd);

template InterpolatorTrajectory< Quaternion<> >::Ptr
CubicSplineFactory::makeClampedSpline (const Path< Quaternion<> >& path,
                                       const std::vector< double >& times,
                                       const Quaternion<>& dStart, const Quaternion<>& dEnd);

// ###########################################################################
// #                          Transform3D Functions                          #
// ###########################################################################

InterpolatorTrajectory< rw::math::Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Transform3D<> >& path, double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < path.size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeNaturalSpline (path, times);
}

InterpolatorTrajectory< rw::math::Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< Timed< Transform3D<> > >& path)
{
    Path< Transform3D<> > pathN;
    std::vector< double > times;
    for (const Timed< Transform3D<> >& tq : path) {
        pathN.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }

    return makeNaturalSpline (pathN, times);
}

InterpolatorTrajectory< rw::math::Transform3DVector<> >::Ptr
CubicSplineFactory::makeNaturalSpline (const Path< rw::math::Transform3D<> >& path,
                                       const std::vector< double >& times)
{
    Path< Transform3DVector<> > NPath;
    for (const Transform3D<>& trans : path) {
        NPath.push_back (Transform3DVector<> (trans));
    }
    return makeNaturalSpline (NPath, times);
}

// ###########################################################################
// #                             SQUAD Functions                             #
// ###########################################################################

InterpolatorTrajectory< rw::math::Quaternion<> >::Ptr
CubicSplineFactory::makeSQUAD (const Path< rw::math::Quaternion<> >& path, double timeStep)
{
    std::vector< double > times;
    for (size_t i = 0; i < path.size (); i++) {
        times.push_back (i * timeStep);
    }
    return makeSQUAD (path, times);
}
InterpolatorTrajectory< rw::math::Quaternion<> >::Ptr
CubicSplineFactory::makeSQUAD (const Path< Timed< rw::math::Quaternion<> > >& tpath)
{
    Path< rw::math::Quaternion<> > pathN;
    std::vector< double > times;
    for (const Timed< rw::math::Quaternion<> >& tq : tpath) {
        pathN.push_back (tq.getValue ());
        times.push_back (tq.getTime ());
    }
    return makeSQUAD (pathN, times);
}

InterpolatorTrajectory< rw::math::Quaternion<> >::Ptr
CubicSplineFactory::makeSQUAD (const Path< rw::math::Quaternion<> >& path,
                               const std::vector< double >& times)
{
    InterpolatorTrajectory< Quaternion<> >::Ptr traj =
        ownedPtr (new InterpolatorTrajectory< Quaternion<> > (times[0]));
    size_t N = path.size () - 1;
    std::vector< Quaternion<> > s (path.size ());
    s[0] = path[0];
    for (size_t i = 1; i < s.size () - 1; i++) {
        const Quaternion<>& qn1 = path[i - 1];
        const Quaternion<>& q0  = path[i];
        const Quaternion<>& q1  = path[i + 1];

        s[i] = q0 * exp (-1 * (ln (q0.inverse () * q1) + ln (q0.inverse () * qn1)) * (1.0 / 4.0));
    }
    s.back () = path.back ();

    for (size_t i = 0; i < N - 1; i++) {
        double duration = times[i + 1] - times[i];
        Interpolator< Quaternion<> >* iptr =
            new SQUADInterpolator< double > (path[i], path[i + 1], s[i], s[i + 1], duration);
        traj->add (ownedPtr (iptr));
    }
    return traj;
}


#ifndef QST_RBM_HPP
#define QST_RBM_HPP
#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <fstream>

namespace qst{

// RBM class
class Rbm{

    int nv_;                        // Number of visible units
    int nh_;                        // Number of hidden units
    int npar_;                      // Number of parameters
    int nchains_;                   // Number of sampling chains

    Eigen::MatrixXd v_;             // Visible states
    Eigen::MatrixXd h_;             // Hidden states
    Eigen::MatrixXd probv_given_h_; // Visible probabilities
    Eigen::MatrixXd probh_given_v_; // Hidden probabilities

    Eigen::MatrixXd W_;             // Weights
    Eigen::VectorXd b_;             // Visible fields
    Eigen::VectorXd c_;             // Hidden fields

    Eigen::VectorXd gamma_;         // Container for hidden contribution to quantities

    std::mt19937 rgen_;             // Random number generator

public:
    // Contructor
    Rbm(Parameters &par):nv_(par.nv_),nh_(par.nh_){
        npar_=nv_+nh_+nv_*nh_;
        nchains_ = par.nc_;
        v_.setZero(nchains_,nv_);
        h_.setZero(nchains_,nh_);
        probv_given_h_.resize(nchains_,nv_);
        probh_given_v_.resize(nchains_,nh_);
        W_.resize(nh_,nv_);
        b_.resize(nv_);
        c_.resize(nh_);
        gamma_.resize(nh_);
        rgen_.seed(13579);
        //std::random_device rd;
        //rgen_.seed(rd());
    }

    // Private members access functions
    inline int Nvisible()const{
        return nv_;
    }
    inline int Nhidden()const{
        return nh_;
    }
    inline int Npar()const{
        return npar_;
    }
    inline int Nchains(){
        return nchains_;
    }
    inline Eigen::VectorXd VisibleStateRow(int s){
        return v_.row(s);
    }

    // Set the visible layer state
    inline void SetVisibleLayer(Eigen::MatrixXd v){
        v_=v;
    }

    // Initialize the network parameters
    void InitRandomPars(int seed,double sigma){
        std::default_random_engine generator(seed);
        std::normal_distribution<double> distribution(0,sigma);
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                W_(i,j)=distribution(generator);
            }
        }
        //b_.setZero();
        //c_.setZero();
        for(int j=0;j<nv_;j++){
            b_(j)=distribution(generator);
        }
        for(int i=0;i<nh_;i++){
            c_(i)=distribution(generator);
        }
    }

    // Compute derivative of the effective visible energy
    Eigen::VectorXd VisEnergyGrad(const Eigen::VectorXd & v){
        Eigen::VectorXd der(npar_);
        int p=0;
        logistic(W_*v+c_,gamma_);
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                der(p)=gamma_(i)*v(j);
                p++;
            }
        }
        for(int j=0;j<nv_;j++){
            der(p)=v(j);
            p++;
        }
        for(int i=0;i<nh_;i++){
            der(p)=gamma_(i);
            p++;
        }
        return -der;
    }

    // Return the probability for state v
    inline double prob(const Eigen::VectorXd & v){
        ln1pexp(W_*v+c_,gamma_);
        return std::exp(v.dot(b_)+gamma_.sum());
    }

    // Conditional Probabilities
    void ProbHiddenGivenVisible(const Eigen::MatrixXd &v,Eigen::MatrixXd &probs){
        logistic((v*W_.transpose()).rowwise() + c_.transpose(),probs);
    }
    void ProbVisibleGivenHidden(const Eigen::MatrixXd &h,Eigen::MatrixXd &probs){
        logistic((h*W_).rowwise() + b_.transpose(),probs);
    }

    // Sample one layer
    void SampleLayer(Eigen::MatrixXd & hv,const Eigen::MatrixXd & probs){
        std::uniform_real_distribution<double> distribution(0,1);
        for(int s=0;s<hv.rows();s++){
            for(int i=0;i<hv.cols();i++){
                hv(s,i)=distribution(rgen_)<probs(s,i);
            }
        }
    }

    // Perform k steps of Gibbs sampling
    void Sample(int steps){
        for(int k=0;k<steps;k++){
            ProbHiddenGivenVisible(v_,probh_given_v_);
            SampleLayer(h_,probh_given_v_);
            ProbVisibleGivenHidden(h_,probv_given_h_);
            SampleLayer(v_,probv_given_h_);
        }
    }

    // Get RBM parameters
    Eigen::VectorXd GetParameters(){
        Eigen::VectorXd pars(npar_);
        int p=0;
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                pars(p)=W_(i,j);
                p++;
            }
        }
        for(int j=0;j<nv_;j++){
            pars(p)=b_(j);
            p++;
        }
        for(int i=0;i<nh_;i++){
            pars(p)=c_(i);
            p++;
        }
        return pars;
    }

    // Set RBM parameters
    void SetParameters(const Eigen::VectorXd & pars){
        int p=0;
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                W_(i,j)=pars(p);
                p++;
            }
        }
        for(int j=0;j<nv_;j++){
            b_(j)=pars(p);
            p++;
        }
        for(int i=0;i<nh_;i++){
            c_(i)=pars(p);
            p++;
        }
    }

    // Read weights from file
    void LoadWeights(std::ifstream &fin){
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                fin >> W_(i,j);
            }
        }
        for(int j=0;j<nv_;j++){
            fin >> b_(j);
        }
        for(int i=0;i<nh_;i++){
            fin >> c_(i);
        }
    }

    // Read weights from file
    void SaveWeights(std::ofstream &fout){
        for(int i=0;i<nh_;i++){
            for(int j=0;j<nv_;j++){
                fout << W_(i,j) << " ";
            }
            fout << std::endl;
        }
        fout << std::endl;
        for(int j=0;j<nv_;j++){
            fout <<  b_(j) << " ";
        }
        fout << std::endl <<std::endl;
        for(int i=0;i<nh_;i++){
            fout << c_(i) << " ";
        }
        fout << std::endl <<std::endl;
    }

    // Functions
    inline void logistic(const Eigen::VectorXd & x,Eigen::VectorXd & y){
        for(int i=0;i<x.size();i++){
            y(i)=1./(1.+std::exp(-x(i)));;
        }
    }
    inline void logistic(const Eigen::MatrixXd & x,Eigen::MatrixXd & y){
        for(int i=0;i<x.rows();i++){
            for(int j=0;j<x.cols();j++){
                //y(i,j)=logistic(x(i,j));
                y(i,j)=1./(1.+std::exp(-x(i,j)));
            }
        }
    }
    inline double ln1pexp(double x){
        if(x>30){
            return x;
        }
        return std::log1p(std::exp(x));
    }
    void ln1pexp(const Eigen::VectorXd & x,Eigen::VectorXd & y){
        for(int i=0;i<x.size();i++){
            y(i)=ln1pexp(x(i));
        }
    }

 };

}

#endif


#include "gl_core_3_2.h"
#include "SDL.h"
#include "SDL_opengl.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "loadshaders_3_2.h"

#include <cmath>
#include <iostream>
#include <stdint.h>

namespace {

constexpr GLubyte *bufferOffset(size_t bytes)
{
    return static_cast<GLubyte *>(0) + bytes;
}

const char kVertexShader[] = R"(
#version 150

in vec4 vPosition;
in vec3 vColor;
uniform mat4 mModelViewProjection;

out vec4 fColor;

void main()
{
    gl_Position = mModelViewProjection * vPosition;
    fColor = vec4(vColor, 1.0);
}
)";
const char kFragmentShader[] = R"(
#version 150

in vec4 fColor;
out vec4 fragColor;

void main()
{
    fragColor = fColor;
}
)";

const int kWindowWidth = 800;
const int kWindowHeight = 600;

enum VAO_IDs { Triangles, NumVAOs };
enum Buffer_IDs { ArrayBuffer, ColorBuffer, NumBuffers };

GLuint VAOs[NumVAOs];
GLuint Buffers[NumBuffers];

const GLuint NumVertices = 6;

GLint vPositionIndex = -1;
GLint vColorIndex = -1;
GLint mModelViewProjectionIndex = -1;

const float squareWidth = 100.0f;
const float squareSpeed = 2 * squareWidth;
Eigen::Matrix4f squareTransform;
Eigen::Vector2f squareDirection;

void initSquare()
{
    glGenVertexArrays(NumVAOs, VAOs);
    glBindVertexArray(VAOs[Triangles]);

    Eigen::Vector3f vertices[NumVertices] = {
        { -0.5, -0.5, 0.0 },
        {  0.5, -0.5, 0.0 },
        { -0.5,  0.5, 0.0 },
        { -0.5,  0.5, 0.0 },
        {  0.5, -0.5, 0.0 },
        {  0.5,  0.5, 0.0 }
    };

    glGenBuffers(NumBuffers, Buffers);
    glBindBuffer(GL_ARRAY_BUFFER, Buffers[ArrayBuffer]);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    glVertexAttribPointer(vPositionIndex, 3, GL_FLOAT, GL_FALSE, 0, bufferOffset(0));
    glEnableVertexAttribArray(vPositionIndex);

    struct { float r, g, b; } colors[NumVertices] = {
        { 0.0, 0.0, 1.0 },
        { 0.0, 0.0, 1.0 },
        { 0.0, 0.0, 1.0 },
        { 0.0, 0.0, 1.0 },
        { 0.0, 0.0, 1.0 },
        { 0.0, 0.0, 1.0 }
    };

    glBindBuffer(GL_ARRAY_BUFFER, Buffers[ColorBuffer]);
    glBufferData(GL_ARRAY_BUFFER, sizeof(colors), colors, GL_STATIC_DRAW);

    glVertexAttribPointer(vColorIndex, 3, GL_FLOAT, GL_FALSE, 0, bufferOffset(0));
    glEnableVertexAttribArray(vColorIndex);

    squareDirection = Eigen::Vector2f(1.0, -1.0).normalized();
    squareTransform << squareWidth,           0, 0, (squareWidth/2)*1.25,
                                 0, squareWidth, 0, kWindowHeight - ((squareWidth/2)*1.25),
                                 0,           0, 1, 0,
                                 0,           0, 0, 1;
}

void init()
{
    glEnable(GL_CULL_FACE);
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);

    GLuint vertexShader = loadShaderFromString(GL_VERTEX_SHADER, kVertexShader, "kVertexShader");
    GLuint fragmentShader = loadShaderFromString(GL_FRAGMENT_SHADER, kFragmentShader, "kFragmentShader");

    GLuint program = linkShader(vertexShader, fragmentShader);
    glUseProgram(program);

    vPositionIndex = glGetAttribLocation(program, "vPosition");
    vColorIndex = glGetAttribLocation(program, "vColor");
    mModelViewProjectionIndex = glGetUniformLocation(program, "mModelViewProjection");

    initSquare();
}

Eigen::Matrix4f createOrthographicProjection(float left, float right, float bottom, float top, float zNear, float zFar)
{
    float a = 2.0 / (right - left);
    float b = 2.0 / (top - bottom);
    float c = 2.0 / (zFar - zNear);
    float d = -(right + left) / (right - left);
    float e = -(top + bottom) / (top - bottom);
    float f = -(zFar + zNear) / (zFar - zNear);

    Eigen::Matrix4f result;
    result << a, 0, 0, d,
              0, b, 0, e,
              0, 0, c, f,
              0, 0, 0, 1;

    return result;
}

const Eigen::Matrix4f kProjection = createOrthographicProjection(0, kWindowWidth, 0, kWindowHeight, -1, 1);

uint32_t oldTime = 0;
uint32_t currentTime = 0;

void updateSquare()
{
    oldTime = currentTime;
    currentTime = SDL_GetTicks();
    float deltaTime = (currentTime - oldTime) / 1000.0f;

    auto pos = squareTransform.topRightCorner<2, 1>();
    if (pos(0) < (squareWidth/2) && squareDirection(0) < 0){
        squareDirection(0) = squareDirection(0) * -1;
        pos(0) = (squareWidth/2);
    }
    if (pos(0) > (kWindowWidth-(squareWidth/2)) && squareDirection(0) > 0) {
        squareDirection(0) = squareDirection(0) * -1;
        pos(0) = (kWindowWidth-(squareWidth/2));
    }
    if (pos(1) < (squareWidth/2) && squareDirection(1) < 0) {
        squareDirection(1) = squareDirection(1) * -1;
        pos(1) = (squareWidth/2);
    }
    if (pos(1) > (kWindowHeight-(squareWidth/2)) && squareDirection(1) > 0) {
        squareDirection(1) = squareDirection(1) * -1;
        pos(1) = (kWindowHeight-(squareWidth/2));
    }

    pos += squareDirection * squareSpeed * deltaTime;
}

void display()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    updateSquare();

    Eigen::Matrix4f modelViewProjection = kProjection * squareTransform;
    glUniformMatrix4fv(mModelViewProjectionIndex, 1, GL_FALSE, modelViewProjection.data());

    glBindVertexArray(VAOs[Triangles]);
    glDrawArrays(GL_TRIANGLES, 0, NumVertices);
}

} // namespace

int main(int argc, char *argv[])
{
    if (SDL_Init(SDL_INIT_VIDEO) != 0) {
        std::cerr << SDL_GetError() << std::endl;
        return -1;
    }

    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);

    // Create an OpenGL capable window
    SDL_Window *window = SDL_CreateWindow(
        "Assignment 1.1",
        SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,
        800, 600,
        SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE
    );
    if (!window) {
        std::cerr << SDL_GetError() << std::endl;
        SDL_Quit();
        return -1;
    }

    // Create an OpenGL context associated with the window.
    SDL_GLContext glcontext = SDL_GL_CreateContext(window);
    if (!glcontext) {
        std::cerr << SDL_GetError() << std::endl;
        SDL_Quit();
        return -1;
    }

    // Load extensions
    if (ogl_LoadFunctions() != ogl_LOAD_SUCCEEDED) {
        SDL_DestroyWindow(window);
        SDL_Quit();
        return -1;
    }

    // Initialize displayables
    init();

    SDL_Event event;
    bool keepRunning = true;
    while (keepRunning) {
        while (SDL_PollEvent(&event)) {
            if (event.type == SDL_QUIT) {
                keepRunning = false;
            }
        }

        // Render scene
        display();

        // Swap buffers
        SDL_GL_SwapWindow(window);
    }

    // Once finished with OpenGL functions, the SDL_GLContext can be deleted.
    SDL_GL_DeleteContext(glcontext);

    // Done! Close the window, clean-up and exit the program.
    SDL_DestroyWindow(window);
    SDL_Quit();

    return 0;
}


/**
 * \file SimpleOverdriveFilter.cpp
 */

#include "SimpleOverdriveFilter.h"
#include <ATK/Utility/fmath.h>
#include <ATK/Utility/ScalarNewtonRaphson.h>

#include <boost/math/special_functions/sign.hpp>

namespace ATK
{
  template<typename DataType_>
  class SimpleOverdriveFilter<DataType_>::SimpleOverdriveFunction
  {
  public:
    using DataType = DataType_;
  protected:
    DataType A;
    DataType B;
    DataType is;
    DataType vt;

    DataType oldy0{0};
    DataType oldexpy0{1};
    DataType oldinvexpy0{1};
    DataType oldy1{0};
    DataType oldexpy1{1};
    DataType oldinvexpy1{1};
  public:
    SimpleOverdriveFunction(DataType dt, DataType R, DataType C, DataType is, DataType vt)
    :A(dt / (2 * C) + R), B(dt / (2 * C) - R), is(is), vt(vt)
    {
    }

    std::pair<DataType, DataType> operator()(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output, DataType y1)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      DataType expdiode_y1_p = fmath::exp(y1 / vt);
      DataType expdiode_y1_m = 1 / expdiode_y1_p;
      DataType expdiode_y0_p;
      DataType expdiode_y0_m;

	    if(y0 == oldy0)
	    {
	      expdiode_y0_p = oldexpy0;
	      expdiode_y0_m = oldinvexpy0;
	    }
	    else if(y0 == oldy1)
	    {
	      expdiode_y0_p = oldexpy1;
	      expdiode_y0_m = oldinvexpy1;
	    }
	    else
	    {
	      expdiode_y0_p = fmath::exp(y0 / vt);
	      expdiode_y0_m = 1 / expdiode_y0_p;
	    }

      oldy0 = y0;
      oldexpy0 = expdiode_y0_p;
      oldinvexpy0 = expdiode_y0_m;

      oldy1 = y1;
      oldexpy1 = expdiode_y1_p;
      oldinvexpy1 = expdiode_y1_m;

      std::pair<DataType, DataType> diode = std::make_pair(is * (expdiode_y1_p - expdiode_y1_m), is * (expdiode_y1_p + expdiode_y1_m) / vt);
      return std::make_pair(A * diode.first + y1 + x0 - x1 + B * is * (expdiode_y0_p - expdiode_y0_m) - y0, A * diode.second + 1);
    }

    DataType estimate(const DataType* ATK_RESTRICT input, DataType* ATK_RESTRICT output)
    {
      auto x0 = input[-1];
      auto x1 = input[0];
      auto y0 = output[-1];
      return affine_estimate(x0, x1, y0);
    }

    DataType affine_estimate(DataType x0, DataType x1, DataType y0)
    {
      auto sinh = (oldexpy1 - oldinvexpy1);
      auto cosh = (oldexpy1 + oldinvexpy1);
      return (x1 - x0 + y0 - is * sinh * B - (sinh - y0 / vt * cosh) * is * A) / (is * cosh * A / vt + 1);
    }
  };

  template <typename DataType>
  SimpleOverdriveFilter<DataType>::SimpleOverdriveFilter()
  :TypedBaseFilter<DataType>(1, 1)
  {
    input_delay = 1;
    output_delay = 1;
  }

  template <typename DataType>
  SimpleOverdriveFilter<DataType>::~SimpleOverdriveFilter()
  {
  }

  template <typename DataType>
  void SimpleOverdriveFilter<DataType>::setup()
  {
    Parent::setup();
    optimizer = std::make_unique<ScalarNewtonRaphson<SimpleOverdriveFunction>>(SimpleOverdriveFunction(static_cast<DataType>(1. / input_sampling_rate),
      10000, static_cast<DataType>(22e-9), static_cast<DataType>(1e-12), static_cast<DataType>(26e-3)));
  }

  template <typename DataType>
  void SimpleOverdriveFilter<DataType>::process_impl(gsl::index size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(gsl::index i = 0; i < size; ++i)
    {
      optimizer->optimize(input + i, output + i);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class SimpleOverdriveFilter<float>;
#endif
  template class SimpleOverdriveFilter<double>;
  }


//          Copyright Rein Halbersma 2014-2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE 50

#include <bitset/compatible/bit_set.hpp>        // bit_set
#include <bitset/compatible/bitset.hpp>         // bitset
#include <bitset/compatible/dynamic_bitset.hpp> // dynamic_bitset
#include <bitset/exhaustive.hpp>                // all_singleton_set_triples
#include <bitset/primitives.hpp>                // mem_is_subset_of, op_bitand, op_bitor, op_xor
#include <xstd/bit_set.hpp>                     // bit_set
#include <boost/dynamic_bitset.hpp>             // dynamic_bitset
#include <boost/mpl/vector.hpp>                 // vector
#include <boost/test/unit_test.hpp>             // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <bitset>                               // bitset
#include <cstdint>                              // uint8_t, uint16_t, uint32_t

BOOST_AUTO_TEST_SUITE(Cubic)

using namespace xstd;

using bitset_types = boost::mpl::vector
<       std::bitset< 0>
,       std::bitset< 1>
,       std::bitset<31>
,       std::bitset<32>
,       std::bitset<33>
,       std::bitset<63>
,       std::bitset<64>
,       std::bitset<65>
,       boost::dynamic_bitset<>
,       bit_set< 0, uint8_t>
,       bit_set< 1, uint8_t>
,       bit_set< 7, uint8_t>
,       bit_set< 8, uint8_t>
,       bit_set< 9, uint8_t>
,       bit_set<15, uint8_t>
,       bit_set<16, uint8_t>
,       bit_set<17, uint8_t>
,       bit_set<24, uint8_t>
,       bit_set< 0, uint16_t>
,       bit_set< 1, uint16_t>
,       bit_set<15, uint16_t>
,       bit_set<16, uint16_t>
,       bit_set<17, uint16_t>
,       bit_set<31, uint16_t>
,       bit_set<32, uint16_t>
,       bit_set<33, uint16_t>
,       bit_set<48, uint16_t>
,       bit_set< 0, uint32_t>
,       bit_set< 1, uint32_t>
,       bit_set<31, uint32_t>
,       bit_set<32, uint32_t>
,       bit_set<33, uint32_t>
,       bit_set<63, uint32_t>
,       bit_set<64, uint32_t>
,       bit_set<65, uint32_t>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(Transitivity, T, bitset_types)
{
        all_singleton_set_triples<T>(mem_is_subset_of());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(AssociativityAndDistributivity, T, bitset_types)
{
        all_singleton_set_triples<T>(op_bitand());
        all_singleton_set_triples<T>(op_bitor());
        all_singleton_set_triples<T>(op_xor());
}

BOOST_AUTO_TEST_SUITE_END()


// Software License Agreement (BSD-3-Clause)
//
// Copyright 2018 The University of North Carolina at Chapel Hill
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.

//! @author William Lee

#ifndef HOLONOMIC_2D_POINT_HPP
#define HOLONOMIC_2D_POINT_HPP

#include "shape_hierarchy.hpp"
#include <mpt/lp_space.hpp>
#include <mpt/box_bounds.hpp>
#include <mpt/goal_state.hpp>
#include <Eigen/Dense>
#include <vector>

namespace mpt_demo {
    using namespace unc::robotics;
    using namespace shape;

    template <typename Scalar = double>
    class Holonomic2DPointScenario {
    public:
        using Space = unc::robotics::mpt::L2Space<Scalar, 2>;
        using Bounds = unc::robotics::mpt::BoxBounds<Scalar, 2>;
        using State = typename Space::Type;
        using Distance = typename Space::Distance;
        using Goal = unc::robotics::mpt::GoalState<Space>;

    private:
        const int width_;
        const int height_;
        const std::vector<Circle<Scalar>> circles_;
        const std::vector<Rect<Scalar>> rects_;
        Bounds bounds_;
        Goal goal_;
        Space space_;

        Bounds makeBounds() {
            Eigen::Matrix<Scalar, 2, 1> min, max;
            min.fill(0);
            max << width_, height_;
            return Bounds(min, max);
        }

    public:
        Holonomic2DPointScenario(
            int width,
            int height,
            std::vector<Circle<Scalar>> &circles,
            std::vector<Rect<Scalar>> &rects,
            State goalState
            )
            :
              width_(width),
              height_(height),
              circles_(circles),
              rects_(rects),
              bounds_(makeBounds()),
              goal_(1e-6, goalState)
        {
        }

        bool valid(const State &q) const {
            for (auto const &c : circles_)
                if (!c.pointIsValid(q))
                    return false;
            for (auto const &r : rects_)
                if (!r.pointIsValid(q))
                    return false;
            return true;
        }

        bool link(const State &a, const State &b) const {
            for (auto const &c : circles_)
                if (!c.segmentIsValid(a, b))
                    return false;
            for (auto const &r : rects_)
                if (!r.segmentIsValid(a, b))
                    return false;
            return true;
        }

        const std::vector<Circle<Scalar>> &circles() const{
            return circles_;
        }

        const Space &space() const {
            return space_;
        }

        const Bounds &bounds() const {
            return bounds_;
        }

        const Goal &goal() const {
            return goal_;
        }
    };
}
#endif


#pragma once
#include <fstream>
#include <iostream>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>

using namespace std;
using namespace boost::geometry;

namespace AtomicEvents {

    class KickingTheBall {

        private:
            double timestamp;
            string playerId;
            model::d2::point_xy<double> playerPos;

        public:
            KickingTheBall();
            KickingTheBall(
                double timestamp,
                string playerId,
                model::d2::point_xy<double> playerPos);
    };

};

namespace ComplexEvents {};


#include <dlib/image_io.h>
#include <dlib/image_processing.h>
#include <dlib/matrix.h>
#include <dlib/matrix/matrix_utilities.h>
#include <dlib/statistics.h>
#include <plot.h>

#include <experimental/filesystem>
#include <iostream>
#include <unordered_map>

using namespace dlib;
namespace fs = std::experimental::filesystem;

const std::vector<std::string> colors{"black", "red",    "blue",  "green",
                                      "cyan",  "yellow", "brown", "magenta"};

using DataType = double;
using Matrix = matrix<DataType>;
using Coords = std::vector<DataType>;
using PointCoords = std::pair<Coords, Coords>;
using Clusters = std::unordered_map<size_t, PointCoords>;

const std::string photo_file_name{"photo.png"};
const std::string data_file_name{"swissroll.dat"};
const std::string labels_file_name{"swissroll_labels.dat"};

void PlotClusters(const Clusters& clusters,
                  const std::string& name,
                  const std::string& file_name) {
  plotcpp::Plot plt(true);
  // plt.SetTerminal("qt");
  plt.SetTerminal("png");
  plt.SetOutput(file_name);
  plt.SetTitle(name);
  plt.SetXLabel("x");
  plt.SetYLabel("y");
  // plt.SetAutoscale();
  plt.GnuplotCommand("set size square");
  plt.GnuplotCommand("set grid");

  auto draw_state = plt.StartDraw2D<Coords::const_iterator>();
  for (auto& cluster : clusters) {
    std::stringstream params;
    params << "lc rgb '" << colors[cluster.first] << "' pt 7";
    plt.AddDrawing(draw_state,
                   plotcpp::Points(
                       cluster.second.first.begin(), cluster.second.first.end(),
                       cluster.second.second.begin(),
                       std::to_string(cluster.first) + " cls", params.str()));
  }

  plt.EndDraw2D(draw_state);
  plt.Flush();
}

void SammonReduction(const std::vector<Matrix>& data,
                     const std::vector<unsigned long>& labels,
                     long target_dim) {
  dlib::sammon_projection sp;
  auto new_data = sp(data, target_dim);

  Clusters clusters;
  for (size_t r = 0; r < new_data.size(); ++r) {
    Matrix vec = new_data[r];
    double x = vec(0, 0);
    double y = vec(1, 0);
    auto l = labels[r];
    clusters[l].first.push_back(x);
    clusters[l].second.push_back(y);
  }

  PlotClusters(clusters, "Sammon Mapping", "sammon-dlib.png");
}

void PCAReduction(const std::vector<Matrix>& data,
                  const std::vector<unsigned long>& labels,
                  double target_dim) {
  dlib::vector_normalizer_pca<Matrix> pca;
  pca.train(data, target_dim / data[0].nr());
  std::vector<Matrix> new_data;
  new_data.reserve(data.size());
  for (size_t i = 0; i < data.size(); ++i) {
    new_data.emplace_back(pca(data[i]));
  }

  Clusters clusters;
  for (size_t r = 0; r < new_data.size(); ++r) {
    Matrix vec = new_data[r];
    double x = vec(0, 0);
    double y = vec(1, 0);
    auto l = labels[r];
    clusters[l].first.push_back(x);
    clusters[l].second.push_back(y);
  }

  PlotClusters(clusters, "PCA", "pca-dlib.png");
}

void LDAReduction(const Matrix& data,
                  const std::vector<unsigned long>& labels,
                  unsigned long target_dim) {
  dlib::matrix<DataType, 0, 1> mean;
  Matrix transform = data;
  dlib::compute_lda_transform(transform, mean, labels, target_dim);

  Clusters clusters;
  for (long r = 0; r < data.nr(); ++r) {
    Matrix row = transform * dlib::trans(dlib::rowm(data, r)) - mean;
    double x = row(0, 0);
    double y = row(1, 0);
    auto l = labels[static_cast<size_t>(r)];
    clusters[l].first.push_back(x);
    clusters[l].second.push_back(y);
  }

  PlotClusters(clusters, "LDA", "lda-dlib.png");
}

void PCACompression(const std::string& image_file, long target_dim) {
  array2d<dlib::rgb_pixel> img;
  load_image(img, image_file);

  array2d<unsigned char> img_gray;
  assign_image(img_gray, img);
  save_png(img_gray, "original.png");

  array2d<DataType> tmp;
  assign_image(tmp, img_gray);
  Matrix img_mat = dlib::mat(tmp);
  img_mat /= 255.;  // scale

  std::cout << "Original data size " << img_mat.size() << std::endl;

  // take patches 8x8
  std::vector<Matrix> data;
  int patch_size = 8;

  for (long r = 0; r < img_mat.nr(); r += patch_size) {
    for (long c = 0; c < img_mat.nc(); c += patch_size) {
      auto sm = dlib::subm(img_mat, r, c, patch_size, patch_size);
      data.emplace_back(dlib::reshape_to_column_vector(sm));
    }
  }

  // normalize data
  auto data_mat = mat(data);
  Matrix m = mean(data_mat);
  Matrix sd = reciprocal(sqrt(variance(data_mat)));

  matrix<decltype(data_mat)::type, 0, 1, decltype(data_mat)::mem_manager_type>
      x(data_mat);
  for (long r = 0; r < x.size(); ++r)
    x(r) = pointwise_multiply(x(r) - m, sd);

  // perform PCA
  Matrix temp, eigen, pca;
  // Compute the svd of the covariance matrix
  dlib::svd(covariance(x), temp, eigen, pca);
  Matrix eigenvalues = diag(eigen);

  rsort_columns(pca, eigenvalues);

  // leave only required number of principal components
  pca = trans(colm(pca, range(0, target_dim)));

  // dimensionality reduction
  std::vector<Matrix> new_data;
  size_t new_size = 0;
  new_data.reserve(data.size());
  for (size_t i = 0; i < data.size(); ++i) {
    new_data.emplace_back(pca * data[i]);
    new_size += static_cast<size_t>(new_data.back().size());
  }

  std::cout << "New data size " << new_size + static_cast<size_t>(pca.size())
            << std::endl;

  // unpack data
  auto pca_matrix_t = dlib::trans(pca);
  Matrix isd = dlib::reciprocal(sd);
  for (size_t i = 0; i < new_data.size(); ++i) {
    Matrix sample = pca_matrix_t * new_data[i];
    new_data[i] = dlib::pointwise_multiply(sample, isd) + m;
  }

  size_t i = 0;
  for (long r = 0; r < img_mat.nr(); r += patch_size) {
    for (long c = 0; c < img_mat.nc(); c += patch_size) {
      auto sm = dlib::reshape(new_data[i], patch_size, patch_size);
      dlib::set_subm(img_mat, r, c, patch_size, patch_size) = sm;
      ++i;
    }
  }

  img_mat *= 255.0;
  assign_image(img_gray, img_mat);
  equalize_histogram(img_gray);
  save_png(img_gray, "compressed.png");
}

int main(int argc, char** argv) {
  if (argc > 1) {
    try {
      auto data_dir = fs::path(argv[1]);
      auto data_file_path = data_dir / data_file_name;
      auto lables_file_path = data_dir / labels_file_name;
      auto photo_file_path = data_dir / photo_file_name;
      if (fs::exists(data_file_path) && fs::exists(lables_file_path) &&
          fs::exists(photo_file_path)) {
        matrix<DataType> data;
        std::vector<Matrix> vdata;
        {
          std::ifstream file(data_file_path);
          file >> data;
          vdata.reserve(static_cast<size_t>(data.nr()));
          for (long row = 0; row < data.nr(); ++row) {
            vdata.emplace_back(dlib::reshape_to_column_vector(
                dlib::subm_clipped(data, row, 0, 1, data.nc())));
          }
        }
        matrix<DataType> labels;
        std::vector<unsigned long> vlables;
        {
          std::ifstream file(lables_file_path);
          file >> labels;
          vlables.resize(static_cast<size_t>(labels.nr()));
          for (long r = 0; r < labels.nr(); ++r) {
            vlables[static_cast<size_t>(r)] =
                static_cast<unsigned long>(labels(r, 0));
          }
        }

        PCACompression(photo_file_path, 10);
        // int target_dim = 2;
        // LDAReduction(data, vlables, target_dim);
        // PCAReduction(vdata, vlables, target_dim);
        // SammonReduction(vdata, vlables, target_dim);
      }
    } catch (const std::exception& err) {
      std::cerr << err.what();
    }
  } else {
    std::cerr << "Please provider path to the datasets folder\n";
  }
  return 0;
}


#ifndef FASTA_HPP
#define FASTA_HPP

#include "shred.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <cmath>
#include <cstdint>
#include <exception>
#include <fstream>
#include <iostream>
#include <map>
#include <numeric>
#include <sstream>

/**
 * A structure to hold a sparse map from k-mer indexes to k-mer
 * counts with increments.
 */
template <typename T = size_t>
struct counter {
  std::map<T, size_t> counts_;

  /**
   * Increment the count for the specified key by one.
   *
   * @param key key whose count is incremented
   */
  void increment(const T& key) {
    if (counts_.find(key) == counts_.end())
      counts_[key] = 0;
    ++counts_[key];
  }

  /**
   * Return the sum of the values of all of the keys.
   */
  size_t count() const {
    size_t y = 0;
    for (const auto& key_count : counts_)
      y += key_count.second;
    return y;
  }
};

/**
 * Return the identifier in the range 0-3 for the specified base.
 * The mapping is defined alphabetically, indexing from zero, so that
 * 'A' maps to 0, 'C' maps to 1, 'G' maps to 2, and 'T' maps to 3,
 * with any other input throwing an exception.
 *
 * @param c character representing a base
 * @return numerical identifier for the base
 * @throw std::runtime_error if c is not one of 'A', 'C', 'G', or
 * 'T'.
 */
std::size_t base_id(char c) {
  switch (c) {
    case 'A' : return 0;
    case 'C' : return 1;
    case 'G' : return 2;
    case 'T' : return 3;
    default: throw
        std::runtime_error("argument to base_id must be one of:"
                           "'A', 'C', 'G', 'T'");
  }
}

/**
 * Return a numerical identifier for the specified kmer.  Each k-mer
 * is read as a base-4 number given the base_id() for each k-mer.  For
 * a fixed K, this assigns each K-mer gets a unique ID between 0 and
 * 4^K - 1 based on a lexicographic start, indexing from 0.
 *
 * @param kmer string representing a k-mer
 * @return identifier for the k-mer.
 * @throw std::runtime_error if there are characters in kmer other
 * than 'A', 'C', 'G', or 'T'.
 */
std::size_t kmer_id(const std::string& kmer) {
  size_t id = 0;
  for (const char& b : kmer)
    id = 4 * id + base_id(b);
  return id;
}

/**
 * A holder mapping sequence ids to their sequence and k-mers.
 */
struct seq_map {
  using mat_t = Eigen::SparseMatrix<double, Eigen::RowMajor>;
  using trip_t = Eigen::Triplet<double>;
  using trips_t = std::vector<trip_t>;

  std::map<std::string, std::string> id_to_seq_;
  int64_t K_;

  /**
   * Return the number of sequences in the domain of the map.
   *
   * @return number of sequences being mapped
   */
  int64_t size() const {
    return id_to_seq_.size();
  }

  /**
   * Return the total number of bases in the range of the map.
   *
   * @return total number of bases in the map
   */
  int64_t total_bases() const {
    return std::accumulate(id_to_seq_.begin(), id_to_seq_.end(), 0,
                           [](const auto& tot, const auto& seq) {
                             return tot + seq.second.size(); });
  }

  /**
   * Return the total number of unique k-mers using this map's k-mer
   *   size.
   *
   * @return total number of unique k-mers
   */
  int64_t kmers() const {
    return pow(4, K_);
  }

  /**
   * Return a vector of strings corresponding to the sequences in the
   * domain of this map.
   *
   * @return the identifiers being mapped
   */
  std::vector<std::string> identifiers() const {
    std::vector<std::string> ids;
    for (const auto& id_seq : id_to_seq_) {
      ids.push_back(id_seq.first);
    }
    return ids;
  }

  std::vector<int> kmer_freq() const {
    int M = std::pow(4, K_);
    int mask = M - 1; // (1u << (2 * K + 1)) - 1;

    std::vector<int> counts(M, 0);
    int n = 0;
    for (const auto& id_seq : id_to_seq_) {
      ++n;
      std::string s = id_seq.second;
      // std::stringstream in(s);
      // shred(K_, in, counts, mask);
      shred2(K_, s, counts, mask);
    }
    return counts;
  }

  /**
   * Return a counter mapping kmers to their frequencies.
   */
  counter<std::string> kmer_frequency() const {
    counter<std::string> c;
    int N = 0;
    for (const auto& id_seq : id_to_seq_) {
      ++N;
      if ((N % 1000) == 0)
	std::cout << N << " / " << id_to_seq_.size() << " isoforms indexed"
		  << std::endl;
      std::string seq = id_seq.second;
      for (size_t i = 0; i < seq.size(); ++i)
	if (seq[i] != 'A' && seq[i] != 'C' && seq[i] != 'G' && seq[i] != 'T')
	  std::cout << "*********** WHOA, found seq[" << i << "] = " << seq[i]
		    << std::endl;
      for (size_t i = 0; i + K_ < seq.size(); ++i) {
        auto kmer = seq.substr(i, K_);
        c.increment(kmer);
      }
    }
    return c;
  }

  /**
   * Return a counter mapping k-mer frequencies to the number of k-mers
   * with that frequency.
   */
  counter<size_t> kmer_frequency_counts() const {
    counter<std::string> c = kmer_frequency();
    counter<size_t> h;
    for (const auto& kmer_freq : c.counts_) {
      h.increment(kmer_freq.second);
    }
    return h;
  }

  mat_t kmer_gene_matrix() const {
    size_t num_kmers = kmers();
    mat_t m(kmers(), size());
    trips_t trips;
    size_t id = 0;
    for (const auto& id_seq : id_to_seq_) {
      std::string seq = id_seq.second;
      counter<size_t> c;
      for (size_t i = 0; i + K_ < seq.size(); ++i) {
        auto kmer = seq.substr(i, K_);
        auto k_id = kmer_id(kmer);
        c.increment(k_id);
      }
      double seq_sz = c.count();
      for (const auto& kmerid_count : c.counts_)
        trips.push_back(trip_t(kmerid_count.first, id,
                               kmerid_count.second / seq_sz));
      ++id;
    }
    m.setFromTriplets(trips.begin(), trips.end());
    return m;
  }

  bool start_seq(std::string& line) {
    return line.size() > 0 && line[0] == '>';
  }

  bool getnextline(std::ifstream& in, const std::string& last,
                   std::string& line) {
    if (!last.empty()) {
      line = last;
      return true;
    }
    while (std::getline(in, line).good())
      if (!line.empty()) {
        return true;
      }
    return false;
  }

  /**
   * Read FASTA formatted data from file with specified name.
   * @param K length of K-mers
   * @param filename name of file from which to read
   */
  seq_map(int64_t K, const std::string& filename)
      : K_(K) {
    std::ifstream in(filename, std::ifstream::in);
    std::string line;
    int64_t count = 0;
    int64_t total_bases = 0;
    std::string lastline = "";
    while (getnextline(in, lastline, line)) {
      lastline = "";
      if (!start_seq(line)) continue;
      std::string id = line.substr(1);
      std::stringstream val;
      while (getnextline(in, lastline, line)) {
        lastline = "";
	if (start_seq(line)) {
          lastline = line;
	  break;
        }
	val << line;
      }
      std::string bp_seq = val.str();
      id_to_seq_[id] = bp_seq;
      total_bases += bp_seq.size();
    }
    in.close();
  }
};

Eigen::VectorXd softmax(const Eigen::VectorXd& alpha) {
  auto alpha_exp = alpha.array().exp();
  return alpha_exp / alpha_exp.sum();
}

struct log_posterior_gradient {
  const double lambda_;
  const Eigen::VectorXd& y_;  // 4^K x 1
  const seq_map::mat_t& x_;   // 4^K x G

  log_posterior_gradient(double lambda,
                         const Eigen::VectorXd& y,
                         const seq_map::mat_t& x)
      : lambda_(lambda), y_(y), x_(x) { }

  double operator()(const Eigen::VectorXd& alpha,
                    Eigen::VectorXd& grad) {
    // implementation lifted and lightly refactored from matrixcalculus.org
    Eigen::VectorXd t0 = alpha.array().exp();
    double t1 = t0.array().sum();
    Eigen::VectorXd t2 = x_.transpose() * (y_.array() * (1 / t1 * x_ * t0).array()).matrix();
    double lpd = y_.transpose() * (x_ * t0 / t1)
        + alpha.dot(alpha) / (2 * lambda_);
    grad = (((1 / t1) * t2).array() * t0.array()).matrix()
        - 1 / (t1 * t1) * t0.transpose() * t2 * t0
        - 1 / lambda_ * alpha;
    return lpd;
  }
};

#endif