/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * 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 <Eigen/Dense>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <openbr/plugins/openbr_internal.h>

#include <openbr/core/common.h>
#include <openbr/core/eigenutils.h>
#include <openbr/core/opencvutils.h>

namespace br
{

/*!
 * \ingroup initializers
 * \brief Initialize Eigen
 * \br_link http://eigen.tuxfamily.org/dox/TopicMultiThreading.html
 * \author Scott Klum \cite sklum
 */
class EigenInitializer : public Initializer
{
    Q_OBJECT

    void initialize() const
    {
        Eigen::initParallel();
    }
};

BR_REGISTER(Initializer, EigenInitializer)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Principal Component Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 *
 * \br_property float keep Options are: [keep < 0 - All eigenvalues are retained, keep == 0 - No PCA is performed and the eigenvectors form an identity matrix, 0 < keep < 1 - Keep is the fraction of the variance to retain, keep >= 1 - keep is the number of leading eigenvectors to retain] Default is 0.95.
 * \br_property int drop BRENDAN OR JOSH FILL ME IN. Default is 0.
 * \br_property bool whiten BRENDAN OR JOSH FILL ME IN. Default is false.
 */
class PCATransform : public Transform
{
    Q_OBJECT
    friend class DFFSTransform;
    friend class LDATransform;

protected:
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    Q_PROPERTY(int drop READ get_drop WRITE set_drop RESET reset_drop STORED false)
    Q_PROPERTY(bool whiten READ get_whiten WRITE set_whiten RESET reset_whiten STORED false)

    BR_PROPERTY(float, keep, 0.95)
    BR_PROPERTY(int, drop, 0)
    BR_PROPERTY(bool, whiten, false)

    Eigen::VectorXf mean, eVals;
    Eigen::MatrixXf eVecs;

    int originalRows;

public:
    PCATransform() : keep(0.95), drop(0), whiten(false) {}

private:
    double residualReconstructionError(const Template &src) const
    {
        Template proj;
        project(src, proj);

        Eigen::Map<const Eigen::VectorXf> srcMap(src.m().ptr<float>(), src.m().rows*src.m().cols);
        Eigen::Map<Eigen::VectorXf> projMap(proj.m().ptr<float>(), keep);

        return (srcMap - mean).squaredNorm() - projMap.squaredNorm();
    }

    void train(const TemplateList &trainingSet)
    {
        if (trainingSet.first().m().type() != CV_32FC1)
            qFatal("Requires single channel 32-bit floating point matrices.");

        originalRows = trainingSet.first().m().rows;
        int dimsIn = trainingSet.first().m().rows * trainingSet.first().m().cols;
        const int instances = trainingSet.size();

        // Map into 64-bit Eigen matrix
        Eigen::MatrixXd data(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(trainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        trainCore(data);
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, keep, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), keep, 1);

        // Do projection
        outMap = eVecs.transpose() * (inMap - mean);
    }

    void store(QDataStream &stream) const
    {
        stream << keep << drop << whiten << originalRows << mean << eVals << eVecs;
    }

    void load(QDataStream &stream)
    {
        stream >> keep >> drop >> whiten >> originalRows >> mean >> eVals >> eVecs;
    }

protected:
    void trainCore(Eigen::MatrixXd data)
    {
        int dimsIn = data.rows();
        int instances = data.cols();
        const bool dominantEigenEstimation = (dimsIn > instances);

        Eigen::MatrixXd allEVals, allEVecs;
        if (keep != 0) {
            // Compute and remove mean
            mean = Eigen::VectorXf(dimsIn);
            for (int i=0; i<dimsIn; i++) mean(i) = data.row(i).sum() / (float)instances;
            for (int i=0; i<dimsIn; i++) data.row(i).array() -= mean(i);

            // Calculate covariance matrix
            Eigen::MatrixXd cov;
            if (dominantEigenEstimation) cov = data.transpose() * data / (instances-1.0);
            else                         cov = data * data.transpose() / (instances-1.0);

            // Compute eigendecomposition. Returns eigenvectors/eigenvalues in increasing order by eigenvalue.
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eSolver(cov);
            allEVals = eSolver.eigenvalues();
            allEVecs = eSolver.eigenvectors();
            if (dominantEigenEstimation) allEVecs = data * allEVecs;
        } else {
            // Null case
            mean = Eigen::VectorXf::Zero(dimsIn);
            allEVecs = Eigen::MatrixXd::Identity(dimsIn, dimsIn);
            allEVals = Eigen::VectorXd::Ones(dimsIn);
        }

        if (keep <= 0) {
            keep = dimsIn - drop;
        } else if (keep < 1) {
            // Keep eigenvectors that retain a certain energy percentage.
            const double totalEnergy = allEVals.sum();
            if (totalEnergy == 0) {
                keep = 0;
            } else {
                double currentEnergy = 0;
                int i=0;
                while ((currentEnergy / totalEnergy < keep) && (i < allEVals.rows())) {
                    currentEnergy += allEVals(allEVals.rows()-(i+1));
                    i++;
                }
                keep = i - drop;
            }
        } else {
            if (keep + drop > allEVals.rows()) {
                qWarning("Insufficient samples, needed at least %d but only got %d.", (int)keep + drop, (int)allEVals.rows());
                keep = allEVals.rows() - drop;
            }
        }

        // Keep highest energy vectors
        eVals = Eigen::VectorXf((int)keep, 1);
        eVecs = Eigen::MatrixXf(allEVecs.rows(), (int)keep);
        for (int i=0; i<keep; i++) {
            int index = allEVals.rows()-(i+drop+1);
            eVals(i) = allEVals(index);
            eVecs.col(i) = allEVecs.col(index).cast<float>() / allEVecs.col(index).norm();
            if (whiten) eVecs.col(i) /= sqrt(eVals(i));
        }

        // Debug output
        if (Globals->verbose) qDebug() << "PCA Training:\n\tDimsIn =" << dimsIn << "\n\tKeep =" << keep;
    }

    void writeEigenVectors(const Eigen::MatrixXd &allEVals, const Eigen::MatrixXd &allEVecs) const
    {
        const int originalCols = mean.rows() / originalRows;

        { // Write out mean image
            cv::Mat out(originalRows, originalCols, CV_32FC1);
            Eigen::Map<Eigen::MatrixXf> outMap(out.ptr<float>(), mean.rows(), 1);
            outMap = mean.col(0);
            // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/mean.png");
        }

        // Write out sample eigen vectors (16 highest, 8 lowest), filename = eigenvalue.
        for (int k=0; k<(int)allEVals.size(); k++) {
            if ((k < 8) || (k >= (int)allEVals.size()-16)) {
                cv::Mat out(originalRows, originalCols, CV_64FC1);
                Eigen::Map<Eigen::MatrixXd> outMap(out.ptr<double>(), mean.rows(), 1);
                outMap = allEVecs.col(k);
                // OpenCVUtils::saveImage(out, Globals->Debug+"/PCA/eigenVectors/"+QString::number(allEVals(k),'f',0)+".png");
            }
        }
    }
};

BR_REGISTER(Transform, PCATransform)

/*!
 * \ingroup transforms
 * \brief Computes Distance From Feature Space (DFFS)
 * \br_paper Moghaddam, Baback, and Alex Pentland.
 *           "Probabilistic visual learning for object representation."
 *           Pattern Analysis and Machine Intelligence, IEEE Transactions on 19.7 (1997): 696-710.
 * \author Josh Klontz \cite jklontz
 * \br_property float keep Sets PCA keep property. Default is 0.95.
 */
class DFFSTransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    BR_PROPERTY(float, keep, 0.95)

    PCATransform pca;
    Transform *cvtFloat;

    void init()
    {
        pca.keep = keep;
        cvtFloat = make("CvtFloat");
    }

    void train(const TemplateList &data)
    {
        pca.train((*cvtFloat)(data));
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        dst.file.set("DFFS", sqrt(pca.residualReconstructionError((*cvtFloat)(src))));
    }

    void store(QDataStream &stream) const
    {
        pca.store(stream);
    }

    void load(QDataStream &stream)
    {
        pca.load(stream);
    }
};

BR_REGISTER(Transform, DFFSTransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace.
 * \author Brendan Klare \cite bklare
 * \author Josh Klontz \cite jklontz
 * \br_property float pcaKeep BRENDAN OR JOSH FILL ME IN. Default is 0.98.
 * \br_property bool pcaWhiten BRENDAN OR JOSH FILL ME IN. Default is false.
 * \br_property int directLDA BRENDAN OR JOSH FILL ME IN. Default is 0.
 * \br_property float directDrop BRENDAN OR JOSH FILL ME IN. Default is 0.1.
 * \br_property QString inputVariable BRENDAN OR JOSH FILL ME IN. Default is "Label".
 * \br_property bool isBinary BRENDAN OR JOSH FILL ME IN. Default is false.
 * \br_property bool normalize BRENDAN OR JOSH FILL ME IN. Default is true.
 */
class LDATransform : public Transform
{
    friend class SparseLDATransform;

    Q_OBJECT
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool pcaWhiten READ get_pcaWhiten WRITE set_pcaWhiten RESET reset_pcaWhiten STORED false)
    Q_PROPERTY(int directLDA READ get_directLDA WRITE set_directLDA RESET reset_directLDA STORED false)
    Q_PROPERTY(float directDrop READ get_directDrop WRITE set_directDrop RESET reset_directDrop STORED false)
    Q_PROPERTY(QString inputVariable READ get_inputVariable WRITE set_inputVariable RESET reset_inputVariable STORED false)
    Q_PROPERTY(bool isBinary READ get_isBinary WRITE set_isBinary RESET reset_isBinary STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, pcaWhiten, false)
    BR_PROPERTY(int, directLDA, 0)
    BR_PROPERTY(float, directDrop, 0.1)
    BR_PROPERTY(QString, inputVariable, "Label")
    BR_PROPERTY(bool, isBinary, false)
    BR_PROPERTY(bool, normalize, true)

    int dimsOut;
    Eigen::VectorXf mean;
    Eigen::MatrixXf projection;
    float stdDev;

    void train(const TemplateList &_trainingSet)
    {
        // creates "Label"
        TemplateList trainingSet = TemplateList::relabel(_trainingSet, inputVariable, isBinary);
        int instances = trainingSet.size();

        // Perform PCA dimensionality reduction
        PCATransform pca;
        pca.keep = pcaKeep;
        pca.whiten = pcaWhiten;
        pca.train(trainingSet);
        mean = pca.mean;

        TemplateList ldaTrainingSet;
        static_cast<Transform*>(&pca)->project(trainingSet, ldaTrainingSet);

        int dimsIn = ldaTrainingSet.first().m().rows * ldaTrainingSet.first().m().cols;

        // OpenBR ensures that class values range from 0 to numClasses-1.
        // Label exists because we created it earlier with relabel
        QList<int> classes = File::get<int>(trainingSet, "Label");
        QMap<int, int> classCounts = trainingSet.countValues<int>("Label");
        const int numClasses = classCounts.size();

        // Map Eigen into OpenCV
        Eigen::MatrixXd data = Eigen::MatrixXd(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Eigen::Map<const Eigen::MatrixXf>(ldaTrainingSet[i].m().ptr<float>(), dimsIn, 1).cast<double>();

        // Removing class means
        Eigen::MatrixXd classMeans = Eigen::MatrixXd::Zero(dimsIn, numClasses);
        for (int i=0; i<instances; i++)  classMeans.col(classes[i]) += data.col(i);
        for (int i=0; i<numClasses; i++) classMeans.col(i) /= classCounts[i];
        for (int i=0; i<instances; i++)  data.col(i) -= classMeans.col(classes[i]);

        PCATransform space1;

        if (!directLDA)
        {
            // The number of LDA dimensions is limited by the degrees
            // of freedom of scatter matrix computed from 'data'. Because
            // the mean of each class is removed (lowering degree of freedom
            // one per class), the total rank of the covariance/scatter
            // matrix that will be computed in PCA is bound by instances - numClasses.
            space1.keep = std::min(dimsIn, instances-numClasses);
            space1.trainCore(data);

            // Divide each eigenvector by sqrt of eigenvalue.
            // This has the effect of whitening the within-class scatter.
            // In effect, this minimizes the within-class variation energy.
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.5);
        }
        else if (directLDA == 2)
        {
            space1.drop = instances - numClasses;
            space1.keep = std::min(dimsIn, instances) - space1.drop;
            space1.trainCore(data);
        }
        else
        {
            // Perform (modified version of) Direct LDA

            // Direct LDA uses to the Null space of the within-class scatter.
            // Thus, the lower rank, is used to our benefit. We are not discarding
            // these vectors now (in non-direct code we use the keep parameter
            // to discard Null space). We keep the Null space b/c this is where
            // the within-class scatter goes to zero, i.e. it is very useful.
            space1.keep = dimsIn;
            space1.trainCore(data);

            if (dimsIn > instances - numClasses) {
                // Here, we are replacing the eigenvalue of the  null space
                // eigenvectors with the eigenvalue (divided by 2) of the
                // smallest eigenvector from the row space eigenvector.
                // This allows us to scale these null-space vectors (otherwise
                // it is a divide by zero.
                double null_eig = space1.eVals(instances - numClasses - 1) / 2;
                for (int i = instances - numClasses; i < dimsIn; i++)
                    space1.eVals(i) = null_eig;
            }

            // Drop the first few leading eigenvectors in the within-class space
            QList<float> eVal_list; eVal_list.reserve(dimsIn);
            float fmax = -1;
            for (int i=0; i<dimsIn; i++) fmax = std::max(fmax, space1.eVals(i));
            for (int i=0; i<dimsIn; i++) eVal_list.append(space1.eVals(i)/fmax);

            QList<float> dSum = Common::CumSum(eVal_list);
            int drop_idx;
            for (drop_idx = 0; drop_idx<dimsIn; drop_idx++)
                if (dSum[drop_idx]/dSum[dimsIn-1] >= directDrop)
                    break;

            drop_idx++;
            space1.keep = dimsIn - drop_idx;

            Eigen::MatrixXf new_vecs = Eigen::MatrixXf(space1.eVecs.rows(), (int)space1.keep);
            Eigen::MatrixXf new_vals = Eigen::MatrixXf((int)space1.keep, 1);

            for (int i = 0; i < space1.keep; i++) {
                new_vecs.col(i) = space1.eVecs.col(i + drop_idx);
                new_vals(i) = space1.eVals(i + drop_idx);
            }

            space1.eVecs = new_vecs;
            space1.eVals = new_vals;

            // We will call this "agressive" whitening. Really, it is not whitening
            // anymore. Instead, we are further scaling the small eigenvalues and the
            // null space eigenvalues (to increase their impact).
            for (int i=0; i<space1.keep; i++) space1.eVecs.col(i) /= pow((double)space1.eVals(i),0.15);
        }

        // Now we project the mean class vectors into this second
        // subspace that minimizes the within-class scatter energy.
        // Inside this subspace we learn a subspace projection that
        // maximizes the between-class scatter energy.
        Eigen::MatrixXd mean2 = Eigen::MatrixXd::Zero(dimsIn, 1);

        // Remove means
        for (int i=0; i<dimsIn; i++)     mean2(i) = classMeans.row(i).sum() / numClasses;
        for (int i=0; i<numClasses; i++) classMeans.col(i) -= mean2;

        // Project into second subspace
        Eigen::MatrixXd data2 = space1.eVecs.transpose().cast<double>() * classMeans;

        // The rank of the between-class scatter matrix is bound by numClasses - 1
        // because each class is a vector used to compute the covariance,
        // but one degree of freedom is lost removing the global mean.
        int dim2 = std::min((int)space1.keep, numClasses-1);
        PCATransform space2;
        space2.keep = dim2;
        space2.trainCore(data2);

        // Compute final projection matrix
        projection = ((space2.eVecs.transpose() * space1.eVecs.transpose()) * pca.eVecs.transpose()).transpose();
        dimsOut = dim2;

        stdDev = 1; // default initialize
        if (isBinary) {
            assert(dimsOut == 1);
            float posVal = 0;
            float negVal = 0;
            Eigen::MatrixXf results(trainingSet.size(),1);
            for (int i = 0; i < trainingSet.size(); i++) {
                Template t;
                project(trainingSet[i],t);
                //Note: the positive class is assumed to be 0 b/c it will
                // typically be the first gallery template in the TemplateList structure
                if (classes[i] == 0)
                    posVal += t.m().at<float>(0,0);
                else if (classes[i] == 1)
                    negVal += t.m().at<float>(0,0);
                else
                    qFatal("Binary mode only supports two class problems.");
                results(i) = t.m().at<float>(0,0);  //used for normalization
            }
            posVal /= classCounts[0];
            negVal /= classCounts[1];

            if (posVal < negVal) {
                //Ensure positive value is supposed to be > 0 after projection
                Eigen::MatrixXf invert = Eigen::MatrixXf::Ones(dimsIn,1);
                invert *= -1;
                projection = invert.transpose() * projection;
            }

            if (normalize)
                stdDev = sqrt(results.array().square().sum() / trainingSet.size());
        }

        qDebug() << "LDA projection dimensions:" << projection.rows() << "->" << projection.cols();
    }

    void project(const Template &src, Template &dst) const
    {
        dst = cv::Mat(1, dimsOut, CV_32FC1);

        // Map Eigen into OpenCV
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        // Do projection
        outMap = projection.transpose() * (inMap - mean);
        if (normalize && isBinary)
            dst.m().at<float>(0,0) = dst.m().at<float>(0,0) / stdDev;
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << directLDA;
        stream << directDrop;
        stream << dimsOut;
        stream << mean;
        stream << projection;
        if (normalize && isBinary)
            stream << stdDev;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> directLDA;
        stream >> directDrop;
        stream >> dimsOut;
        stream >> mean;
        stream >> projection;
        if (normalize && isBinary)
            stream >> stdDev;
    }
};

BR_REGISTER(Transform, LDATransform)

/*!
 * \ingroup transforms
 * \brief Projects input into learned Linear Discriminant Analysis subspace learned on a sparse subset of features with the highest weight in the original LDA algorithm.
 * \author Brendan Klare \cite bklare
 * \br_property float varThreshold BRENDAN FILL ME IN. Default is 1.5.
 * \br_property float pcaKeep BRENDAN FILL ME IN. Default is 0.98.
 * \br_property bool normalize BRENDAN FILL ME IN. Default is true.
 */
class SparseLDATransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float varThreshold READ get_varThreshold WRITE set_varThreshold RESET reset_varThreshold STORED false)
    Q_PROPERTY(float pcaKeep READ get_pcaKeep WRITE set_pcaKeep RESET reset_pcaKeep STORED false)
    Q_PROPERTY(bool normalize READ get_normalize WRITE set_normalize RESET reset_normalize STORED false)
    BR_PROPERTY(float, varThreshold, 1.5)
    BR_PROPERTY(float, pcaKeep, 0.98)
    BR_PROPERTY(bool, normalize, true)

    LDATransform ldaSparse;
    int dimsOut;
    QList<int> selections;

    Eigen::VectorXf mean;

    void init()
    {
        ldaSparse.init();
        ldaSparse.pcaKeep = pcaKeep;
        ldaSparse.inputVariable = "Label";
        ldaSparse.isBinary = true;
        ldaSparse.normalize = true;
    }

    void train(const TemplateList &_trainingSet)
    {

        LDATransform ldaOrig;
        ldaOrig.init();
        ldaOrig.inputVariable = "Label";
        ldaOrig.pcaKeep = pcaKeep;
        ldaOrig.isBinary = true;
        ldaOrig.normalize = true;

        ldaOrig.train(_trainingSet);

        //Only works on binary class problems for now
        assert(ldaOrig.projection.cols() == 1);
        float ldaStd = EigenUtils::stddev(ldaOrig.projection);
        for (int i = 0; i < ldaOrig.projection.rows(); i++)
            if (abs(int(ldaOrig.projection(i))) > varThreshold * ldaStd)
                selections.append(i);

        TemplateList newSet;
        for (int i = 0; i < _trainingSet.size(); i++) {
            cv::Mat x(_trainingSet[i]);
            cv::Mat y = cv::Mat(selections.size(), 1, CV_32FC1);
            int idx = 0;
            int cnt = 0;
            for (int j = 0; j < x.rows; j++)
                for (int k = 0; k < x.cols; k++, cnt++)
                    if (selections.contains(cnt))
                        y.at<float>(idx++,0) = x.at<float>(j, k);
            newSet.append(Template(_trainingSet[i].file, y));
        }
        ldaSparse.train(newSet);
        dimsOut = ldaSparse.dimsOut;
    }

    void project(const Template &src, Template &dst) const
    {
        Eigen::Map<Eigen::MatrixXf> inMap((float*)src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
        Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), dimsOut, 1);

        int d = selections.size();
        cv::Mat inSelect(d,1,CV_32F);
        for (int i = 0; i < d; i++)
            inSelect.at<float>(i) = src.m().at<float>(selections[i]);
        ldaSparse.project(Template(src.file, inSelect), dst);
    }

    void store(QDataStream &stream) const
    {
        stream << pcaKeep;
        stream << ldaSparse;
        stream << dimsOut;
        stream << selections;
    }

    void load(QDataStream &stream)
    {
        stream >> pcaKeep;
        stream >> ldaSparse;
        stream >> dimsOut;
        stream >> selections;
    }
};

BR_REGISTER(Transform, SparseLDATransform)

using namespace Eigen;
using namespace cv;

/*!
 * \ingroup transforms
 * \brief Projects input into a within-class minimizing subspace.
 *
 * Like LDA but without the explicit between-class consideration.
 *
 * Note on Compression:
 * Projection matricies can become quite large, resulting in proportionally large model files.
 * WCDA automatically alleviates this issue with lossy compression of the projection matrix.
 * Each element is stored as an 8-bit integer instead of a 32-bit float, resulting in a 75% reduction in the size of the projection matrix.
 * A non-linear (sqrt) scaling is used because element values are distributed around 0, in affect allowing for higher precision storage of the more frequently occuring values.
 *
 * \author Josh Klontz \cite jklontz
 */
class WCDATransform : public Transform
{
    Q_OBJECT
    Q_PROPERTY(float keep READ get_keep WRITE set_keep RESET reset_keep STORED false)
    BR_PROPERTY(float, keep, 0.98)

    VectorXf mean;
    MatrixXf projection;

    typedef Matrix<quint8, Dynamic, Dynamic> CompressedMatrix;
    CompressedMatrix compressed;
    float a, b;

    static CompressedMatrix compress(MatrixXf src, float &a, float &b)
    {
        for (int i=0; i<src.rows(); i++)
            for (int j=0; j<src.cols(); j++) {
                float &element = src(i, j);
                element = sqrtf(fabsf(element)) * ((element >= 0) ? 1 : -1);
            }
        b = src.minCoeff();
        a = (src.maxCoeff() - b) / 255;
        return ((src.array() - b) / a).cast<quint8>();
    }

    static MatrixXf decompress(const CompressedMatrix &src, float a, float b)
    {
        MatrixXf dst = src.cast<float>().array() * a + b;
        for (int i=0; i<dst.rows(); i++)
            for (int j=0; j<dst.cols(); j++) {
                float &element = dst(i, j);
                element = (element * element) * ((element >= 0) ? 1 : -1);
            }
        return dst;
    }

    void train(const TemplateList &_templates)
    {
        // Relabel ensures that class values range from 0 to numClasses-1.
        const TemplateList templates = TemplateList::relabel(_templates, "Label", false);
        const int instances = templates.size();
        const int dimsIn = templates.first().m().rows * templates.first().m().cols;

        // Map data into Eigen
        MatrixXf data(dimsIn, instances);
        for (int i=0; i<instances; i++)
            data.col(i) = Map<const VectorXf>(templates[i].m().ptr<float>(), dimsIn);

        // Perform PCA dimensionality reduction
        VectorXf pcaEvals;
        MatrixXf pcaEvecs;
        trainCore(data, keep, mean, pcaEvals, pcaEvecs);
        data = pcaEvecs.transpose() * (data.colwise() - mean);

        // Get ground truth
        const QList<int> classes = File::get<int>(templates, "Label");
        const QMap<int, int> classCounts = templates.countValues<int>("Label");
        const int numClasses = classCounts.size();

        // Compute and remove the class means
        MatrixXf classMeans = MatrixXf::Zero(data.rows(), numClasses);
        for (int i=0; i<instances;  i++) classMeans.col(classes[i]) += data.col(i);
        for (int i=0; i<numClasses; i++) classMeans.col(i) /= classCounts[i];
        for (int i=0; i<instances;  i++) data.col(i) -= classMeans.col(classes[i]);

        // Compute the within-class projection
        VectorXf wcMean, wcEvals;
        MatrixXf wcEvecs;
        trainCore(data, keep, wcMean, wcEvals, wcEvecs);

        // This has the effect of whitening the within-class scatter.
        // In effect, this minimizes the within-class variation energy.
        for (int i=0; i<keep; i++)
            wcEvecs.col(i) /= sqrt(wcEvals(i));

        projection = wcEvecs.transpose() * pcaEvecs.transpose();

        // Compress and restore the projection matrix
        compressed = compress(projection, a, b);
        projection = decompress(compressed, a, b);
    }

    static void trainCore(MatrixXf data, float &keep, VectorXf &mean, VectorXf &eVals, MatrixXf &eVecs)
    {
        const int dimsIn = data.rows();
        const int instances = data.cols();
        const bool dominantEigenEstimation = (dimsIn > instances);

        // Compute and remove mean
        mean = data.rowwise().sum() / instances;
        data.colwise() -= mean;

        // Calculate and normalize covariance matrix
        MatrixXf cov;
        if (dominantEigenEstimation) cov = data.transpose() * data;
        else                         cov = data * data.transpose();
        cov /= (instances-1);

        // Compute eigendecomposition, returning eigenvectors/eigenvalues in increasing order by eigenvalue.
        SelfAdjointEigenSolver<MatrixXf> eSolver(cov);
        VectorXf allEVals = eSolver.eigenvalues();
        MatrixXf allEVecs = eSolver.eigenvectors();
        if (dominantEigenEstimation)
            allEVecs = data * allEVecs;

        if (keep < 1) {
            // Keep eigenvectors that retain a certain energy percentage.
            const float desiredEnergy = keep * allEVals.sum();
            float currentEnergy = 0;
            int i = 0;
            while ((currentEnergy < desiredEnergy) && (i < allEVals.rows())) {
                currentEnergy += allEVals(allEVals.rows()-(i+1));
                i++;
            }
            keep = i;
        } else {
            if (keep > allEVals.rows()) {
                qWarning("Insufficient samples, needed at least %d but only got %d.", (int)keep, (int)allEVals.rows());
                keep = allEVals.rows();
            }
        }

        // Keep highest energy vectors
        eVals = VectorXf((int)keep);
        eVecs = MatrixXf(allEVecs.rows(), (int)keep);
        for (int i=0; i<keep; i++) {
            const int index = allEVals.rows()-(i+1);
            eVals(i) = allEVals(index);
            eVecs.col(i) = allEVecs.col(index) / allEVecs.col(index).norm();
        }
    }

    void project(const Template &src, Template &dst) const
    {
        dst = Mat(1, projection.rows(), CV_32FC1);
        Map<const VectorXf> inMap(src.m().ptr<float>(), src.m().rows * src.m().cols);
        Map<VectorXf> outMap(dst.m().ptr<float>(), projection.rows());
        outMap = projection * (inMap - mean);
    }

    void store(QDataStream &stream) const
    {
        stream << mean << compressed << a << b;
    }

    void load(QDataStream &stream)
    {
        stream >> mean >> compressed >> a >> b;
        projection = decompress(compressed, a, b);
    }
};

BR_REGISTER(Transform, WCDATransform)

// For use in BBLDAAlignmentTransform
// A single decision boundary with gaussian distributed responses
struct DecisionBoundary
{
    VectorXf function;
    float positiveMean, negativeMean, positiveSD, negativeSD;
};

QDataStream &operator<<(QDataStream &stream, const DecisionBoundary &db)
{
    return stream << db.function << db.positiveMean << db.negativeMean << db.positiveSD << db.negativeSD;
}

QDataStream &operator>>(QDataStream &stream, DecisionBoundary &db)
{
    return stream >> db.function >> db.positiveMean >> db.negativeMean >> db.positiveSD >> db.negativeSD;
}

/*!
 * \ingroup transforms
 * \brief Boosted Binary LDA classifier for local alignment.
 * \author Unknown \cite Unknown
 */
class BBLDAAlignmentTransform : public MetaTransform
{
    Q_OBJECT
    Q_PROPERTY(QList<int> anchors READ get_anchors WRITE set_anchors RESET reset_anchors STORED false) // A list of two indicies to provide initial rotation and scaling of training data
    Q_PROPERTY(QString detectionKey READ get_detectionKey WRITE set_detectionKey RESET reset_detectionKey STORED false) // Metadata key containing the detection bounding box
    Q_PROPERTY(int iad READ get_iad WRITE set_iad RESET reset_iad STORED false) // Inter-anchor distance in pixels
    Q_PROPERTY(float radius READ get_radius WRITE set_radius RESET reset_radius STORED false) // Kernel size as a fraction of IAD
    Q_PROPERTY(int resolution READ get_resolution WRITE set_resolution RESET reset_resolution STORED false) // Project resolution for sampling rotation and scale
    BR_PROPERTY(QList<int>, anchors, QList<int>())
    BR_PROPERTY(QString, detectionKey, "FrontalFace")
    BR_PROPERTY(int, iad, 16)
    BR_PROPERTY(float, radius, 2)
    BR_PROPERTY(int, resolution, 20)

    typedef Matrix<quint8, Dynamic, Dynamic> MatrixX8U;

    struct RegistrationMatrix
    {
        Mat src;
        Point2f center;
        double angle, scale;

        RegistrationMatrix(const Mat &src = Mat(), const Point2f &center = Point2f(), double angle = 0, double scale = 1)
            : src(src)
            , center(center)
            , angle(angle)
            , scale(scale)
        {}

        Mat calculateRotationMatrix2D(int size) const
        {
            Mat rotationMatrix2D = getRotationMatrix2D(center, angle, scale);
            rotationMatrix2D.at<double>(0, 2) -= (center.x - size / 2.0); // Adjust the center to the middle of the dst image
            rotationMatrix2D.at<double>(1, 2) -= (center.y - size / 2.0);
            return rotationMatrix2D;
        }

        Point2f invert(double x, double y, int size) const
        {
            Mat m;
            invertAffineTransform(calculateRotationMatrix2D(size), m);

            Mat src(1, 1, CV_64FC2);
            src.ptr<double>()[0] = y; // According to the docs these should be specified in the opposite order ...
            src.ptr<double>()[1] = x; // ... however running it seems to suggest this is the correct way

            Mat dst;
            transform(src, dst, m);
            return Point2f(dst.ptr<double>()[0], dst.ptr<double>()[1]);
        }

        MatrixX8U warp(int size) const
        {
            const Mat rotationMatrix2D = calculateRotationMatrix2D(size);
            Mat dst;
            warpAffine(src, dst, rotationMatrix2D, Size(size, size), INTER_LINEAR, BORDER_REFLECT);
            return Map<MatrixX8U>(dst.ptr<quint8>(), size, size);
        }
    };

    float rotationMax, scaleMin, scaleMax, xTranslationMax, yTranslationMin, yTranslationMax;
    QList<DecisionBoundary> decisionBoundaries;

    static void show(const char *name, const MatrixX8U &data)
    {
        const Mat mat(data.rows(), data.cols(), CV_8UC1, (void*) data.data());
        imshow(name, mat);
        waitKey(-1);
    }

    static void show(const char *name, MatrixXf data)
    {
        data.array() -= data.minCoeff();
        data.array() *= (255 / data.maxCoeff());
        show(name, MatrixX8U(data.cast<quint8>()));
    }

    static void show(const char *name, VectorXf data)
    {
        MatrixXf resized = data;
        const int size = int(sqrtf(float(data.rows())));
        resized.resize(size, size);
        show(name, resized);
    }

    static MatrixXf getSample(const MatrixXf &registered, int j, int k, int kernelSize)
    {
        MatrixXf sample(registered.block(j, k, kernelSize, kernelSize));
        const float norm = sample.norm();
        if (norm > 0)
            sample /= norm;
        return sample;
    }

    static MatrixXf getSample(const QList<MatrixXf> &registered, int i, int j, int k, int kernelSize)
    {
        return getSample(registered[i], j, k, kernelSize);
    }

    static float IADError(const MatrixXf &responses, const MatrixXf &mask)
    {
        const int responseSize = sqrtf(responses.cols());
        int numImages = 0;
        float totalError = 0;
        for (int i=0; i<responses.rows(); i++) {
            float maxResponse = -std::numeric_limits<float>::max();
            int maxX = -1;
            int maxY = -1;
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++)
                    if (mask(i, j*responseSize + k) > 0) {
                        const float response = responses(i, j*responseSize+k);
                        if (response > maxResponse) {
                            maxResponse = response;
                            maxY = j;
                            maxX = k;
                        }
                    }
            totalError += sqrtf(powf(maxX - responseSize/2, 2) + powf(maxY - responseSize/2, 2)) / responseSize;
            numImages++;
        }
        return totalError / numImages;
    }

    static bool isPositive(int j, int k, int responseSize)
    {
        return (abs(j - responseSize/2) <= 0) && (abs(k - responseSize/2) <= 0);
    }

    static MatrixXf /* output responses */ computeBoundaryRecursive(const QList<MatrixXf> &registered, int kernelSize, const MatrixXf &prior /* input responses */, QList<DecisionBoundary> &boundaries)
    {
        const int numImages = registered.size();
        const int responseSize = registered.first().cols() - kernelSize + 1;
        int positiveSamples = 0;
        int negativeSamples = 0;

        // Compute weights, a weight of zero operates as a mask
        MatrixXf weights(numImages, responseSize*responseSize);
        float totalPositiveWeight = 0, totalNegativeWeight = 0;
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++) {
                    const float probability = prior(i, j*responseSize+k);
                    const bool positive = isPositive(j, k, responseSize);
                    // Weight by probability ...
                    float weight = positive ? (2 - probability) // Subtracting from a number greater than 1 helps ensure that we don't overfit outliers
                                            : probability;
                    // ... and by distance
                    const float distance = sqrtf(powf(j - responseSize/2, 2) + powf(k - responseSize/2, 2));
                    if (positive) weight *= 1 / (distance + 1);
                    else          weight *= distance;
                    weights(i, j*responseSize+k) = weight;
                    if (weight > 0) {
                        if (positive) { totalPositiveWeight += weight; positiveSamples++; }
                        else          { totalNegativeWeight += weight; negativeSamples++; }
                    }
                }

        // Normalize weights to preserve class sample ratio
        const float positiveWeightAdjustment = positiveSamples / totalPositiveWeight;
        const float negativeWeightAdjustment = negativeSamples / totalNegativeWeight;
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++)
                    weights(i, j*responseSize+k) *= isPositive(j, k, responseSize) ? positiveWeightAdjustment : negativeWeightAdjustment;

        // Compute weighted class means
        MatrixXf positiveMean = MatrixXf::Zero(kernelSize, kernelSize);
        MatrixXf negativeMean = MatrixXf::Zero(kernelSize, kernelSize);
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++) {
                    const MatrixXf sample(getSample(registered, i, j, k, kernelSize));
                    const float weight = weights(i, j*responseSize+k);
                    if (weight > 0) {
                        if (isPositive(j, k, responseSize)) positiveMean += sample * weight;
                        else                                negativeMean += sample * weight;
                    }
                }
        positiveMean /= positiveSamples;
        negativeMean /= negativeSamples;

        // Compute weighted scatter matrix and decision boundary
        MatrixXf scatter = MatrixXf::Zero(kernelSize*kernelSize, kernelSize*kernelSize);
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++) {
                    const float weight = weights(i, j*responseSize+k);
                    if (weight > 0) {
                        const MatrixXf &centeredSampleMatrix = getSample(registered, i, j, k, kernelSize) - (isPositive(j, k, responseSize) ? positiveMean : negativeMean);
                        const Map<const VectorXf> centeredSample(centeredSampleMatrix.data(), kernelSize*kernelSize);
                        scatter.noalias() += centeredSample * centeredSample.transpose() * weights(i, j*responseSize+k);
                    }
                }
        scatter /= (positiveSamples + negativeSamples); // normalize for numerical stability
        DecisionBoundary db;
        db.function = scatter.inverse() * VectorXf(positiveMean - negativeMean); // fisher linear discriminant

        // Compute response values to decision boundary
        MatrixXf posterior(numImages, responseSize*responseSize);
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++)
                    if (weights(i, j*responseSize + k) > 0) {
                        const MatrixXf sample(getSample(registered, i, j, k, kernelSize));
                        posterior(i, j*responseSize + k) = db.function.transpose() * Map<const VectorXf>(sample.data(), kernelSize*kernelSize);
                    }

        // Compute class response means
        db.positiveMean = 0;
        db.negativeMean = 0;
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++)
                    if (weights(i, j*responseSize + k) > 0) {
                        const float response = posterior(i, j*responseSize + k);
                        if (isPositive(j, k, responseSize)) db.positiveMean += response;
                        else                                db.negativeMean += response;
                    }
        db.positiveMean /= positiveSamples;
        db.negativeMean /= negativeSamples;

        // Compute class response standard deviations
        db.positiveSD = 0;
        db.negativeSD = 0;
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++)
                    if (weights(i, j*responseSize + k) > 0) {
                        const float response = posterior(i, j*responseSize + k);
                        if (isPositive(j, k, responseSize)) db.positiveSD += powf(response-db.positiveMean, 2.f);
                        else                                db.negativeSD += powf(response-db.negativeMean, 2.f);
                    }
        db.positiveSD = sqrtf(db.positiveSD / positiveSamples);
        db.negativeSD = sqrtf(db.negativeSD / negativeSamples);

        // Normalize responses and propogating prior probabilities
        for (int i=0; i<numImages; i++)
            for (int j=0; j<responseSize; j++)
                for (int k=0; k<responseSize; k++) {
                    float &response = posterior(i, j*responseSize + k);
                    if (weights(i, j*responseSize + k) > 0) {
                        const float positiveDensity = 1 / (sqrtf(2 * CV_PI) * db.positiveSD) * expf(-0.5 * powf((response - db.positiveMean) / db.positiveSD, 2.f));
                        const float negativeDensity = 1 / (sqrtf(2 * CV_PI) * db.negativeSD) * expf(-0.5 * powf((response - db.negativeMean) / db.negativeSD, 2.f));
                        response = prior(i, j*responseSize+k) * positiveDensity / (positiveDensity + negativeDensity);
                    } else {
                        response = 0;
                    }
                }
        const float previousMeanError = IADError(prior, weights);
        const float meanError = IADError(posterior, weights);
        qDebug() << "Samples positive & negative:" << positiveSamples << negativeSamples;
        qDebug() << "Positive mean & stddev:" << db.positiveMean << db.positiveSD;
        qDebug() << "Negative mean & stddev:" << db.negativeMean << db.negativeSD;
        qDebug() << "Mean error before & after:" << previousMeanError << meanError;

        if (meanError < previousMeanError) {
            boundaries.append(db);
            return computeBoundaryRecursive(registered, kernelSize, posterior, boundaries);
        } else {
            return prior;
        }
    }

    void train(const TemplateList &data)
    {
        QList<RegistrationMatrix> registrationMatricies;
        {
            if (Globals->verbose)
                qDebug("Preprocessing training data...");

            rotationMax     = -std::numeric_limits<float>::max();
            scaleMin        =  std::numeric_limits<float>::max();
            scaleMax        = -std::numeric_limits<float>::max();
            xTranslationMax = -std::numeric_limits<float>::max();
            yTranslationMin =  std::numeric_limits<float>::max();
            yTranslationMax = -std::numeric_limits<float>::max();

            foreach (const Template &t, data) {
                const QRectF detection = t.file.get<QRectF>(detectionKey);
                const QList<QPointF> points = t.file.points();
                const float length = sqrt(pow(points[anchors[1]].x() - points[anchors[0]].x(), 2) +
                                          pow(points[anchors[1]].y() - points[anchors[0]].y(), 2));
                const float rotation = atan2(points[anchors[1]].y() - points[anchors[0]].y(),
                                             points[anchors[1]].x() - points[anchors[0]].x()) * 180 / CV_PI;
                const float scale = length / detection.width();
                const float xCenter = (points[anchors[0]].x() + points[anchors[1]].x()) / 2;
                const float yCenter = (points[anchors[0]].y() + points[anchors[1]].y()) / 2;
                const float xTranslation = (xCenter - detection.x() - detection.width() /2) / detection.width();
                const float yTranslation = (yCenter - detection.y() - detection.height()/2) / detection.width();

                if (detection.contains(xCenter, yCenter) /* Sometimes the face detector gets the wrong face */) {
                    rotationMax = max(rotationMax, fabsf(rotation));
                    scaleMin    = min(scaleMin, scale);
                    scaleMax    = max(scaleMax, scale);
                    xTranslationMax = max(xTranslationMax, fabsf(xTranslation));
                    yTranslationMin = min(yTranslationMin, yTranslation);
                    yTranslationMax = max(yTranslationMax, yTranslation);
                }

                registrationMatricies.append(RegistrationMatrix(t, Point2f(xCenter, yCenter), rotation, iad / length));
            }
        }

        if (Globals->verbose)
            qDebug("Learning affine model ...");

        // Construct the registered training data for the landmark
        const int regionSize = 2 * iad * radius; // Train on a search size that is twice to the kernel size
        QList<MatrixXf> registered;
        foreach (const RegistrationMatrix &registrationMatrix, registrationMatricies)
            registered.append(registrationMatrix.warp(regionSize).cast<float>());

        // Boosted LDA
        const int numImages = registered.size();
        const int kernelSize = iad * radius;
        const int responseSize = regionSize - kernelSize + 1;
        const MatrixXf prior = MatrixXf::Ones(numImages, responseSize*responseSize);
        const MatrixXf responses = computeBoundaryRecursive(registered, kernelSize, prior, decisionBoundaries);

//        for (int i=0; i<numImages; i++) {
//            float maxResponse = -std::numeric_limits<float>::max();
//            int maxX = -1;
//            int maxY = -1;
//            for (int j=0; j<responseSize; j++)
//                for (int k=0; k<responseSize; k++) {
//                    const float response = responses(i, j*responseSize+k);
//                    if (response > maxResponse) {
//                        maxResponse = response;
//                        maxY = j;
//                        maxX = k;
//                    }
//                }

//            MatrixXf draw = registered[i];
//            const int r = 3;
//            maxX += kernelSize / 2;
//            maxY += kernelSize / 2;
//            for (int i=-r; i<=r; i++)
//                draw(regionSize/2 + i, regionSize/2) *= 2;
//            for (int i=-r; i<=r; i++)
//                draw(regionSize/2, regionSize/2 + i) *= 2;
//            for (int i=-r; i<=r; i++)
//                draw(maxX + i, maxY) /= 2;
//            for (int i=-r; i<=r; i++)
//                draw(maxX, maxY + i) /= 2;
//            show("draw", draw);
//            show("responses", VectorXf(responses.row(i)));
//        }

        if (Globals->verbose)
            qDebug("Learned %d function(s) with error %g", decisionBoundaries.size(), IADError(responses, prior));
    }

    void project(const Template &src, Template &dst) const
    {
        dst = src;
        const QRectF detection = src.file.get<QRectF>(detectionKey);
        RegistrationMatrix registrationMatrix(src, OpenCVUtils::toPoint(detection.center()));

        const int regionSize = iad * (1 + radius);
        const int kernelSize = iad * radius;
        const int responseSize = regionSize - kernelSize + 1;
        const float rotationStep = (2 * rotationMax) / (resolution - 1);
        const float scaleStep = (scaleMax - scaleMin) / (resolution - 1);

        float bestResponse = -std::numeric_limits<float>::max(), bestRotation = 0, bestScale = 0;
        int bestX = 0, bestY =0;

        for (float rotation = -rotationMax; rotation <= rotationMax; rotation += rotationStep) {
            registrationMatrix.angle = rotation;
            for (float scale = scaleMin; scale <= scaleMax; scale += scaleStep) {
                registrationMatrix.scale = iad / (scale * detection.width());
                const MatrixXf registered = registrationMatrix.warp(regionSize).cast<float>();

                for (int j=0; j<responseSize; j++)
                    for (int k=0; k<responseSize; k++) {
                        const MatrixXf sample = getSample(registered, j, k, kernelSize);
                        float posterior = 1;
                        foreach (const DecisionBoundary &db, decisionBoundaries) {
                            const float response = db.function.transpose() * Map<const VectorXf>(sample.data(), kernelSize*kernelSize);
                            const float positiveDensity = 1 / (sqrtf(2 * CV_PI) * db.positiveSD) * expf(-0.5 * powf((response - db.positiveMean) / db.positiveSD, 2.f));
                            const float negativeDensity = 1 / (sqrtf(2 * CV_PI) * db.negativeSD) * expf(-0.5 * powf((response - db.negativeMean) / db.negativeSD, 2.f));
                            posterior *= positiveDensity / (positiveDensity + negativeDensity);
                        }
                        if (posterior > bestResponse) {
                            bestResponse = posterior;
                            bestX = k + kernelSize/2;
                            bestY = j + kernelSize/2;
                            bestRotation = rotation;
                            bestScale = scale;
                        }
                    }
            }
        }
    }

    void store(QDataStream &stream) const
    {
        stream << rotationMax << scaleMin << scaleMax << xTranslationMax << yTranslationMin << yTranslationMax << decisionBoundaries;
    }

    void load(QDataStream &stream)
    {
        stream >> rotationMax >> scaleMin >> scaleMax >> xTranslationMax >> yTranslationMin >> yTranslationMax >> decisionBoundaries;
    }
};

BR_REGISTER(Transform, BBLDAAlignmentTransform)

} // namespace br

#include "classification/lda.moc"


//---------------------------------------------------------------------------//
//!
//! \file   fges_sab_gen.cpp
//! \author Alex Robinson
//! \brief  Free gas elastic scattering S(alpha,beta) table generator
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>
#include <Teuchos_CommandLineProcessor.hpp>
#include <Teuchos_FancyOStream.hpp>
#include <Teuchos_VerboseObject.hpp>

// FRENSIE Includes
#include "Utility_BilinearGridGenerator.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_UniformDistribution.hpp"
#include "Utility_KinematicHelpers.hpp"
#include "MonteCarlo_NuclearScatteringAngularDistribution.hpp"
#include "DataGen_FreeGasElasticSAlphaBetaFunction.hpp"

int main( int argc, char** argv )
{
  Teuchos::RCP<Teuchos::FancyOStream> out =
    Teuchos::VerboseObjectBase::getDefaultOStream();

  double energy = 2.5e-8; // MeV
  double atomic_weight_ratio = 0.999167;
  double temperature = 2.5301e-8; // MeV
  double convergence_tol = 0.0001;

  // Set up the command line options
  Teuchos::CommandLineProcessor sab_clp;

  sab_clp.setDocString( "Free gas elastic scattering S(alpha,beta,E) table "
			"generator\n" );
  sab_clp.setOption( "energy",
		     &energy,
		     "Energy at which table will be generated (MeV)",
		     true );
  sab_clp.setOption( "atomic_weight_ratio",
		     &atomic_weight_ratio,
		     "Atomic weight ratio of scatterer",
		     true );
  sab_clp.setOption( "temperature",
		     &temperature,
		     "Temperature at which the table will be generated (MeV)",
		     true );
  sab_clp.setOption( "convergence_tol",
		     &convergence_tol,
		     "Tolerance used to refine grid points" );

  sab_clp.throwExceptions( false );

  // Parse the command line
  Teuchos::CommandLineProcessor::EParseCommandLineReturn
    parse_return = sab_clp.parse( argc, argv );

  if( parse_return != Teuchos::CommandLineProcessor::PARSE_SUCCESSFUL )
  {
    sab_clp.printHelpMessage( argv[0], *out );

    return parse_return;
  }

  // Create the S(alpha,beta,E) function
  // Initialize the zero temperature cross section
  Teuchos::RCP<Utility::OneDDistribution> cross_section(
			  new Utility::UniformDistribution( 0.0, 20.0, 1.0 ) );

  // Initialize the scattering probability distribution
  Teuchos::RCP<Utility::OneDDistribution> isotropic_distribution(
			  new Utility::UniformDistribution( -1.0, 1.0, 0.5 ) );

  // Initialize the scattering distribution
  MonteCarlo::NuclearScatteringAngularDistribution::AngularDistribution
    distribution( 2 );

  distribution[0].first = 0.0;
  distribution[0].second = isotropic_distribution;

  distribution[1].first = 20.0;
  distribution[1].second = isotropic_distribution;

  Teuchos::RCP<MonteCarlo::NuclearScatteringAngularDistribution>
    scattering_distribution(
			 new MonteCarlo::NuclearScatteringAngularDistribution(
							      distribution ) );

  // Create the S(alpha,beta,E) function object
  DataGen::FreeGasElasticSAlphaBetaFunction sab_function(
						       cross_section,
						       scattering_distribution,
						       atomic_weight_ratio,
						       temperature );

  // Create the function wrappers
  boost::function<double (double beta, double alpha)> sab_function_wrapper =
    boost::bind<double>( boost::ref(sab_function), _2, _1, energy );

  boost::function<double (double beta)> alpha_min_wrapper =
    boost::bind<double>( Utility::calculateAlphaMin,
			 energy,
			 _1,
			 atomic_weight_ratio,
			 temperature );

  boost::function<double (double beta)> alpha_max_wrapper =
    boost::bind<double>( Utility::calculateAlphaMax,
			 energy,
			 _1,
			 atomic_weight_ratio,
			 temperature );

  // Create the bilinear grid generator
  Utility::BilinearGridGenerator sab_grid_generator( sab_function_wrapper,
						     alpha_min_wrapper,
						     alpha_max_wrapper );

  double beta_min = Utility::calculateBetaMin( energy, temperature );

  Teuchos::Array<double> initial_beta_grid( 2 );
  initial_beta_grid[0] = beta_min;
  initial_beta_grid[1] = -beta_min;

  Teuchos::Array<double> linear_beta_grid;
  Teuchos::Array<Teuchos::Array<double> > linear_alpha_grids;

  // Generate the alpha and beta grids
  *out << "Generating alpha and beta grids...";
  out->flush();

  sab_grid_generator.generate( linear_beta_grid,
			       linear_alpha_grids,
			       initial_beta_grid,
			       convergence_tol,
			       1e-21,
			       1e-10 );

  *out << "done!" << std::endl << std::endl;

  out->precision( 20 );

  // Print out the beta grid
  *out << "Beta Grid: ";

  for( unsigned i = 0; i < linear_beta_grid.size(); ++i )
    *out << linear_beta_grid[i] << " ";
  *out << std::endl;

  // Print out the alpha grids and the S(alpha,beta,E) function values
  for( unsigned i = 0; i < linear_beta_grid.size(); ++i )
  {
    *out << "Alpha Grid For Beta Index: " << i << std::endl;
    for( unsigned j = 0; j < linear_alpha_grids[i].size(); ++j )
      *out << linear_alpha_grids[i][j] << " ";
    *out << std::endl;

    for( unsigned j = 0; j < linear_alpha_grids[i].size(); ++j )
    {
      *out << sab_function( linear_alpha_grids[i][j],
			    linear_beta_grid[i],
			    energy )
	   << " ";
    }
    *out << std::endl;
  }
}

//---------------------------------------------------------------------------//
// end fges_sab_gen.cpp
//---------------------------------------------------------------------------//


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_STRAIGHT_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/rem_pio2_straight.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_straight_, tag::cpu_,
			     (A0),
			     (scalar_ < floating_<A0> > )
			     )
  {
    typedef boost::fusion::tuple<A0,A0,nt2::int32_t>           result_type;
    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
      nt2::rem_pio2_straight(a0,
                             boost::fusion::at_c<0>(res),
                             boost::fusion::at_c<1>(res)
                            );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_straight_, tag::cpu_,
			     (A0),
			     (scalar_< floating_<A0> >)
			     (scalar_< floating_<A0> >)
			     (scalar_< floating_<A0> >)
			     )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& x, A0 & xr, A0& xc) const
    {
      xr = x-Pio2_1<A0>();
      xr -= Pio2_2<A0>();
      xr -= Pio2_3<A0>();
      xc = nt2::Zero<A0>();
      return  nt2::One<result_type>();
    }
  };
} }
#endif


#include "world.h"

#include <armadillo>
#include <iostream>
#include <fstream>

namespace game {

void
World::generate_landscape(){
  arma::arma_rng::set_seed_random();
  this->landscape = arma::zeros(2, 2 * this->n_points);
  this->slopes = arma::zeros(this->n_points-1);

  float best_slope = 90.;

  for(int i = 0; i < this->landscape.n_cols; i+=2){
    int top = i;
    int bottom = top + 1;

    //x at the same position
    this->landscape.col(top).row(0) = top * 1800./this->n_points;
    this->landscape.col(bottom).row(0) = top * 1800./this->n_points;

    //y at a random position, top and bottom 50 pixels apart
    const auto y_pos = 1500 + 100 * (2*arma::randu()-1);
    this->landscape.col(top).row(1) = y_pos - 50;
    this->landscape.col(bottom).row(1) = 1800;

    if (i > 0 && i < 52){
      const arma::vec left = this->landscape.col(i-2);
      const arma::vec right = this->landscape.col(i);

      const auto dx = right(0) - left(0);
      const auto dy = right(1) - left(1);
      const auto hyp = std::pow(std::pow(dx, 2) + std::pow(dy, 2), 0.5);

      auto current_slope = std::asin(dy/hyp) * 180/arma::datum::pi;
      this->slopes(i-2) = current_slope;

      if(std::abs(current_slope) < best_slope){
        best_slope = std::abs(current_slope);
        best_lz = arma::join_horiz(left, this->landscape.col(i-1), right, this->landscape.col(i+1));
        best_lz_pos.x = 0.5 * (left.at(0, 0) + right.at(0,0));
        best_lz_pos.y = 0.5 * (left.at(1,0) + right.at(1,0));

      }

    }
  }
}

sf::VertexArray
to_vertices(
    const arma::mat &landscape,
    const sf::Color color = sf::Color::Black
    ){

  sf::VertexArray vertices(sf::TriangleStrip, landscape.n_cols);
  for(int i = 0; i < vertices.getVertexCount(); i++){
    vertices[i].position = {float(landscape.col(i)(0)), float(landscape.col(i)(1))};
    vertices[i].color = color;
  }

  return vertices;
}

std::pair<arma::mat, float>
World::get_lz_and_slope(){

  const auto x_pos = this->rocket.getPosition().x;

  const auto index = 2*floor(n_points/2. * x_pos/1800);

  return {this->landscape.cols(index, index+3), this->slopes(index)};

}

sf::Vector2f
World::get_lowest_point(){

  sf::Vector2f result = {0., 0.};

  const std::vector<sf::Vector2f> y_points = {
    this->rocket.getTransform().transformPoint(0, 0),
    this->rocket.getTransform().transformPoint(150, 0),
    this->rocket.getTransform().transformPoint(0, 150),
    this->rocket.getTransform().transformPoint(150, 150)
  };

  for(const auto & y_point : y_points){
    if(y_point.y > result.y){
      result = y_point;
    }
  }

  return result;

}

void
World::check_landed(){

  const auto [lz, slope] = this->get_lz_and_slope();
  const auto lowest_point = this->get_lowest_point();

  const auto world_y = this->best_lz_pos.y;

  this->landed = (world_y < lowest_point.y);
  this->distance_to_ground = std::max(world_y - lowest_point.y, 0.f);

  const auto vel = this->rocket.velocity;
  const auto speed = std::sqrt(std::pow(vel.x, 2) + std::pow(vel.y, 2));
  this->under_safe_speed = (speed < this->safe_speed);

  const auto tilt = this->rocket.getRotation() < 180
                        ? this->rocket.getRotation()
                        : -(360 - this->rocket.getRotation());

  const auto tilt_diff = std::abs(slope - tilt);
  this->under_safe_tilt = tilt_diff < this->safe_tilt;

  const auto x_diff =
      std::abs(this->rocket.getPosition().x - this->best_lz_pos.x);

  this->over_lz = (x_diff < 1800./this->n_points);
}

void
World::reinitialize() {
  this->landed = false;
  this->log = "";
  this->current_time = this->clock.getElapsedTime().asSeconds();

  this->n_points = 50;
  this->generate_landscape();

  this->rocket = Rocket();
  this->rocket.scale(0.4, 0.4);
  this->rocket.setTexture(this->lander_texture);

  this->written = false;
}

void
World::print_info(
    sf::RenderWindow &window
    ){

  sf::Font font;
  font.loadFromFile("fonts/Courier_New_Bold.ttf");

  const auto make_text = [&font](const std::string &str, const std::string &val) -> sf::Text{
    sf::Text result;
    result.setFont(font);
    result.setString(str + " : " + val);
    result.setFillColor(sf::Color::Black);
    return result;
  };

  const auto [lz, slope_val] = this->get_lz_and_slope();

  const auto tilt_val = this->rocket.getRotation() < 180
                        ? this->rocket.getRotation()
                        : -(360 - this->rocket.getRotation());

  const auto x_diff_val =this->rocket.getPosition().x - this->best_lz_pos.x;

  auto altitude   = make_text("altitude", std::to_string(this->distance_to_ground));
  auto x_diff     = make_text("distance", std::to_string(x_diff_val));
  auto x_velocity = make_text("x m/s", std::to_string(this->rocket.velocity.x));
  auto y_velocity = make_text("y m/s", std::to_string(this->rocket.velocity.y));
  auto slope      = make_text("slope", std::to_string(slope_val));
  auto tilt       = make_text("tilt", std::to_string(tilt_val));
  auto rotation   = make_text("rotation", std::to_string(this->rocket.rotation));

  altitude.setPosition((sf::Vector2f{10., 0.}));
  x_diff.setPosition(sf::Vector2f{10, 35.});
  x_velocity.setPosition(sf::Vector2f{10., 70.});
  y_velocity.setPosition(sf::Vector2f{10., 105.});
  slope.setPosition(sf::Vector2f{10., 140.});
  tilt.setPosition(sf::Vector2f{10., 175.});
  rotation.setPosition(sf::Vector2f{10., 210.});

  if(this->under_safe_speed){
    x_velocity.setFillColor(sf::Color::Green);
    y_velocity.setFillColor(sf::Color::Green);
  } else {
    x_velocity.setFillColor(sf::Color::Red);
    y_velocity.setFillColor(sf::Color::Red);
  }

  if(this->under_safe_tilt){
    tilt.setFillColor(sf::Color::Green);
  } else {
    tilt.setFillColor(sf::Color::Red);
  }

  if(this->over_lz){
    x_diff.setFillColor(sf::Color::Green);
  } else {
    x_diff.setFillColor(sf::Color::Red);
  }

  std::string instance_log = "";

  if (!this->landed) {
    instance_log += std::to_string(this->distance_to_ground) + " ";
    instance_log += std::to_string(x_diff_val) + " ";
    instance_log += std::to_string(this->over_lz) + " ";
    instance_log += std::to_string(this->rocket.velocity.x) + " ";
    instance_log += std::to_string(this->rocket.velocity.y) + " ";
    instance_log += std::to_string(this->under_safe_speed) + " ";
    instance_log += std::to_string(tilt_val) + " ";
    instance_log += std::to_string(this->under_safe_tilt) + " ";
    instance_log += std::to_string(this->rocket.rotation) + " ";
    instance_log += std::to_string(this->time_since_last_thrust) + " ";
    instance_log += std::to_string(this->time_since_last_turn) + " ";
    instance_log += std::to_string(sf::Keyboard::isKeyPressed(sf::Keyboard::W)) + " ";
    instance_log += std::to_string(sf::Keyboard::isKeyPressed(sf::Keyboard::A)) + " ";
    instance_log += std::to_string(sf::Keyboard::isKeyPressed(sf::Keyboard::D)) + "\n";
    this->log += instance_log;
  }

  if (this->landed &&
      !this->written &&
      this->under_safe_tilt &&
      this->under_safe_speed &&
      this->over_lz) {

    this->written = true;

    std::ofstream log_file;
    log_file.open("NN_training/data.txt", std::ios_base::app);
    log_file << this->log;
    log_file.close();

  }

  window.draw(altitude);
  window.draw(x_diff);
  window.draw(x_velocity);
  window.draw(y_velocity);
  window.draw(slope);
  window.draw(tilt);
  window.draw(rotation);

}

void
World::update(
    sf::RenderWindow & window,
    sf::Sound & engine_sound
    ) {

  const float dt = this->clock.getElapsedTime().asSeconds() - this->current_time;
  this->current_time = this->clock.getElapsedTime().asSeconds();

  const auto [lz_mat, slope] = this->get_lz_and_slope();
  const auto lz = to_vertices(lz_mat, sf::Color::Red);
  this->check_landed();

  if(!this->landed){
    const auto turned = this->rocket.turn(dt);
    this->time_since_last_turn = turned ? 0. : this->time_since_last_turn + dt;
    const auto thrust = this->rocket.accelerate(engine_sound);
    this->time_since_last_thrust = thrust ? 0. : this->time_since_last_thrust + dt;

    this->rocket.update_velocity(dt);
    this->rocket.update_position(dt);
  }

  window.draw(to_vertices(this->landscape));
  window.draw(lz);
  window.draw(to_vertices(this->best_lz, sf::Color::Green));

  sf::CircleShape lz_point(20., 4);
  lz_point.setOrigin(20., 20.);
  lz_point.setPosition(this->best_lz_pos);
  lz_point.setFillColor(sf::Color::Red);

  window.draw(lz_point);
  window.draw(this->rocket);

}

World::World(){

    this->lander_texture.loadFromFile("images/lander.png");
    this->current_time = this->clock.getElapsedTime().asSeconds();

    this->n_points = 50;
    this->generate_landscape();

    this->rocket = Rocket();
    this->rocket.scale(0.4, 0.4);
    this->rocket.setTexture(this->lander_texture);

    this->landed = false;

};

}


#include <iostream>
#include <Eigen/Eigen>
#include <HDF5pp.h>

// alias row-major types for <Eigen/Dense>
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatD;

int main()
{
  // open empty file
  H5p::File file = H5p::File("example.hdf5", "w");

  // some example data
  MatD data(2,2);
  data(0,0) = 0.1;
  data(0,1) = 1.1;
  data(1,0) = 2.1;
  data(1,1) = 3.1;

  // write data
  file.write("/data", data);

  // read data
  MatD read_data = file.read<MatD>("/data");

  // print read data
  std::cout << read_data << std::endl;

  return 0;
}


/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */
#include <iostream>
#include <vector>
#include <iomanip>
#include <nlopt.hpp>

#include <Eigen/Dense>
#include <random>
#include "timer.hpp"

#include "solver_gurobi.hpp"
#include "solver_params.hpp"
#include "termcolor.hpp"

#include <fstream>

using namespace termcolor;

typedef PANTHER_timers::Timer MyTimer;

ConvexHullsOfCurve createStaticObstacle(double x, double y, double z, int num_seg, double bbox_x, double bbox_y,
                                        double bbox_z)
{
  ConvexHullsOfCurve hulls_curve;
  std::vector<Point_3> points;

  points.push_back(Point_3(x - bbox_x / 2.0, y - bbox_y / 2.0, z - bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y - bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y + bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x - bbox_x / 2.0, y + bbox_y / 2.0, z - bbox_z / 2.0));

  points.push_back(Point_3(x + bbox_x / 2.0, y + bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y + bbox_y / 2.0, z - bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y - bbox_y / 2.0, z + bbox_z / 2.0));
  points.push_back(Point_3(x + bbox_x / 2.0, y - bbox_y / 2.0, z - bbox_z / 2.0));

  CGAL_Polyhedron_3 hull_interval = convexHullOfPoints(points);

  for (int i = 0; i < num_seg; i++)
  {
    hulls_curve.push_back(hull_interval);  // static obstacle
  }

  return hulls_curve;
}

int main()
{
  double bbox_x = 0.4;
  double bbox_y = 0.4;
  double bbox_z = 0.4;
  int num_seg = 4;

  ConvexHullsOfCurve hulls_curve = createStaticObstacle(0.0, 0.0, bbox_z / 2.0, num_seg, bbox_x, bbox_y, bbox_z);
  ConvexHullsOfCurves hulls_curves;
  hulls_curves.push_back(hulls_curve);
  ConvexHullsOfCurves_Std hulls_std = vectorGCALPol2vectorStdEigen(hulls_curves);

  par_solver parameters;
  parameters.v_max = 2 * Eigen::Vector3d::Ones();
  parameters.a_max = 2 * Eigen::Vector3d::Ones();
  parameters.dc = 0.01;
  parameters.dist_to_use_straight_guess = 1;
  parameters.a_star_samp_x = 5;
  parameters.a_star_samp_y = 5;
  parameters.a_star_samp_z = 5;
  parameters.a_star_fraction_voxel_size = 0.5;
  parameters.num_seg = num_seg;
  parameters.deg_pos = 3;
  parameters.weight = 1.0;
  parameters.epsilon_tol_constraints = 0.001;
  parameters.xtol_rel = 0.0000000000001;
  parameters.ftol_rel = 0.0000000000001;
  parameters.solver = "LD_MMA";
  parameters.basis = "B_SPLINE";
  parameters.a_star_bias = 1.0;
  parameters.allow_infeasible_guess = true;
  parameters.Ra = 4.0;

  parameters.alpha_shrink = 0.95;

  SolverGurobi my_solver(parameters);  // my_solver(a,g) a polynomials of degree 3
  my_solver.setMaxRuntimeKappaAndMu(0.2, 0.5, 0.5);
  mt::state initial_state;
  initial_state.pos = Eigen::Vector3d(-4.0, 0.0, 0.0);

  mt::state final_state;
  final_state.pos = Eigen::Vector3d(4.0, 0.0, 0.0);

  double t_min = 0.0;
  double t_max = t_min + (final_state.pos - initial_state.pos).norm() / (0.3 * parameters.v_max(0));

  std::cout << green << "t_final" << t_min << reset << std::endl;
  std::cout << green << "t_init_" << t_max << reset << std::endl;

  my_solver.setInitStateFinalStateInitTFinalT(initial_state, final_state, t_min, t_max);

  my_solver.setHulls(hulls_std);

  std::cout << "Calling optimize" << std::endl;
  bool converged = my_solver.optimize();

  // double time_needed = my_solver.getTimeNeeded();
  double delta = (t_max - t_min) / num_seg;
}
// std::ofstream myfile;
// myfile.open("/home/jtorde/Desktop/ws/src/panther/panther/src/solvers/nlopt/example.txt");

// double tmp = 8.0;
//  for (double tmp = 3; tmp < 50; tmp = tmp + 0.05)
// {

// if (converged)
// {
//   myfile << num_seg << ", " << delta << ", " << time_needed << std::endl;
// }
// //  }
// myfile.close();


#include <modern/lib.hpp>

#include <tuple>
#include <vector>
#include <algorithm>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>

namespace ba = boost::accumulators;

std::tuple<double, double> accumulate_vector(const std::vector<double>& values) {

    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::moment<2>>> acc;

    std::for_each(std::begin(values), std::end(values), std::ref(acc));

    return {ba::mean(acc), ba::moment<2>(acc)};
}


/*
 * Copyright 2017-, Rohan Budhirja, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <Eigen/Core>

class CausalFilter
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** --- CONSTRUCTOR ---- */
  CausalFilter(const double &timestep,
               const int& xSize,
               const Eigen::VectorXd& filter_numerator,
               const Eigen::VectorXd& filter_denominator);

  void get_x_dx_ddx(const Eigen::VectorXd& base_x,
                Eigen::VectorXd& x_output_dx_ddx);

  void switch_filter(const Eigen::VectorXd& filter_numerator,
                     const Eigen::VectorXd& filter_denominator);

private:
  double m_dt;      /// sampling timestep of the input signal
  int m_x_size;
  Eigen::VectorXd::Index m_filter_order_m;
  Eigen::VectorXd::Index m_filter_order_n;

  Eigen::VectorXd m_filter_numerator;
  Eigen::VectorXd m_filter_denominator;
  bool first_sample;
  int pt_numerator;
  int pt_denominator;
  Eigen::MatrixXd input_buffer;
  Eigen::MatrixXd output_buffer;
}; // class CausalFilter


#define DEBUG 1
/**
 * File    : E2.cpp
 * Author  : Kazune Takahashi
 * Created : 12/7/2019, 4:55:25 PM
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

class SuffixArray
{
public:
  // Sparse Table from drken-san
  // http://drken1215.hatenablog.com/entry/2019/09/16/014600
  template <class T = int>
  class SparseTable
  {
    vector<vector<T>> dat;
    vector<int> height;

  public:
    SparseTable() {}
    SparseTable(vector<T> const &V)
    {
      int N{static_cast<int>(V.size())}, h{0};
      while ((1 << h) < N)
      {
        ++h;
      }
      dat.assign(h, vector<T>(1 << h));
      height.assign(N + 1, 0);
      for (int i = 2; i <= N; i++)
      {
        height[i] = height[i >> 1] + 1;
      }
      for (int i = 0; i < N; ++i)
      {
        dat[0][i] = V[i];
      }
      for (int i = 1; i < h; ++i)
      {
        for (int j = 0; j < N; ++j)
        {
          dat[i][j] = min(dat[i - 1][j], dat[i - 1][min(j + (1 << (i - 1)), N - 1)]);
        }
      }
    }

    T get(int a, int b) const
    {
      return min(dat[height[b - a]][a], dat[height[b - a]][b - (1 << height[b - a])]);
    }
  };

private:
  int N, K;
  string S;
  vector<int> rank, sa, lcp;
  SparseTable<int> st;

public:
  SuffixArray() {}
  SuffixArray(string S) : N{static_cast<int>(S.size())}, K{1}, S{S}, rank(N + 1), sa(N + 1), lcp(N)
  {
    // initialize for 1 char
    for (auto i = 0; i <= N; i++)
    {
      sa[i] = i;
    }
    for (auto i = 0; i < N; i++)
    {
      rank[i] = static_cast<int>(S[i]);
    }
    rank[N] = -1;
    // lambda for comparing
    auto compare_sa = [&](auto i, auto j) {
      if (rank[i] != rank[j])
      {
        return rank[i] < rank[j];
      }
      else
      {
        int ri{i + K <= N ? rank[i + K] : -1};
        int rj{j + K <= N ? rank[j + K] : -1};
        return ri < rj;
      }
    };
    // construct sa with rank
    vector<int> tmp(N + 1);
    for (; K <= N; K *= 2)
    {
      sort(sa.begin(), sa.end(), compare_sa);
      tmp[sa[0]] = 0;
      for (auto i = 1; i <= N; i++)
      {
        tmp[sa[i]] = tmp[sa[i - 1]] + (compare_sa(sa[i - 1], sa[i]) ? 1 : 0);
      }
      swap(rank, tmp);
    }
    // construct lcp with sa and rank
    for (auto i = 0; i <= N; i++)
    {
      rank[sa[i]] = i;
    }
    int h{0};
    lcp[0] = 0;
    for (auto i = 0; i < N; i++)
    {
      int j{sa[rank[i] - 1]};
      if (h > 0)
      {
        --h;
      }
      for (; j + h < N && i + h < N; ++h)
      {
        if (S[j + h] != S[i + h])
        {
          break;
        }
      }
      lcp[rank[i] - 1] = h;
    }
    st = SparseTable<int>(lcp);
  }

  int operator[](int i) const
  {
    return sa[i];
  }

  vector<int> const &LCP() const
  {
    return lcp;
  }

  int LCP(int a, int b) const
  {
    return st.get(min(rank[a], rank[b]), max(rank[a], rank[b]));
  }

  bool contain(string const &T) const
  {
    int b{lower_bound(T)};
    return b <= N && S.compare(sa[b], T.size(), T) == 0;
  }

  int count(string const &T) const
  {
    int lb{lower_bound(T)};
    int ub{upper_bound(T)};
    if (lb > ub)
    {
      return 0;
    }
    return ub - lb + 1;
  }

private:
  template <typename T>
  void binary_search(int &a, int &b, T cmp) const
  {
    while (abs(a - b) > 1)
    {
      int c{(a + b) / 2};
      if (cmp(c))
      {
        a = c;
      }
      else
      {
        b = c;
      }
    }
  }

  int lower_bound(string const &T) const
  {
    int a{-1}, b{N + 1};
    auto cmp = [&](auto c) {
      return S.compare(sa[c], T.size(), T) < 0;
    };
    binary_search(a, b, cmp);
    return b;
  }

  int upper_bound(string const &T) const
  {
    int a{-1}, b{N + 1};
    auto cmp = [&](auto c) {
      return S.compare(sa[c], T.size(), T) <= 0;
    };
    binary_search(a, b, cmp);
    return a;
  }
};

// ----- main() -----
int main()
{
  int N;
  string S;
  cin >> N >> S;
  SuffixArray sa(S);
  int ans{0};
  for (auto i = 0; i < N; i++)
  {
    for (auto j = i + 1; j < N; j++)
    {
      int t{min(sa.LCP(i, j), j - i)};
      ch_max(ans, t);
    }
  }
  cout << ans << endl;
}


// Copyright (C) 2009-2012 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0
// (see accompanying file LICENSE_1_0.txt or a copy at
// http://www.boost.org/LICENSE_1_0.txt)
// Home at http://www.boost.org/libs/local_function

#include <boost/local_function.hpp>
#include <boost/detail/lightweight_test.hpp>

int main(void) {
    int BOOST_LOCAL_FUNCTION( (int x) (int y) ) {
        return x + y;
    } BOOST_LOCAL_FUNCTION_NAME(add)

    BOOST_TEST(add(1, 2) == 3);
    return boost::report_errors();
}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_dae_pph.hpp>
#include <test/unit/math/rev/functor/dae_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using pph_test_types = boost::mp11::mp_product<
    ode_test_tuple, ::testing::Types<dae_functor>,
    ::testing::Types<double>,                                 // t
    ::testing::Types<double, stan::math::var_value<double>>,  // yy0
    ::testing::Types<double, stan::math::var_value<double>>,  // yp0
    ::testing::Types<stan::math::var_value<double>>           // theta
    >;

TYPED_TEST_SUITE_P(pph_dae_test);
TYPED_TEST_P(pph_dae_test, param_finite_diff) {
  const double h = 0.01;

  // after theta perturbation we need to make IC consistent
  this->theta += h;
  this->yy0[2] = stan::math::value_of(this->theta);
  this->yp0[1] = 1.0 - stan::math::value_of(this->yy0[2]);
  auto res_ub = this->apply_solver();

  // after theta perturbation we need to make IC consistent
  this->theta -= 2.0 * h;
  this->yy0[2] = stan::math::value_of(this->theta);
  this->yp0[1] = 1.0 - stan::math::value_of(this->yy0[2]);
  auto res_lb = this->apply_solver();

  const size_t nt = this->times().size();
  std::vector<Eigen::Matrix<stan::math::var, -1, 1>> grad_fd(nt);
  for (size_t i = 0; i < nt; ++i) {
    grad_fd[i] = (res_ub[i] - res_lb[i]) / (2.0 * h);
  }

  this->theta += h;
  this->yy0[2] = stan::math::value_of(this->theta);
  this->yp0[1] = 1.0 - stan::math::value_of(this->yy0[2]);
  auto res = this->apply_solver();
  std::vector<double> g;
  std::vector<stan::math::var> vec{this->theta};

  for (size_t i = 0; i < nt; i++) {
    for (size_t j = 0; j < res[0].size(); j++) {
      g.clear();
      res[i][j].grad(vec, g);

      double tol = 1.e-5;
      if (j == 2)
        tol = 2.e-5;
      double g_fd = stan::math::value_of(grad_fd[i][j]);

      EXPECT_NEAR(g[0], g_fd, tol)
          << "Gradient of DAE solver failed with initial positions"
          << " known and parameters unknown at time index " << i
          << ", equation index " << j;
      stan::math::set_zero_all_adjoints();
    }
  }
}

REGISTER_TYPED_TEST_SUITE_P(pph_dae_test, param_finite_diff);
INSTANTIATE_TYPED_TEST_SUITE_P(StanOde, pph_dae_test, pph_test_types);


#ifndef SKYLARK_ML_MODEL_HPP
#define SKYLARK_ML_MODEL_HPP

#include <elemental.hpp>
#include <skylark.hpp>
#include <cmath>
#include <boost/mpi.hpp>
#include <sstream>
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <string>
#include <vector>
#include "kernels.hpp"
#include "options.hpp"

#ifdef SKYLARK_HAVE_OPENMP
#include <omp.h>
#endif

namespace skylark { namespace ml {

int classification_accuracy(elem::Matrix<double>& Yt, elem::Matrix<double>& Yp) {
    int correct = 0;
    double o, o1;
    int pred;

    for(int i=0; i < Yp.Height(); i++) {
        o = Yp.Get(i,0);
        pred = 0;
        if (Yp.Width()==1)
            pred = (o >= 0)? +1:-1;

        for(int j=1; j < Yp.Width(); j++) {
            o1 = Yp.Get(i,j);
            if ( o1 > o) {
                o = o1;
                pred = j;
            }
        }

        if(pred == (int) Yt.Get(i,0))
            correct++;
    }
    return correct;
}

template <typename InputType, typename OutputType>
struct model_t
{
public:
    typedef InputType input_type;
    typedef OutputType output_type;

    // TODO the following two should depend on the input type
    // TODO explicit doubles is not desired.
    typedef elem::Matrix<double> intermediate_type;
    typedef elem::Matrix<double> coef_type;

    typedef skylark::sketch::sketch_transform_t<input_type, intermediate_type>
    feature_transform_type;

    model_t(std::vector<const feature_transform_type *>& maps, bool scale_maps,
        int num_features, int num_outputs) :
        _coef(num_features, num_outputs), _maps(maps), _scale_maps(scale_maps),
        _starts(maps.size()), _finishes(maps.size()) {

        // TODO verify all N dimension of the maps match

        elem::MakeZeros(_coef);

        int nf = 0;
        for(int i = 0; i < _maps.size(); i++) {
            _starts[i] = nf;
            _finishes[i] = nf + _maps[i]->get_S() - 1;
            nf += _maps[i]->get_S();
        }

        _num_input_features = (_maps.size() == 0) ?
            num_features : _maps[0]->get_N();
    }

    model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    model_t(const std::string& fname) {
        std::ifstream is(fname);

        // Skip all lines begining with "#"
        while(is.peek() == '#')
            is.ignore(std::numeric_limits<std::streamsize>::max(), '\n');

        boost::property_tree::ptree pt;
        boost::property_tree::read_json(is, pt);
        is.close();
        build_from_ptree(pt);
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;
        pt.put("skylark_object_type", "model:linear-on-features");
        pt.put("skylark_version", VERSION);

        pt.put("num_features", _coef.Height());
        pt.put("num_outputs", _coef.Width());
        pt.put("num_input_features", _num_input_features);

        boost::property_tree::ptree ptfmap;
        ptfmap.put("number_maps", _maps.size());
        ptfmap.put("scale_maps", _scale_maps);

        boost::property_tree::ptree ptmaps;
        for(int i = 0; i < _maps.size(); i++)
            ptmaps.push_back(std::make_pair(std::to_string(i),
                    _maps[i]->to_ptree()));
        ptfmap.add_child("maps", ptmaps);

        pt.add_child("feature_mapping", ptfmap);

        std::stringstream scoef;
        elem::Print(_coef, "", scoef);
        pt.put("coef_matrix", scoef.str());

        return pt;
    }

    /**
     * Saves the model to a file named fname. You may want to use this method
     * from only a single rank.
     */
    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    void predict(input_type& X, output_type& PV, output_type& DV,
        int num_threads = 1) const {

        int d = base::Height(X);
        int k = base::Width(_coef);
        int n = base::Width(X);

        if (_maps.size() == 0)  {
            // No maps (linear case)

            DV.Resize(n, k);
            base::Gemm(elem::TRANSPOSE,elem::NORMAL,1.0, X, _coef, 0.0, DV);
        } else {
            // Non-linear case

            coef_type Wslice;
            int j, start, finish, sj;

            elem::Zeros(DV, n, k);
#           ifdef SKYLARK_HAVE_OPENMP
#           pragma omp parallel for if(num_threads > 1) private(j, start, finish, sj) num_threads(num_threads)
#           endif
            for(j = 0; j < _maps.size(); j++) {
                start = _starts[j];
                finish = _finishes[j];
                sj = finish - start  + 1;

                intermediate_type z(sj, n);
                _maps[j]->apply(X, z, sketch::columnwise_tag());

                if (_scale_maps)
                    elem::Scal(sqrt(double(sj) / d), z);

                output_type o(n, k);

                elem::LockedView(Wslice, _coef, start, 0, sj, k);
                base::Gemm(elem::TRANSPOSE, elem::NORMAL, 1.0, z, Wslice, o);

#               ifdef SKYLARK_HAVE_OPENMP
#               pragma omp critical
#               endif
                base::Axpy(+1.0, o, DV);
            }
        }

        double o, o1, pred;
        for(int i=0; i < DV.Height(); i++) {
            o = DV.Get(i,0);
            pred = 0;
            if (DV.Width()==1)
                pred = (o >= 0)? +1:-1;

            for(int j=1; j < DV.Width(); j++) {
                o1 = DV.Get(i,j);
                if ( o1 > o) {
                    o = o1;
                    pred = j;
                }
            }

            PV.Set(i,0, pred);
        }
    }

    void get_probabilities(input_type& X, output_type& P, int num_threads = 1)
        const;
    coef_type& get_coef() { return _coef; }
    static double evaluate(output_type& Yt, output_type& Yp,
        const boost::mpi::communicator& comm);

    int get_num_outputs() const { return _coef.Width(); }
    int get_input_size() const { return _num_input_features; }

protected:

    void build_from_ptree(const boost::property_tree::ptree &pt) {
        int num_features = pt.get<int>("num_features");
        int num_outputs = pt.get<int>("num_outputs");
        _coef.Resize(num_features, num_outputs);

        _num_input_features = pt.get<int>("num_input_features");

        int num_maps = pt.get<int>("feature_mapping.number_maps");
        _maps.resize(num_maps);
        const boost::property_tree::ptree &ptmaps =
            pt.get_child("feature_mapping.maps");
        for(int i = 0; i < num_maps; i++)
            _maps[i] =
                feature_transform_type::from_ptree(
                   ptmaps.get_child(std::to_string(i)));

        int nf = 0;
        _starts.resize(num_maps);
        _finishes.resize(num_maps);
        for(int i = 0; i < _maps.size(); i++) {
            _starts[i] = nf;
            _finishes[i] = nf + _maps[i]->get_S() - 1;
            nf += _maps[i]->get_S();
        }

        _scale_maps = pt.get<bool>("feature_mapping.scale_maps");

        std::istringstream coef_str(pt.get<std::string>("coef_matrix"));
        double *buffer = _coef.Buffer();
        int ldim = _coef.LDim();
        for(int i = 0; i < num_features; i++) {
            std::string line;
            std::getline(coef_str, line);
            std::istringstream coefstream(line);
            for(int j = 0; j < num_outputs; j++) {
                std::string token;
                coefstream >> token;
                buffer[i + j * ldim] = atof(token.c_str());
            }
        }
    }

private:
    coef_type _coef;
    int _num_input_features;
    std::vector<const feature_transform_type *> _maps; // TODO use shared_ptr
    bool _scale_maps;

    std::vector<int> _starts, _finishes;
};

template <typename InputType, typename OutputType>
double model_t<InputType, OutputType>::evaluate(OutputType& Yt,
    OutputType& Yp, const boost::mpi::communicator& comm) {

    int rank = comm.rank();

    int correct = classification_accuracy(Yt, Yp);
    double accuracy = 0.0;
    int totalcorrect, total;
    boost::mpi::reduce(comm, correct, totalcorrect, std::plus<double>(), 0);
    boost::mpi::reduce(comm, Yt.Height(), total, std::plus<size_t>(), 0);

    if(rank ==0)
        accuracy =  totalcorrect*100.0/total;
    return accuracy;
}

} }

#endif /* SKYLARK_ML_MODEL_HPP */


/*
 * phase_lattice_mkl.hpp
 *
 * Copyright 2011 Mario Mulansky
 * Copyright 2012 Karsten Ahnert
 *
 * 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)
 */

#ifndef PHASE_LATTICE_MKL_HPP_
#define PHASE_LATTICE_MKL_HPP_

#include <cmath>

#include <mkl_blas.h>
#include <mkl_vml_functions.h>
#include <boost/array.hpp>

template< size_t N >
struct phase_lattice_mkl
{
    typedef double value_type;
    typedef boost::array< value_type , N > state_type;

    value_type m_epsilon;
    state_type m_omega;
    state_type m_tmp;

    phase_lattice_mkl() : m_epsilon( 6.0/(N*N) ) // should be < 8/N^2 to see phase locking
    {
        for( size_t i=1 ; i<N-1 ; ++i )
            m_omega[i] = m_epsilon*(N-i);
    }

    void inline operator()( const state_type &x , state_type &dxdt , const double t )
    {
        const int n = x.size();

        dxdt[0] = m_omega[0] + sin( x[1] - x[0] );

        vdSub( n-1 , &(x[1]) , &(x[0]) , &(m_tmp[0]) );
        vdSin( n-1 , &(m_tmp[0]) , &(m_tmp[0]) );
        vdAdd( n-2 , &(m_tmp[0]) , &(m_tmp[1]) , &(dxdt[1]) );
        vdAdd( n-2 , &(dxdt[1]) , &(m_omega[1]) , &(dxdt[1]) );

        dxdt[N-1] = m_omega[N-1] + sin( x[N-1] - x[N-2] );
    }

};

#endif /* PHASE_LATTICE_MKL_HPP_ */