#include "PCAProjection.h"

#include "datatools/table/TableDataCall.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/IntParam.h"

#include "MDSProjection.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <set>
#include <sstream>

using namespace megamol;
using namespace megamol::infovis;
using namespace Eigen;

MDSProjection::MDSProjection(void)
        : megamol::core::Module()
        , dataOutSlot("dataOut", "Ouput")
        , dataInSlot("dataIn", "Input")
        , reduceToNSlot("nComponents", "Number of components (dimensions) to keep")
        , datahash(0)
        , dataInHash(0)
        , columnInfos() {

    this->dataInSlot.SetCompatibleCall<megamol::datatools::table::TableDataCallDescription>();
    this->MakeSlotAvailable(&this->dataInSlot);

    this->dataOutSlot.SetCallback(megamol::datatools::table::TableDataCall::ClassName(),
        megamol::datatools::table::TableDataCall::FunctionName(0), &MDSProjection::getDataCallback);
    this->dataOutSlot.SetCallback(megamol::datatools::table::TableDataCall::ClassName(),
        megamol::datatools::table::TableDataCall::FunctionName(1), &MDSProjection::getHashCallback);
    this->MakeSlotAvailable(&this->dataOutSlot);

    reduceToNSlot << new ::megamol::core::param::IntParam(2);
    this->MakeSlotAvailable(&reduceToNSlot);
}

MDSProjection::~MDSProjection(void) {
    this->Release();
}

bool MDSProjection::create(void) {
    return true;
}

void MDSProjection::release(void) {}

bool MDSProjection::getDataCallback(core::Call& c) {
    try {
        megamol::datatools::table::TableDataCall* outCall = dynamic_cast<megamol::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL)
            return false;

        megamol::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::datatools::table::TableDataCall>();
        if (inCall == NULL)
            return false;

        inCall->SetFrameID(outCall->GetFrameID());
        if (!(*inCall)())
            return false;

        bool finished = dataProjection(inCall);
        if (finished == false)
            return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);

        // set outCall
        if (this->columnInfos.size() != 0) {
            outCall->Set(this->columnInfos.size(), this->data.size() / this->columnInfos.size(),
                this->columnInfos.data(), this->data.data());
        } else {
            outCall->Set(0, 0, NULL, NULL);
        }

    } catch (...) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("Failed to execute %hs::processData\n"), ClassName());
        return false;
    }

    return true;
}

bool MDSProjection::getHashCallback(core::Call& c) {
    try {
        megamol::datatools::table::TableDataCall* outCall = dynamic_cast<megamol::datatools::table::TableDataCall*>(&c);
        if (outCall == NULL)
            return false;

        megamol::datatools::table::TableDataCall* inCall =
            this->dataInSlot.CallAs<megamol::datatools::table::TableDataCall>();
        if (inCall == NULL)
            return false;

        inCall->SetFrameID(outCall->GetFrameID());
        if (!(*inCall)(1))
            return false;

        outCall->SetFrameCount(inCall->GetFrameCount());
        outCall->SetDataHash(this->datahash);
    } catch (...) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("Failed to execute %hs::getHashCallback\n"), ClassName());
        return false;
    }

    return true;
}

bool megamol::infovis::MDSProjection::dataProjection(megamol::datatools::table::TableDataCall* inCall) {
    // Test if inData has changed and if slots have changed
    if (this->dataInHash == inCall->DataHash()) {
        if (!reduceToNSlot.IsDirty()) {
            return true; // Nothing to do
        }
    }

    auto columnCount = inCall->GetColumnsCount();
    auto column_infos = inCall->GetColumnsInfos();
    auto rowsCount = inCall->GetRowsCount();
    auto inData = inCall->GetData();

    int outputDimCount = this->reduceToNSlot.Param<core::param::IntParam>()->Value();
    if (outputDimCount <= 0 || outputDimCount > columnCount) {
        megamol::core::utility::log::Log::DefaultLog.WriteError(
            _T("%hs: No valid Dimension Count has been given\n"), ClassName());
        return false;
    }

    // Load data in a Matrix
    Eigen::MatrixXd inDataMat = Eigen::MatrixXd(rowsCount, columnCount);
    for (int row = 0; row < rowsCount; row++) {
        for (int col = 0; col < columnCount; col++) {
            inDataMat(row, col) = inData[row * columnCount + col];
        }
    }

    // generate dissimilarity Matrix( squared euclidean Distance matrix)
    Eigen::MatrixXd delta2 = euclideanDissimilarityMatrix(inDataMat).array().pow(2);
    // compute MDS
    Eigen::MatrixXd result = classicMds(delta2, outputDimCount);

    // generate new columns
    this->columnInfos.clear();
    this->columnInfos.resize(outputDimCount);

    for (int indexX = 0; indexX < outputDimCount; indexX++) {
        columnInfos[indexX]
            .SetName("MDS" + std::to_string(indexX))
            .SetType(megamol::datatools::table::TableDataCall::ColumnType::QUANTITATIVE)
            .SetMinimumValue(result.col(indexX).minCoeff())
            .SetMaximumValue(result.col(indexX).maxCoeff());
    }

    // Result Matrix into Output
    this->data.clear();
    this->data.reserve(rowsCount * outputDimCount);

    for (size_t row = 0; row < rowsCount; row++) {
        for (size_t col = 0; col < outputDimCount; col++)
            this->data.push_back(result(row, col));
    }

    this->dataInHash = inCall->DataHash();
    this->datahash++;
    reduceToNSlot.ResetDirty();

    return true;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::euclideanDissimilarityMatrix(Eigen::MatrixXd dataMatrix) {
    // generate euclidean Distance matrix
    int rowsCount = dataMatrix.rows();
    Eigen::MatrixXd distanceMatrix = Eigen::MatrixXd::Zero(rowsCount, rowsCount);
    for (int row = 1; row < rowsCount; row++) {
        for (int col = 0; col < row; col++) {
            double distance = (dataMatrix.row(row) - dataMatrix.row(col)).norm();

            distanceMatrix(row, col) = distance;
            distanceMatrix(col, row) = distance;
        }
    }

    return distanceMatrix;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::classicMds(
    Eigen::MatrixXd squaredDissimilarityMatrix, int outputDimension) {
    // centering matrix

    int rowsCount = squaredDissimilarityMatrix.rows();
    assert(squaredDissimilarityMatrix.rows() == squaredDissimilarityMatrix.cols());

    Eigen::MatrixXd J = Eigen::MatrixXd::Identity(rowsCount, rowsCount) -
                        (1.0 / (double)rowsCount) * Eigen::MatrixXd::Ones(rowsCount, rowsCount);

    // Apply double centering
    Eigen::MatrixXd B = -0.5 * J * squaredDissimilarityMatrix * J;

    // Compute eigenvalues and eigenvectors
    EigenSolver<MatrixXd> eigSolver(B);

    VectorXd eigVal = eigSolver.eigenvalues().real();
    MatrixXd eigVec = eigSolver.eigenvectors().real();

    // Sort eigenvalues (with index): descending
    // Each eigenvalue represents variance
    typedef std::pair<float, int> eigenPair;
    std::vector<eigenPair> sorted;
    for (unsigned int i = 0; i < eigVec.cols(); ++i) {
        sorted.push_back(std::make_pair(eigVal(i), i));
    }
    std::sort(sorted.begin(), sorted.end(), [&sorted](eigenPair& a, eigenPair& b) { return a.first > b.first; });

    // Create Matrix out of sorted (and selected) eigenvectors, with the frobenius norm
    // of an eigenvector beeing the corresponding eigenvalue. lambda = |v|^2
    MatrixXd result = MatrixXd(eigVec.rows(), outputDimension);
    for (int i = 0; i < outputDimension; ++i) {
        result.col(i) = eigVec.col(sorted[i].second) * sqrt(abs(eigVal(sorted[i].second)));
    }

    return result;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::bMatrix(
    Eigen::MatrixXd X, Eigen::MatrixXd W, Eigen::MatrixXd dissimilarityMatrix) {
    assert(X.rows() == W.rows());
    assert(X.rows() == dissimilarityMatrix.rows());
    assert(W.rows() == W.cols());
    assert(dissimilarityMatrix.rows() == dissimilarityMatrix.cols());
    int nPoints = X.rows();

    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(nPoints, nPoints);

    // upper and lower triangle Matrix
    for (int i = 1; i < nPoints; i++) {
        for (int j = 0; j < i; j++) {
            double distance = (X.row(i) - X.row(j)).norm();
            if (distance == 0.0) {
                B(i, j) = 0;
                B(j, i) = 0;
            } else {
                double Bij = -W(i, j) * dissimilarityMatrix(i, j) / distance;
                B(i, j) = Bij;
                B(j, i) = Bij;
            }
        }
    }
    // diagonal
    for (int i = 0; i < nPoints; i++) {
        B(i, i) = -B.row(i).sum();
    }

    return B;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::vMatrix(Eigen::MatrixXd W) {
    assert(W.rows() == W.cols());
    int nPoints = W.rows();

    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(nPoints, nPoints);

    // upper and lower triangle Matrix
    for (int i = 1; i < nPoints; i++) {
        for (int j = 0; j < i; j++) {
            B(i, j) = -W(i, j);
            B(j, i) = -W(i, j);
        }
    }

    // diagonal
    for (int i = 0; i < nPoints; i++) {
        B(i, i) = -W.row(i).sum();
    }

    return B;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::smacofMds(Eigen::MatrixXd dissimilarityMatrix, int outputDimension,
    int countSteps, Eigen::MatrixXd weightsMatrix, double tolerance) {
    assert(dissimilarityMatrix.rows() == dissimilarityMatrix.cols());

    // centering matrix
    int nPoints = dissimilarityMatrix.rows();
    bool weightsAllOne = weightsMatrix(0, 0) == 1 && weightsMatrix.cols() == 1 && weightsMatrix.rows() == 1;
    Eigen::MatrixXd Vp;
    if (!weightsAllOne) {
        Vp = (vMatrix(weightsMatrix) + Eigen::MatrixXd::Ones(nPoints, nPoints)).inverse() -
             pow(nPoints, -2) * Eigen::MatrixXd::Ones(nPoints, nPoints);
    } else {
        weightsMatrix = Eigen::MatrixXd::Ones(nPoints, nPoints);
    }

    // start algortihm
    // initial X
    const bool randomInit = true; // TODO: param
    Eigen::MatrixXd X;
    if (randomInit) {
        srand(1337); // TODO: param
        X = 100.0 * Eigen::MatrixXd::Random(dissimilarityMatrix.rows(), outputDimension);
    } else {
        // Initialize with classic MDS or something else good
        X = classicMds(dissimilarityMatrix.array().square(), outputDimension);
    }

    // stress
    double curStress = stress(dissimilarityMatrix, X, weightsMatrix);
    double oldStress;
    int k;

    for (k = 0; k < countSteps; k++) {
        if (weightsAllOne) {
            Eigen::MatrixXd B = bMatrix(X, weightsMatrix, dissimilarityMatrix);
            X = ((B * X).array()) / (float)nPoints;

        } else {
            X = Vp * bMatrix(X, weightsMatrix, dissimilarityMatrix) * X;
        }
        oldStress = curStress;
        curStress = stress(dissimilarityMatrix, X, weightsMatrix);

        // Early termination
        if (abs(oldStress - curStress) < tolerance) {
            break;
        }
    }

    // megamol::core::utility::log::Log::DefaultLog.WriteInfo("MDS Computation:\n"
    //                                       " Min/Max Weight: %f / %f\n"
    //                                       " Steps: %i\n"
    //                                       " Stress (Delta): %f ( %f )\n",
    //    weightsMatrix.maxCoeff(), weightsMatrix.minCoeff(), k, curStress, oldStress - curStress);

    return X;
}

Eigen::MatrixXd megamol::infovis::MDSProjection::ordinalMds(Eigen::MatrixXd dissimilarityMatrix, int outputDimension,
    int countSteps, Eigen::MatrixXd weightsMatrix, double tolerance) {

    // start algortihm
    // initial X
    bool randomInit = true;
    Eigen::MatrixXd X;
    if (randomInit) {
        srand(1337);                                                                      // same Seed
        X = 100.0 * Eigen::MatrixXd::Random(dissimilarityMatrix.cols(), outputDimension); // random init with the same
    } else {
        X = classicMds(dissimilarityMatrix.array().square(), outputDimension); // or random //or something else good
    }

    // sort each row and generate an Index Matrix

    Eigen::MatrixXd sortedIndexes = Eigen::MatrixXd::Zero(dissimilarityMatrix.rows(), dissimilarityMatrix.cols());

    // row schleife
    for (int row = 0; row < dissimilarityMatrix.rows(); row++) {
        typedef std::pair<float, int> indexValuePair;
        std::vector<indexValuePair> sorted;
        for (unsigned int i = 0; i < dissimilarityMatrix.cols(); ++i) {
            sorted.push_back(std::make_pair(dissimilarityMatrix(row, i), i)); //<-----bei dissmatrix richtige Indexe
        }
        std::sort(sorted.begin(), sorted.end(),
            [&sorted](indexValuePair& a, indexValuePair& b) { return a.first < b.first; });
        for (unsigned int i = 0; i < dissimilarityMatrix.cols(); ++i) {
            sortedIndexes(row, i) = sorted[i].second;
        }
    }

    // start: modified Kruskal-Shepard
    for (int i = 0; i < countSteps; i++) {
        // calculate distances
        Eigen::MatrixXd distances = euclideanDissimilarityMatrix(X);
        Eigen::MatrixXd newDistances = distances;

        for (int row = 0; row < dissimilarityMatrix.rows(); row++) {

            int medianIndex;
            dissimilarityMatrix.row(row).minCoeff(&medianIndex);

            std::set<int> notMonotoncols;

            bool consecutive = false;

            for (int sortedIndex = 0; sortedIndex < dissimilarityMatrix.cols() - 1; sortedIndex++) {

                int dissA = sortedIndexes(row, sortedIndex);
                int dissB = sortedIndexes(row, sortedIndex + 1);

                if (dissA == medianIndex)
                    continue;
                if (dissB == medianIndex)
                    continue;

                bool valueAdded = false;

                if ((dissimilarityMatrix(row, dissA) < dissimilarityMatrix(row, dissB)) &&
                    (distances(row, dissA) > distances(row, dissB))) {
                    valueAdded = true;
                }
                if ((dissimilarityMatrix(row, dissA) > dissimilarityMatrix(row, dissB)) &&
                    (distances(row, dissA) < distances(row, dissB))) {
                    valueAdded = true;
                }
                if ((dissimilarityMatrix(row, dissA) == dissimilarityMatrix(row, dissB)) &&
                    (distances(row, dissA) != distances(row, dissB))) {
                    // valueAdded = true;
                }

                if (valueAdded) {
                    notMonotoncols.insert(dissA);
                    notMonotoncols.insert(dissB);

                    consecutive = true;
                }

                if ((valueAdded == false) && (consecutive == true)) {
                    consecutive = false;

                    if (notMonotoncols.size() != 0) {
                        double avg = 0;
                        for (int i : notMonotoncols) {
                            avg += distances(row, i);
                        }
                        avg /= (double)notMonotoncols.size();

                        for (int i : notMonotoncols)
                            newDistances(row, i) = avg;

                        notMonotoncols.clear();
                    }
                }
            }
        }
        Eigen::MatrixXd Xold = Eigen::MatrixXd::Zero(dissimilarityMatrix.cols(), outputDimension);
        double alpha = 1;

        Xold = X;

        for (size_t i = 0, nRows = distances.rows(), nCols = distances.cols(); i < nRows; ++i)
            for (size_t j = 0; j < nRows; j++) {
                if (i == j)
                    continue;
                X.row(i) = X.row(i).array() + alpha / (dissimilarityMatrix.cols() - 1) *
                                                  (1 - newDistances(i, j) / distances(i, j)) *
                                                  (Xold.row(j).array() - Xold.row(i).array());
            }
    }

    return X;
}

double megamol::infovis::MDSProjection::stress(
    Eigen::MatrixXd dissimilarityMatrix, Eigen::MatrixXd dataPointsMatrix, Eigen::MatrixXd weightsMatrix) {

    assert(dissimilarityMatrix.rows() == dissimilarityMatrix.cols());
    int nPoints = dissimilarityMatrix.rows();
    assert(dissimilarityMatrix.rows() == dataPointsMatrix.rows());
    int dims = dataPointsMatrix.cols();

    if (weightsMatrix(0, 0) == 1 && weightsMatrix.cols() == 1 && weightsMatrix.rows() == 1) {
        weightsMatrix = Eigen::MatrixXd::Ones(nPoints, nPoints);
    }

    double sum = 0;
    for (int j = 1; j < nPoints; j++) {
        for (int i = 0; i < j; i++) {
            double distance = (dataPointsMatrix.row(i) - dataPointsMatrix.row(j)).norm();
            sum += weightsMatrix(i, j) * pow(dissimilarityMatrix(i, j) - distance, 2);
        }
    }

    return sum;
}


#include "Utils.hpp"
#include "SparseLinearSolvers.hpp"

//extern "C" {
//#include <SuiteSparse_config.h>
//#include <umfpack.h>
//#include "cblas.h"
//#include "amd.h"
//#include "colamd.h"
//#include "ldl.h"
//#include "umfpack_get_determinant.h"
//}

#include <iostream>
#include <Eigen/Sparse>
//#include <Eigen/UmfPackSupport>

Eigen::VectorXd solveBICG(
    const Eigen::SparseMatrix<double>& A,
    const Eigen::VectorXd& b
   )
{
  using namespace Eigen;
  BiCGSTAB<SparseMatrix<double>> solver(A);
  return solver.solve(b);
}

Eigen::VectorXd solveLU(
    const Eigen::SparseMatrix<double>& A,
    const Eigen::VectorXd& b
   )
{
  using namespace Eigen;
  SparseLU<SparseMatrix<double>> solver;
  solver.analyzePattern(A);
  solver.factorize(A);
  return solver.solve(b);
}

//Eigen::VectorXd solveUMFLU(
    //const Eigen::SparseMatrix<double>& A,
    //const Eigen::VectorXd& b

   //)
//{
  //using namespace Eigen;
  //UmfPackLU<SparseMatrix<double>> solver;
  //solver.analyzePattern(A);
  //solver.factorize(A);
  //return solver.solve(b);
//}

Eigen::VectorXd solveCG(
    const Eigen::SparseMatrix<double>& A,
    const Eigen::VectorXd& b)
{
  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> cg;
  cg.compute(A);
  return cg.solve(b);
}

Eigen::VectorXd cask::sparse_linear_solvers::EigenSolver::solve(
    const Eigen::SparseMatrix<double>& A,
    const Eigen::VectorXd& b)
{
  return solveBICG(A, b);
}


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
#endif
#include <csim/update_ops_cpp.hpp>

void test_single_control_single_target(std::function<void(UINT,UINT,UINT,const CTYPE*,CTYPE*,ITYPE)> func) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target,control;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit control-1 single qubit gate
        target = rand_int(n);
        control = rand_int(n - 1);
        if (control >= target) control++;
        U = get_eigen_matrix_random_single_qubit_unitary();
        func(control, 1, target, (CTYPE*)U.data(), state, dim);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P0, n) + get_expanded_eigen_matrix_with_identity(control, P1, n)*get_expanded_eigen_matrix_with_identity(target, U, n)) * test_state;
        state_equal(state, test_state, dim, "single qubit control sinlge qubit dense gate");

        // single qubit control-0 single qubit gate
        target = rand_int(n);
        control = rand_int(n - 1);
        if (control >= target) control++;
        U = get_eigen_matrix_random_single_qubit_unitary();
        func(control, 0, target, (CTYPE*)U.data(), state, dim);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P1, n) + get_expanded_eigen_matrix_with_identity(control, P0, n)*get_expanded_eigen_matrix_with_identity(target, U, n)) * test_state;
        state_equal(state, test_state, dim, "single qubit control sinlge qubit dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitControlSingleQubitDenseMatrixTest) {
	test_single_control_single_target(single_qubit_control_single_qubit_dense_matrix_gate);
	test_single_control_single_target(single_qubit_control_single_qubit_dense_matrix_gate_single_unroll);
#ifdef _OPENMP
	test_single_control_single_target(single_qubit_control_single_qubit_dense_matrix_gate_parallel_unroll);
#endif
#ifdef _USE_SIMD
	test_single_control_single_target(single_qubit_control_single_qubit_dense_matrix_gate_single_simd);
#ifdef _OPENMP
	test_single_control_single_target(single_qubit_control_single_qubit_dense_matrix_gate_parallel_simd);
#endif
#endif
}

void test_two_control_single_target(std::function<void(const UINT*, const UINT*, UINT, UINT, const CTYPE*, CTYPE*, ITYPE)> func) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;
    UINT controls[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // two qubit control-10 single qubit gate
        std::random_shuffle(index_list.begin(), index_list.end());
        target = index_list[0];
        controls[0] = index_list[1];
        controls[1] = index_list[2];

        U = get_eigen_matrix_random_single_qubit_unitary();
        UINT mvalues[2] = { 1,0 };
        func(controls, mvalues, 2, target, (CTYPE*)U.data(), state, dim);
        test_state = (
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n)*get_expanded_eigen_matrix_with_identity(target, U, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n)
            ) * test_state;
        state_equal(state, test_state, dim, "two qubit control sinlge qubit dense gate");

    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitControlSingleQubitDenseMatrixTest) {
	test_two_control_single_target(multi_qubit_control_single_qubit_dense_matrix_gate);
	test_two_control_single_target(multi_qubit_control_single_qubit_dense_matrix_gate_single_unroll);
#ifdef _OPENMP
	test_two_control_single_target(multi_qubit_control_single_qubit_dense_matrix_gate_parallel_unroll);
#endif
#ifdef _USE_SIMD
	test_two_control_single_target(multi_qubit_control_single_qubit_dense_matrix_gate_single_simd);
#ifdef _OPENMP
	test_two_control_single_target(multi_qubit_control_single_qubit_dense_matrix_gate_parallel_simd);
#endif
#endif
}

TEST(UpdateTest, SingleQubitControlTwoQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
    Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

    UINT targets[2], control;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit 1-controlled qubit dense matrix gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();
        std::random_shuffle(index_list.begin(), index_list.end());
        targets[0] = index_list[0];
        targets[1] = index_list[1];
        control = index_list[2];

        Umerge = kronecker_product(U2, U);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P0, n) + get_expanded_eigen_matrix_with_identity(control, P1, n)*get_expanded_eigen_matrix_with_identity(targets[1], U2, n) * get_expanded_eigen_matrix_with_identity(targets[0], U, n)) * test_state;
        single_qubit_control_multi_qubit_dense_matrix_gate(control, 1, targets, 2, (CTYPE*)Umerge.data(), state, dim);
        state_equal(state, test_state, dim, "single qubit control two-qubit separable dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitControlTwoQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
    Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

    UINT targets[2], controls[2],mvalues[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {

        // two qubit control-11 two qubit gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();
        std::random_shuffle(index_list.begin(), index_list.end());
        targets[0] = index_list[0];
        targets[1] = index_list[1];
        controls[0] = index_list[2];
        controls[1] = index_list[3];

        mvalues[0] = 1; mvalues[1] = 1;
        Umerge = kronecker_product(U2, U);
        multi_qubit_control_multi_qubit_dense_matrix_gate(controls, mvalues, 2, targets, 2, (CTYPE*)Umerge.data(), state, dim);
        test_state = (
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n)*get_expanded_eigen_matrix_with_identity(targets[0], U, n)*get_expanded_eigen_matrix_with_identity(targets[1], U2, n)
            ) * test_state;
        state_equal(state, test_state, dim, "two qubit control two qubit dense gate");
    }
    release_quantum_state(state);
}


// Copyright 2021 RoboJackets
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

// BEGIN STUDENT CODE

#ifndef KALMAN_FILTER_HPP_
#define KALMAN_FILTER_HPP_

#include <Eigen/Dense>

namespace mineral_deposit_tracking
{

template<int StateSize>
class KalmanFilter
{
public:
  using VectorType = Eigen::Matrix<double, StateSize, 1>;
  using MatrixType = Eigen::Matrix<double, StateSize, StateSize>;

  KalmanFilter(
    const MatrixType & transition_matrix,
    const MatrixType & process_covariance,
    const MatrixType & observation_matrix)
  : transition_matrix_(transition_matrix),
    process_covariance_(process_covariance),
    observation_matrix_(observation_matrix),
    estimate_(VectorType::Zero()),
    estimate_covariance_(MatrixType::Identity() * 500)
  {
  }

  void Reset(const VectorType & initial_state, const MatrixType & initial_covariance)
  {
    estimate_ = initial_state;
    estimate_covariance_ = initial_covariance;
  }

  void TimeUpdate()
  {
    estimate_ = transition_matrix_ * estimate_;
    estimate_covariance_ = transition_matrix_ * estimate_covariance_ *
      transition_matrix_.transpose() + process_covariance_;
  }

  void MeasurementUpdate(
    const VectorType & measurement,
    const MatrixType & measurement_covariance)
  {
    const MatrixType innovation_covariance =
      (observation_matrix_ * estimate_covariance_ * observation_matrix_.transpose()) +
      measurement_covariance;

    const MatrixType gain = estimate_covariance_ * observation_matrix_.transpose() *
      innovation_covariance.inverse();

    estimate_ = estimate_ + (gain * (measurement - (observation_matrix_ * estimate_)));

    const MatrixType tmp = MatrixType::Identity() - (gain * observation_matrix_);

    estimate_covariance_ = (tmp * estimate_covariance_ * tmp.transpose()) +
      (gain * measurement_covariance * gain.transpose());
  }

  const VectorType & GetEstimate() const
  {
    return estimate_;
  }

  const MatrixType & GetEstimateCovariance() const
  {
    return estimate_covariance_;
  }

private:
  const MatrixType transition_matrix_;
  const MatrixType process_covariance_;
  const MatrixType observation_matrix_;
  VectorType estimate_;
  MatrixType estimate_covariance_;
};

}  // namespace mineral_deposit_tracking

#endif  // KALMAN_FILTER_HPP_

// END STUDENT CODE


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl-MatrixOps.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================
namespace  {

BOOST_PYTHON_FUNCTION_OVERLOADS(isEqual_overloads_2_3, gmtl::isEqual, 2, 3)

}// namespace

// Module ======================================================================
void _Export_gmtl_MatrixOps_h()
{
    def("add", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::add, return_internal_reference< 1 >());
    def("add", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::add, return_internal_reference< 1 >());
    def("identity", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &))&gmtl::identity, return_internal_reference< 1 >());
    def("identity", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &))&gmtl::identity, return_internal_reference< 1 >());
    def("invertTrans", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invertTrans, return_internal_reference< 1 >());
    def("invertTrans", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invertTrans, return_internal_reference< 1 >());
    def("invertOrthogonal", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invertOrthogonal, return_internal_reference< 1 >());
    def("invertOrthogonal", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invertOrthogonal, return_internal_reference< 1 >());
    def("invertAffine", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invertAffine, return_internal_reference< 1 >());
    def("invertAffine", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invertAffine, return_internal_reference< 1 >());
    def("invert", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invert, return_internal_reference< 1 >());
    def("invert", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invert, return_internal_reference< 1 >());
    def("invert", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &))&gmtl::invert, return_internal_reference< 1 >());
    def("invert", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &))&gmtl::invert, return_internal_reference< 1 >());
    def("invertFull_GJ", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invertFull_GJ, return_internal_reference< 1 >());
    def("invertFull_GJ", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invertFull_GJ, return_internal_reference< 1 >());
    def("invertFull", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::invertFull, return_internal_reference< 1 >());
    def("invertFull", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::invertFull, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::mult, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &, const float &))&gmtl::mult, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &, const float &))&gmtl::mult, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, float))&gmtl::mult, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::mult, return_internal_reference< 1 >());
    def("mult", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, float))&gmtl::mult, return_internal_reference< 1 >());
    def("postMult", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::postMult, return_internal_reference< 1 >());
    def("postMult", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::postMult, return_internal_reference< 1 >());
    def("preMult", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::preMult, return_internal_reference< 1 >());
    def("preMult", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::preMult, return_internal_reference< 1 >());
    def("sub", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::sub, return_internal_reference< 1 >());
    def("sub", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::sub, return_internal_reference< 1 >());
    def("transpose", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &))&gmtl::transpose, return_internal_reference< 1 >());
    def("transpose", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &))&gmtl::transpose, return_internal_reference< 1 >());
    def("transpose", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &))&gmtl::transpose, return_internal_reference< 1 >());
    def("transpose", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &))&gmtl::transpose, return_internal_reference< 1 >());
    def("zero", (gmtl::Matrix<float,3,3> & (*)(gmtl::Matrix<float,3,3> &))&gmtl::zero, return_internal_reference< 1 >());
    def("zero", (gmtl::Matrix<float,4,4> & (*)(gmtl::Matrix<float,4,4> &))&gmtl::zero, return_internal_reference< 1 >());
    def("isEqual", (bool (*)(const gmtl::Matrix<float,4,4> &, const gmtl::Matrix<float,4,4> &, const float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("isEqual", (bool (*)(const gmtl::Matrix<float,3,3> &, const gmtl::Matrix<float,3,3> &, const float))&gmtl::isEqual, isEqual_overloads_2_3());
    def("combineMatrixStates", &gmtl::combineMatrixStates);
}


//  (C) Copyright Jeremy Murphy 2015.
//  Use, modification and distribution are subject to 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)

#include <boost/config.hpp>
#define BOOST_TEST_MAIN
#include <boost/array.hpp>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <utility>

typedef boost::mpl::list<int, long, boost::multiprecision::cpp_int> signed_integral_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_zero, T, signed_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(2), static_cast<T>(0));
    BOOST_CHECK_EQUAL(a, static_cast<T>(2));
    a = boost::math::gcd(static_cast<T>(0), static_cast<T>(2));
    BOOST_CHECK_EQUAL(a, static_cast<T>(2));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_signed, T, signed_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(-40902), static_cast<T>(-24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
    a = boost::math::gcd(static_cast<T>(40902), static_cast<T>(24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
}

typedef boost::mpl::list<unsigned, unsigned long> unsigned_integral_test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_unsigned, T, unsigned_integral_test_types)
{
    T a = boost::math::gcd(static_cast<T>(40902), static_cast<T>(24140));
    BOOST_CHECK_EQUAL(a, static_cast<T>(34));
    a = boost::math::gcd(static_cast<T>(1836311903), static_cast<T>(2971215073)); // 46th and 47th Fibonacci numbers. 47th is prime.
    BOOST_CHECK_EQUAL(a, static_cast<T>(1));
}


//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef BERNOULLI_CLASSIFIER_HPP_
#define BERNOULLI_CLASSIFIER_HPP_

#include <boost/random/bernoulli_distribution.hpp>

namespace clotho {
namespace classifiers {

template < class RNG, class RealType = double >
class bernoulli_classifier {
public:
    typedef RealType real_type;
    typedef size_t  result_type;

    typedef bernoulli_classifier< RNG, RealType, Result > self_type;

    typedef boost::random::bernoulli_distribution< RealType > bernoulli_type;

    bernoulli_classifier( RNG * rng, real_type p = 0.5 ) :
        m_rng( rng )
        , m_bern( p ) {
    }

    bernoulli_classifier( const self_type & rhs ) : m_rng( rhs.m_rng ), m_bern( rhs.m_bern.param()) {}

    template < class Element >
    result_type operator()( const Element & elem ) const {
        return m_bern( *m_rng );
    }

    template < class ElementIterator >
    result_type operator()( ElementIterator elem_it, size_t idx) const {
        return m_bern( *m_rng );
    }

    virtual ~bernoulli_classifier() {}

protected:
    RNG             * m_rng;
    bernoulli_type  m_bern;
};

}   // namespace classifier {
}   // namespace clotho {

#endif  // BERNOULLI_CLASSIFIER_HPP_


/******************************************************************************
 * Copyright (c) 2015 - 2016 Philipp Schubert.                                *
 * All rights reserved. This program and the accompanying materials are made  *
 * available under the terms of LICENSE.txt.                                  *
 *                                                                            *
 * Contributors:                                                              *
 *     Philipp Schubert                                                       *
 *****************************************************************************/

/**
 * @file pseudoinverse.cpp
 * @brief Enthält die Implementation der Datei pseudoinverse.hh.
 *
 * Hier wird der Prototyp zur Berechnung eines Pseudoinversen einer
 * Dreiecksmatrix implementiert.
 *
 * @author Philipp D. Schubert
 * @bug Keine Bugs bekannt.
 */

#include <armadillo>
#include <omp.h>
#include <stdlib.h>
#include <string.h>
#include <typeinfo>

extern "C" {
#include "m.h"
#include "tm.h"
#include "utils.h"
}

extern "C" tm_t *pseudoinverse(const tm_t *t) {
  m_t *m = initM(t->size, t->size);
  unsigned int i, j;
#pragma omp parallel for shared(t, m) private(i, j) schedule(dynamic)
  for (i = 0; i < t->size; ++i) {
    for (j = 0; j < t->size; ++j) {
      m->elems[i * t->size + j] = (j <= i) ? t->elems[(i * (i + 1)) / 2 + j]
                                           : t->elems[(j * (j + 1)) / 2 + i];
    }
  }
  // check if we should use single or double precision and use adequate
  // armadillo data structures
  if (typeid(real_t).name() == typeid(float).name()) {
    arma::fmat f_am =
        arma::fmat((float *)m->elems, t->size, t->size, false, false);
    arma::fmat f_inverse = arma::pinv(f_am);
    // caution: override data memory of m
    memcpy(m->elems, f_inverse.memptr(), t->size * t->size * sizeof(real_t));
  } else {
    arma::mat d_am =
        arma::mat((double *)m->elems, t->size, t->size, false, false);
    arma::mat d_inverse = arma::pinv(d_am);
    // caution: override data memory of m
    memcpy(m->elems, d_inverse.memptr(), t->size * t->size * sizeof(real_t));
  }
  tm_t *result = initTM(t->size);
#pragma omp parallel for shared(result, m) private(i, j) schedule(dynamic)
  for (i = 0; i < t->size; ++i) {
    for (j = 0; j <= i; ++j) {
      result->elems[(i * (i + 1)) / 2 + j] = m->elems[i * t->size + j];
    }
  }
  free(m);
  return result;
}


//  Created by jimmy on 2016-06-14.
//  Copyright © 2016 jimmy. All rights reserved.
//

#ifndef __Kabsch__
#define __Kabsch__

// This code is modifed from released in public domain from
// https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp

#include <Eigen/Geometry>

// This is general Kabsh algorithm,
// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd input_pts, Eigen::Matrix3Xd output_pts);

// This is Kabsh algorithm for camera pose estimation
// Assume the points in camera coordiantes and world coordinates have same scale
// The input 3D points are stored as columns. No scale effect
Eigen::Affine3d Find3DAffineTransformSameScale(Eigen::Matrix3Xd input_pts, Eigen::Matrix3Xd output_pts);

#endif /* __Kabsch__ */


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 BOOST_GEOMETRY_UTIL_MATH_HPP
#define BOOST_GEOMETRY_UTIL_MATH_HPP

#include <cmath>
#include <limits>

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace math
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Type, bool IsFloatingPoint>
struct equals
{
    static inline bool apply(Type const& a, Type const& b)
    {
        return a == b;
    }
};

template <typename Type>
struct equals<Type, true>
{
    static inline bool apply(Type const& a, Type const& b)
    {
        // See http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.17,
        // FUTURE: replace by some boost tool or boost::test::close_at_tolerance
        return std::abs(a - b) <= std::numeric_limits<Type>::epsilon() * std::abs(a);
    }
};

template <typename Type, bool IsFloatingPoint>
struct equals_with_epsilon : public equals<Type, IsFloatingPoint> {};

/*!
\brief Short construct to enable partial specialization for PI, currently not possible in Math.
*/
template <typename T>
struct define_pi
{
    static inline T apply()
    {
        // Default calls Boost.Math
        return boost::math::constants::pi<T>();
    }
};

} // namespace detail
#endif

template <typename T>
inline T pi() { return detail::define_pi<T>::apply(); }

// Maybe replace this by boost equals or boost ublas numeric equals or so

/*!
    \brief returns true if both arguments are equal.
    \ingroup utility
    \param a first argument
    \param b second argument
    \return true if a == b
    \note If both a and b are of an integral type, comparison is done by ==.
    If one of the types is floating point, comparison is done by abs and
    comparing with epsilon. If one of the types is non-fundamental, it might
    be a high-precision number and comparison is done using the == operator
    of that class.
*/

template <typename T1, typename T2>
inline bool equals(T1 const& a, T2 const& b)
{
    typedef typename select_most_precise<T1, T2>::type select_type;
    return detail::equals
        <
            select_type,
            boost::is_floating_point<select_type>::type::value
        >::apply(a, b);
}

template <typename T1, typename T2>
inline bool equals_with_epsilon(T1 const& a, T2 const& b)
{
    typedef typename select_most_precise<T1, T2>::type select_type;
    return detail::equals_with_epsilon
        <
            select_type,
            boost::is_floating_point<select_type>::type::value
        >::apply(a, b);
}

double const d2r = geometry::math::pi<double>() / 180.0;
double const r2d = 1.0 / d2r;

/*!
    \brief Calculates the haversine of an angle
    \ingroup utility
    \note See http://en.wikipedia.org/wiki/Haversine_formula
    haversin(alpha) = sin2(alpha/2)
*/
template <typename T>
inline T hav(T const& theta)
{
    T const half = T(0.5);
    T const sn = sin(half * theta);
    return sn * sn;
}

/*!
\brief Short utility to return the square
\ingroup utility
\param value Value to calculate the square from
\return The squared value
*/
template <typename T>
inline T sqr(T const& value)
{
    return value * value;
}

/*!
\brief Short utility to workaround gcc/clang problem that abs is converting to integer
\ingroup utility
*/
template<typename T>
inline T abs(const T& t)
{
    using std::abs;
    return abs(t);
}

} // namespace math

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_MATH_HPP


// Copyright [2020] Yoonyoung (Jamie) Cho

#include "bglpy/graph.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>

#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/grid_graph.hpp>

#include <iostream>

#include <fmt/format.h>

namespace cho {
namespace graph {
// Define all the type aliases ...
struct Node2D {
  float x;
  float y;
};

struct Edge2D {
  /* float weight{std::numeric_limits<float>::max()}; */
  float weight;
};

struct found_goal {};

template <typename VertexProperty = boost::no_property,
          typename EdgeProperty = boost::property<boost::edge_weight_t, float>>
using GraphBase =
    boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
                          VertexProperty, EdgeProperty>;
using Graph2D = GraphBase<Node2D, Edge2D>;

// visitor that terminates when we find the goal
template <class Vertex>
class astar_goal_visitor : public boost::default_astar_visitor {
 public:
  astar_goal_visitor(const Vertex& goal) : m_goal(goal) {}
  template <class Graph>
  void examine_vertex(const Vertex& u, Graph& g) {
    // fmt::print("v{},{}\n", u[0], u[1]);
    if (u == m_goal) {
      throw found_goal();
    }
  }

 private:
  Vertex m_goal;
};

template <class Graph, class Vertex, class LocMap, class CostType>
class distance_heuristic : public boost::astar_heuristic<Graph, CostType> {
 public:
  distance_heuristic(const LocMap& loc_map, const Vertex& goal)
      : loc_map_(loc_map), goal_(goal) {}
  CostType operator()(const Vertex& u) {
    const auto& v0 = loc_map_(u);
    const auto& v1 = loc_map_(goal_);
    // fmt::print("H{} {} - {} {}\n", v0.first, v0.second, v1.first, v1.second);
    const float& dx = v0.first - v1.first;
    const float& dy = v0.second - v1.second;
    return std::sqrt(dx * dx + dy * dy);
  }

 private:
  LocMap loc_map_;
  Vertex goal_;
};

template <typename T>
struct matrix_hash : std::unary_function<T, size_t> {
  std::size_t operator()(T const& matrix) const {
    // Note that it is oblivious to the storage order of Eigen matrix (column-
    // or row-major). It will give you the same hash value for two different
    // matrices if they are the transpose of each other in different storage
    // order.
    size_t seed = 0;
    for (size_t i = 0; i < matrix.size(); ++i) {
      auto elem = *(matrix.data() + i);
      seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) +
              (seed >> 2);
    }
    return seed;
  }
};

auto BuildGridGraph(const MatrixXu& grid) {
  boost::array<int, 2> dims({int(grid.rows()), int(grid.cols())});
  auto g_ptr = std::make_shared<boost::grid_graph<2, int, int>>(dims);
  auto& g = *g_ptr;

  using Graph = std::remove_reference<decltype(g)>::type;
  using Vertex = Graph::vertex_descriptor;
  using VertexSet = boost::unordered_set<Vertex>;
  using FGraph = boost::vertex_subset_complement_filter<Graph, VertexSet>::type;

  std::shared_ptr<VertexSet> barriers = std::make_shared<VertexSet>();

  std::unordered_map<Eigen::Vector2i, Vertex, matrix_hash<Eigen::Vector2i>>
      ivmap;
  for (int i = 0; i < grid.size(); ++i) {
    const auto& v = vertex(i, g);
    // iv0.y() * grid.rows() + iv0.x()

    const int x = i % grid.rows();
    const int y = i / grid.rows();
    if (grid(x, y)) {
      barriers->insert(v);
    }

    ivmap.emplace(Eigen::Vector2i{x, y}, v);
  }
  auto graph = std::make_shared<FGraph>(
      boost::make_vertex_subset_complement_filter(g, *barriers));

  // NOTE(jamie): barriers must be returned here,
  // since make_vertex_subset_complement_filter does not track barriers.
  return std::make_tuple(g_ptr, graph, ivmap, barriers);
}

auto BuildGraph(
    const MatrixXu& grid, cho::graph::Graph2D* const g_ptr,
    std::unordered_map<
        Eigen::Vector2i,
        std::remove_reference<decltype(*g_ptr)>::type::vertex_descriptor,
        matrix_hash<Eigen::Vector2i>>* const ivmap) {
  // Type alias
  using Graph = std::remove_reference<decltype(*g_ptr)>::type;
  using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
  using Edge = typename boost::graph_traits<Graph>::edge_descriptor;

  auto& g = *g_ptr;
  g.m_vertices.reserve(grid.size());

  // Separately track (index -> vertex) map.
  std::unordered_map<Eigen::Vector2i, Vertex, matrix_hash<Eigen::Vector2i>>
      vmap;

  // Add Nodes first.
  for (int i = 0; i < grid.rows(); ++i) {
    for (int j = 0; j < grid.cols(); ++j) {
      if (!grid(i, j)) {
        continue;
      }
      const Vertex& src = boost::add_vertex(g);
      g[src].x = i;
      g[src].y = j;
      vmap.emplace(Eigen::Vector2i{i, j}, src);
    }
  }

  if (ivmap) {
    *ivmap = vmap;
  }

  // Add Edges.
  // g.m_edges.reserve(8 * grid.rows() * grid.cols());
  for (int i = 0; i < grid.rows(); ++i) {
    for (int j = 0; j < grid.cols(); ++j) {
      if (!grid(i, j)) {
        continue;
      }
      for (int di = -1; di <= 1; ++di) {
        for (int dj = -1; dj <= 1; ++dj) {
          const int i2 = i + di;
          const int j2 = j + dj;
          // Skip out-of-bounds
          if (i2 < 0 || i2 >= grid.rows() || j2 < 0 || j2 >= grid.cols()) {
            continue;
          }

          // Skip self.
          if (i == i2 && j == j2) {
            continue;
          }

          // Skip empty cell.
          if (!grid(i2, j2)) {
            continue;
          }

          // Add that edge.
          /* fmt::print("E {},{} - {},{}\n", i, j, i2, j2); */

          const Edge& e =
              boost::add_edge(vmap.at({i, j}), vmap.at({i2, j2}), g).first;
          g[e].weight = (di * di + dj * dj);
        }
      }
    }
  }

  return g;
}
VecVector2i FindPath(const MatrixXu& grid, const Eigen::Vector2i& iv0,
                     const Eigen::Vector2i& iv1) {
  // fmt::print(" Input [I-J] Order ");
  // fmt::print(" {} {} -> {} {}", iv0.x(), iv0.y(), iv1.x(), iv1.y());

  // using Graph = cho::graph::Graph2D;
  // using Vertex = Graph::vertex_descriptor;

  // Graph g
  // std::unordered_map<Eigen::Vector2i, Vertex, matrix_hash<Eigen::Vector2i>>
  // ivmap; BuildGraph(grid, &g, &ivmap);

  auto e = BuildGridGraph(grid);
  auto& g0 = *std::get<0>(e);
  auto& g1 = *std::get<1>(e);
  // auto& ivmap = std::get<2>(e);
  auto& barrier = std::get<3>(e);

  using Graph = std::remove_reference<decltype(g1)>::type;
  // using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
  using Vertex = std::remove_reference<decltype(g0)>::type::vertex_descriptor;
  boost::static_property_map<float> wmap(1);

  // for (int i = 0; i < num_vertices(g0); ++i) {
  //    const auto& v = vertex(i, g0);
  //    if (i == num_vertices(g0) - 1) {
  //        // fmt::print("last : {} {}\n", v[0], v[1]);
  //    }
  //}

  // THIS IS ABSOLUTELY THE MOST CONFUSING PART.
  // vertex id is incremented 0-dim first for whatever reason...
  // vid = iv0.x() * grid.c
  const Vertex& v0 = vertex(iv0.y() * grid.rows() + iv0.x(), g0);
  const Vertex& v1 = vertex(iv1.y() * grid.rows() + iv1.x(), g0);

  // std::vector<Vertex> prv_map(boost::num_vertices(g));
  // std::vector<float> d(boost::num_vertices(g));
  boost::unordered_map<Vertex, Vertex, boost::hash<Vertex>> prv_map;
  boost::associative_property_map<decltype(prv_map)> prv_pmap(prv_map);

  boost::unordered_map<Vertex, float, boost::hash<Vertex>> dist_map;
  boost::associative_property_map<
      boost::unordered_map<Vertex, float, boost::hash<Vertex>>>
      dist_pmap(dist_map);

  boost::unordered_map<Vertex, std::pair<float, float>, boost::hash<Vertex>>
      loc_map;

  const auto& GetXy = [](const Vertex& v) -> auto {
    return std::make_pair(v[0], v[1]);
    // const int idx = boost::get(boost::vertex_index, g1, v);
    // int x = idx / grid.cols();
    // int y = idx % grid.cols();
    // return std::make_pair(x, y);
  };

  try {
    boost::astar_search_tree(
        g1, v0,
        distance_heuristic<Graph, Vertex,
                           std::remove_reference<decltype(GetXy)>::type, float>(
            GetXy, v1),
        boost::predecessor_map(prv_pmap)
            .distance_map(dist_pmap)
            // .weight_map(boost::get(&cho::graph::Edge2D::weight, g))
            .weight_map(wmap)
            .visitor(astar_goal_visitor<Vertex>(v1)));
  } catch (const found_goal& fg) {
    std::list<Eigen::Vector2i> path;
    auto v = v1;
    while (true) {
      int x, y;
      std::tie(x, y) = GetXy(v);
      // path.emplace_front(g[v].x, g[v].y);
      path.emplace_front(x, y);
      // fmt::print("{} {}", x, y);

      const auto& prv = prv_map[v];
      if (v == prv) {
        break;
      }
      v = prv;
    }

    return VecVector2i{path.begin(), path.end()};
  }
  return {};
}
}  // namespace graph
}  // namespace cho


/**
 * @file contourplot.cc
 * @brief NPDE homework ContourPlot code
 * @author Oliver Rietmann
 * @date 25.03.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <fstream>
#include <iostream>

#include "contourplot.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

constexpr double Square(double x) { return x * x; }
constexpr double Cube(double x) { return x * x * x; }

int main(int /*argc*/, char** /*argv*/) {
  /* SAM_LISTING_BEGIN_0 */
  // Compute isolines using gradF and computeIsolinePoints(...)
  Eigen::MatrixXd isolinePoints = ContourPlot::crookedEgg();

  // Compute isolines using F and computeIsolinePointsDQ(...)
  auto F = [](Eigen::Vector2d x) {
    return Square(x.squaredNorm()) - Cube(x[0]) - Cube(x[1]);
  };
  Eigen::Vector2d y0(2.0, 0.0);
  double T = 12.0;
  Eigen::MatrixXd isolinePointsDQ =
      ContourPlot::computeIsolinePointsDQ(F, y0, T);

  // Write .csv file with 4 columns containg the x and y coordinates of the two
  // isolines
  std::ofstream csvFile;
  csvFile.open("contourplot.csv");
  csvFile << isolinePoints.format(CSVFormat) << std::endl;
  csvFile << isolinePointsDQ.format(CSVFormat) << std::endl;
  csvFile.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/contourplot.csv" << std::endl;

  // Call the Python script to plot the isolines
  std::system("python3 " CURRENT_SOURCE_DIR
              "/contourplot.py " CURRENT_BINARY_DIR
              "/contourplot.csv " CURRENT_BINARY_DIR "/contourplot.eps");
  /* SAM_LISTING_END_0 */

  return 0;
}


#include <vector>
#include <algorithm>
#include <fstream>

#include "glog/logging.h"
#include <boost/concept_check.hpp>

#include "caffe/util/anchors.hpp"

#include "caffe/layers/proposal_layer.hpp"

namespace caffe{

template <typename Dtype>
void ProposalLayer<Dtype>::LayerSetUp( const vector< Blob< Dtype >* > &bottom,
                                const vector< Blob< Dtype >* > &top )
{
  ProposalParameter proposal_param = this->layer_param_.proposal_param();
  std::vector<std::vector <double> > base_anchors;

  if(proposal_param.scales_size()!=0)
  {
    std::vector<int> anchor_scales;
    for(size_t i=0; i<proposal_param.scales_size(); ++i)
      anchor_scales.push_back(proposal_param.scales(i));
    base_anchors = generate_anchors(anchor_scales);
  }
  else
  {
    base_anchors = generate_anchors();
  }

  m_base_anchors.Reshape(std::vector<int>({(int)base_anchors.size(), (int)base_anchors[0].size()}));
  for(std::size_t i=0; i < base_anchors.size(); ++i)
    for(std::size_t j=0; j< base_anchors[i].size(); ++j)
      m_base_anchors.mutable_cpu_data()[m_base_anchors.offset(i,j)] = base_anchors[i][j];

  CHECK(bottom.size()==3 || bottom.size()==4);
  m_num_anchors = m_base_anchors.shape(0);
  m_feat_stride = proposal_param.feat_stride();
  m_nms_thresh = proposal_param.nms_thresh();
  m_pre_nms_topN = proposal_param.pre_nms_topn();
  m_post_nms_topN = proposal_param.post_nms_topn();
  m_min_size = proposal_param.min_size();

  CHECK(top.size()==2 || top.size()==1);
  top[0]->Reshape(std::vector<int>({1,5}));
  if(top.size() == 2)
    top[1]->Reshape(std::vector<int>({1,1,1,1}));
}

template <typename Dtype>
void ProposalLayer<Dtype>::Reshape( const vector< Blob< Dtype >* > &bottom,
                             const vector< Blob< Dtype >* > &top )
{
  CHECK(bottom[0]->shape(0)==1); //TODO rewrite for batches

  int height = bottom[0]->shape(2);
  int width = bottom[0]->shape(3);
//   m_img_anchors = std::vector<Anchor>();
//   for(int i=0; i<height*width; ++i)
//   {
//   /**this is the shift generation matrix generations for anchors
//     *we do the shift directly on base anchors
//     *shifts.push_back(
//     *   std::vector<int>({(i%width)*m_feat_stride,
//     *                     (i/width)*m_feat_stride,
//     *                     (i%width)*m_feat_stride,
//     *                     (i/width)*m_feat_stride}));
//     *
//     *Then add every shift to every anchor. That will be the anchors on the pics.
//     */
//     for(int j=0; j < m_num_anchors ; ++j)
//     {
//       m_img_anchors.push_back(
//         std::vector<double>({m_base_anchors[j][0]+(i%width)*m_feat_stride,
//                         m_base_anchors[j][1]+(i/width)*m_feat_stride,
//                         m_base_anchors[j][2]+(i%width)*m_feat_stride,
//                         m_base_anchors[j][3]+(i/width)*m_feat_stride})
//       );
//     }
//   }

  m_anchors.Reshape(std::vector<int>( {height * width * m_num_anchors, 4}));

  m_proposals.ReshapeLike(m_anchors);

  std::size_t scores_num = bottom[0]->shape(2)*bottom[0]->shape(3)*bottom[0]->shape(1);
  m_scores.Reshape(std::vector<int>(1,scores_num-m_num_anchors));
  m_indexes.Reshape(std::vector<int>(1,scores_num-m_num_anchors));
  m_iou.Reshape(std::vector<int>(1,m_pre_nms_topN*m_pre_nms_topN));
  top[0]->Reshape(std::vector<int>({static_cast<int>(m_post_nms_topN),5}));
}

template <typename Dtype>
void ProposalLayer<Dtype>::Forward_cpu( const vector< Blob< Dtype >* > &bottom,
                                 const vector< Blob< Dtype >* > &top )
{
  if(bottom.size()==4)
  {
    m_pre_nms_topN = bottom[3]->cpu_data()[bottom[3]->offset(0,0)];
    m_post_nms_topN = bottom[3]->cpu_data()[bottom[3]->offset(0,1)];
    m_nms_thresh =bottom[3]->cpu_data()[bottom[3]->offset(0,2)];
    m_min_size = bottom[3]->cpu_data()[bottom[3]->offset(0,3)];
  }
  int height = bottom[0]->shape(2);
  int width = bottom[0]->shape(3);

  for(std::size_t ind=0; ind<height*width*m_num_anchors; ++ind)
  {
    int j=ind%m_num_anchors;
    int i=ind/m_num_anchors;

    m_anchors.mutable_cpu_data()[ind*4] = m_base_anchors.mutable_cpu_data()[j*4+0]+(i%width)*m_feat_stride;
    m_anchors.mutable_cpu_data()[ind*4+1] = m_base_anchors.mutable_cpu_data()[j*4+1]+(i/width)*m_feat_stride;
    m_anchors.mutable_cpu_data()[ind*4+2] = m_base_anchors.mutable_cpu_data()[j*4+2]+(i%width)*m_feat_stride;
    m_anchors.mutable_cpu_data()[ind*4+3] = m_base_anchors.mutable_cpu_data()[j*4+3]+(i/width)*m_feat_stride;
  }

  for(int i=0; m_indexes.shape(1);++i)
    m_indexes.mutable_cpu_data()[i]=i;
  //transponate the input blob (n,anchors_num*4,h,w) -> (n,h,w, anchors_num*4), then reshape (n,h*w*anchors_num,4)
  //                                                                          where rows are ordered by (h, w, anchor_ind)
  const Dtype* bbox_deltas_blob =  bottom[1]->mutable_cpu_data();
  Dtype* proposal_cpu = m_proposals.mutable_cpu_data();

  std::size_t ind_prop = 0;
  for(int h=0; h<bottom[1]->shape(2); ++h)
    for(int w=0; w<bottom[1]->shape(3);++w)
      for(int ch=0; ch<bottom[1]->shape(1)/4;++ch)
      {
        //(h*bottom->shape(3)+w)*(bottom[1]->shape(1)/4)+ch;
        proposal_cpu[ind_prop*4] = bbox_deltas_blob[bottom[1]->offset(0,ch*4,h,w)];     //[0][ch*4][h][w],
        proposal_cpu[ind_prop*4+1] = bbox_deltas_blob[bottom[1]->offset(0,ch*4+1,h,w)];   //[0][ch*4+1][h][w],
        proposal_cpu[ind_prop*4+2] = bbox_deltas_blob[bottom[1]->offset(0,ch*4+2,h,w)];   //[0][ch*4+2][h][w],
        proposal_cpu[ind_prop*4+3] = bbox_deltas_blob[bottom[1]->offset(0,ch*4+3,h,w)];   //[0][ch*4+3][h][w]
        ++ind_prop;
      }
  //transponate the input blob (n,anchors_num,h,w) -> (n,h,w, anchors_num), then reshape (n,h*w*anchors_num,1)
  //where rows are ordered by (h, w, anchor_ind)

  const Dtype* scores_blob = bottom[0]->mutable_cpu_data();
  Dtype* scores_cpu = m_scores.mutable_cpu_data();

  std::size_t scr_ind=0;//this is necessery too keep
  for(int h=0; h<bottom[0]->shape(2); ++h)
    for(int w=0; w<bottom[0]->shape(3);++w)
      for(int ch=m_num_anchors; ch<bottom[0]->shape(1);++ch)
      {
        scores_cpu[scr_ind++] = scores_blob[bottom[0]->offset(0,ch,h,w)];
      }

  bbox_transform_inv();

  clip_boxes(bottom[2]->cpu_data()[bottom[2]->offset(0,1)],bottom[2]->cpu_data()[bottom[2]->offset(0,0)]);

  Dtype min_size= m_min_size * bottom[2]->cpu_data()[bottom[2]->offset(0,2)];

   std::vector<std::size_t> index_vec;
   for(int i=0; i<m_proposals.shape(0); ++i)
     if(proposal_cpu[i*4+2]-proposal_cpu[i*4]+1>=min_size && proposal_cpu[i*4+3]-proposal_cpu[i*4+1]+1>=min_size)
       index_vec.push_back(i);

  std::partial_sort(index_vec.begin(),index_vec.begin()+m_pre_nms_topN,index_vec.end(),
                    [scores_cpu](const std::size_t& a,const std::size_t& b){return scores_cpu[a]>scores_cpu[b];});

  if(index_vec.size()>m_pre_nms_topN)
    index_vec.resize(m_pre_nms_topN);

  index_vec=cpu_base_nms(index_vec);

  if(index_vec.size()>m_post_nms_topN)
    index_vec.resize(m_post_nms_topN);

  for(int i=0; i<index_vec.size(); ++i){
    top[0]->mutable_cpu_data()[top[0]->offset(i,0)] = 0;
    top[0]->mutable_cpu_data()[top[0]->offset(i,1)] = proposal_cpu[index_vec[i]*4];
    top[0]->mutable_cpu_data()[top[0]->offset(i,2)] = proposal_cpu[index_vec[i]*4+1];
    top[0]->mutable_cpu_data()[top[0]->offset(i,3)] = proposal_cpu[index_vec[i]*4+2];
    top[0]->mutable_cpu_data()[top[0]->offset(i,4)] = proposal_cpu[index_vec[i]*4+3];
  }

  //TODO implement score out blob

}

template <typename Dtype>
std::vector<std::size_t> ProposalLayer<Dtype>::cpu_base_nms(
  const std::vector<std::size_t>& index_vec
)
{
  std::vector<std::size_t> keep;
  std::vector<double> areas;
  Dtype* proposals = m_proposals.mutable_cpu_data();

  keep.push_back(index_vec[0]);
  areas.push_back((proposals[keep[0]*4+2]-proposals[keep[0]*4]+1)*(proposals[keep[0]*4+3]-proposals[keep[0]*4+1]+1));

  for(size_t i=1; i<index_vec.size(); ++i)
  {
    int current=index_vec[i];
    double area_current=(proposals[current*4+2]-proposals[current*4]+1)*(proposals[current*4+3]-proposals[current*4+1]+1);

    size_t j=0;
    while(j<keep.size())
    {
      double inter_x1=std::max(proposals[keep[j]*4+0],proposals[current*4+0]);
      double inter_x2=std::min(proposals[keep[j]*4+2],proposals[current*4+2]);
      double inter_y1=std::max(proposals[keep[j]*4+1],proposals[current*4+1]);
      double inter_y2=std::min(proposals[keep[j]*4+3],proposals[current*4+3]);
      double w=std::max<double>(0, inter_x2-inter_x1+1);
      double h=std::max<double>(0, inter_y2-inter_y1+1);
      double inter = w*h;
      double overlap= inter / (areas[j]+area_current-inter);
      if (overlap >= m_nms_thresh)
        break;
      ++j;
    }

    if(j==keep.size())
    {
      keep.push_back(current);
      areas.push_back(area_current);
    }
  }
  return keep;
}

template <typename Dtype>
void ProposalLayer<Dtype>::Backward_cpu( const vector< Blob< Dtype >* > &top,
                                  const vector< bool > &propagate_down,
                                  const vector< Blob< Dtype >* > &bottom )
{
  // This layer dont need backward
}

template <typename Dtype>
void ProposalLayer<Dtype>::clip_boxes(const double& img_w, const double& img_h )
{
  Dtype* proposals_cpu = m_proposals.mutable_cpu_data();

  for(std::size_t i=0; i < m_proposals.shape(0); ++i)
  {
    proposals_cpu[i*4] = std::max<double>(std::min<double>(proposals_cpu[i*4],img_w-1),0);
    proposals_cpu[i*4+1] = std::max<double>(std::min<double>(proposals_cpu[i*4+1],img_h-1),0);
    proposals_cpu[i*4+2] = std::max<double>(std::min<double>(proposals_cpu[i*4+2],img_w-1),0);
    proposals_cpu[i*4+3] = std::max<double>(std::min<double>(proposals_cpu[i*4+3],img_h-1),0);
  }
}

template <typename Dtype>
void ProposalLayer<Dtype>::bbox_transform_inv()
{
  const Dtype* anchors = m_anchors.mutable_cpu_data();
  Dtype* proposals = m_proposals.mutable_cpu_data();

  for(std::size_t i = 0; i < m_proposals.shape(0); ++i)
  {
    double width = anchors[i*4+2] - anchors[i*4] + 1;
    double height = anchors[i*4+3] - anchors[i*4+1] + 1;;
    double ctr_x = anchors[i*4] + 0.5 * width;
    double ctr_y = anchors[i*4+1] + 0.5 * height;

    double pred_ctr_x = proposals[i*4] * width + ctr_x;
    double pred_ctr_y = proposals[i*4+1]* height + ctr_y;
    double pred_w = exp( proposals[i*4+2] ) * width;
    double pred_h = exp( proposals[i*4+3] ) * height;

    proposals[i*4] = pred_ctr_x - 0.5 * pred_w;
    proposals[i*4+1] = pred_ctr_y - 0.5 * pred_h;
    proposals[i*4+2] = pred_ctr_x + 0.5 * pred_w;
    proposals[i*4+3] = pred_ctr_y + 0.5 * pred_h;
  }

}

#ifdef CPU_ONLY
STUB_GPU(ProposalLayer);
#endif

INSTANTIATE_CLASS(ProposalLayer);
REGISTER_LAYER_CLASS(Proposal);
}//caffe


#ifndef CANNON_GEOM_KD_TREE_INDEXED_H
#define CANNON_GEOM_KD_TREE_INDEXED_H

#include <Eigen/Core>
#include <CGAL/Epick_d.h>
#include <CGAL/Search_traits_d.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/property_map.h>
#include <boost/tuple/tuple.hpp>

using namespace Eigen;

/*!
 * \file cannon/geom/kd_tree_indexed.hpp
 * \brief File containing KDTreeIndexed class definition.
 */

namespace cannon {
  namespace geom {

    /*!
     * \brief Class representing a kd-tree which keeps track of the index of inserted points.
     *
     * This class is an extension on top of a straightforward kd-tree that
     * associates an index with each inserted point. This is useful if the
     * inserted points are also kept in some structure outside of the kd-tree,
     * in which case storing the index for each point obviates the need to
     * search for retrieved nearest neighbor points.
     *
     * \sa cannon::geom::KDTree
     */
    class KDTreeIndexed {
      public:
        using Kernel = CGAL::Epick_d<CGAL::Dynamic_dimension_tag>;
        using Point_d = typename Kernel::Point_d;
        using Point_and_int = boost::tuple<Point_d, int>;

        using TraitsBase = CGAL::Search_traits_d<Kernel, CGAL::Dynamic_dimension_tag>;
        using Traits = CGAL::Search_traits_adapter<Point_and_int,
              CGAL::Nth_of_tuple_property_map<0, Point_and_int>, TraitsBase>;

        using NeighborSearch = CGAL::Orthogonal_k_neighbor_search<Traits>;
        using Tree = typename NeighborSearch::Tree;

        KDTreeIndexed() = delete;

        /*!
         * Constructor taking the dimensionality of points that this kd-tree will hold.
         *
         * \param dim The dimension of points to be held.
         */
        KDTreeIndexed(int dim) : dim_(dim) {}

        /*!
         * Method to insert points into the kd-tree.
         *
         * \param pts Matrix of points to be added to the kd-tree. Each column
         * should be a point to be inserted, matching insertion of a single
         * point being insertion of a column vector
         */
        void insert(const MatrixXd& pts);

        /*!
         * Method to clear this kd-tree, removing all held points.
         */
        void clear();

        /*!
         * Method to query the nearest neighbor of the input point.
         *
         * \param query Point to find nearest neighbor of.
         *
         * \returns A pair consisting of the closest previously inserted point
         * to the query point and its index.
         */
        std::pair<VectorXd, int> get_nearest_neighbor(const VectorXd& query);

        /*!
         * Method to get the index of the nearest neighbor of the input point.
         *
         * \param query Point to find nearest neighbor of.
         *
         * \returns The index of the nearest neighbor.
         */
        int get_nearest_idx(const VectorXd& query);

        /*!
         * Method to get the number of points in this kd-tree.
         *
         * \returns Number of points in this tree.
         */
        int get_size();

      private:
        int dim_; //!< Dimension of points in this kd-tree
        Tree tree_; //!< Internal CGAL kd-tree object
        unsigned int furthest_idx_ = 0; //!< Greatest index assigned to a point in this kd-tree so far
    };

  } // namespace geom
} // namespace cannon

#endif /* ifndef CANNON_GEOM_KD_TREE_INDEXED_H */


/*
 * generator.hpp
 *
 *  Created on: Apr 19, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "../math/container_traits.hpp"
#include "../math/typedefs.hpp"
#include "generator_crtp.hpp"
#include "parameters.hpp"

namespace tsdf {

template<typename ScalarContainer>
class Generator {
};

template<typename Scalar>
class Generator<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> > :
		public GeneratorCRTP<Generator<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> >,
				Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>> {

public:
	typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> Mat;
	using GeneratorCRTP<Generator<Mat>, Mat>::GeneratorCRTP;
	typedef eig::Matrix<Scalar,4,1> Vec4;
	typedef eig::Matrix<Scalar,3,1> Vec3;
	typedef eig::Matrix<Scalar,2,1> Vec2;
	typedef eig::Matrix<Scalar,3,3> Mat3;
	typedef eig::Matrix<Scalar,2,2> Mat2;

	Mat generate__none(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
			int image_y_coordinate = 0) const;

	Mat generate__bilinear_image_space(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
			int image_y_coordinate = 0) const;

	Mat generate__bilinear_voxel_space(
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate = 0) const;

	Mat generate__ewa_image_space(
					const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
					const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
					int image_y_coordinate = 0) const;

	Mat generate__ewa_voxel_space(
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate = 0) const;

	Mat generate__ewa_voxel_space_inclusive(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
			int image_y_coordinate = 0) const;
private:
	template <typename SamplingBoundsFunction, typename VoxelValueFunction>
	Mat generate__ewa_aux(SamplingBoundsFunction&& compute_sampling_bounds, VoxelValueFunction&& compute_voxel_value,
					const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
					const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
					int image_y_coordinate) const;
};

template<typename Scalar>
class Generator<Eigen::Tensor<Scalar, 3, Eigen::ColMajor> > :
		public GeneratorCRTP<Generator<Eigen::Tensor<Scalar, 3, Eigen::ColMajor> >,
			Eigen::Tensor<Scalar, 3, Eigen::ColMajor> > {
public:
	typedef Eigen::Tensor<Scalar, 3, Eigen::ColMajor> Ts;
	using GeneratorCRTP<Generator<Ts>, Ts>::GeneratorCRTP;
	typedef eig::Matrix<Scalar,4,1> Vec4;
	typedef eig::Matrix<Scalar,3,1> Vec3;
	typedef eig::Matrix<Scalar,2,1> Vec2;
	typedef eig::Matrix<Scalar,3,3> Mat3;
	typedef eig::Matrix<Scalar,2,2> Mat2;

	Ts generate__none(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
			int image_y_coordinate = 0) const;

	Ts generate__bilinear_image_space(
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate = 0) const;

	Ts generate__bilinear_voxel_space(
					const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
					const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
					int image_y_coordinate = 0) const;

	Ts generate__ewa_image_space(
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate = 0) const;

	Ts generate__ewa_voxel_space(
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate = 0) const;

	Ts generate__ewa_voxel_space_inclusive(
			const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
			const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
			int image_y_coordinate = 0) const;
private:
template <typename SamplingBoundsFunction, typename VoxelValueFunction>
Ts generate__ewa_aux(SamplingBoundsFunction&& compute_sampling_bounds, VoxelValueFunction&& compute_voxel_value,
				const Eigen::Matrix<unsigned short, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& depth_image,
				const Eigen::Matrix<Scalar, 4, 4, Eigen::ColMajor>& camera_pose,
				int image_y_coordinate) const;
};

typedef Parameters<eig::MatrixXf> Parameters2d;
typedef Parameters<math::Tensor3f> Parameters3d;
typedef Generator<eig::MatrixXf> Generator2d;
typedef Generator<math::Tensor3f> Generator3d;

}  // namespace tsdf


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ARG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ARG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-arithmetic
    Function object implementing arg capabilities

    Computes the angular orientation of its parameter.

    @par semantic:
    For any given value @c x of floating type @c T:

    @code
    T r = arg(x);
    @endcode

    is equivalent to:

    @code
    T r = (is_nan(x)) ? x : (is_negative(x)) ? Pi<T>() : Zero<T>();
    @endcode

    @par Notes:

    - Returns \f$+0\f$ or \f$\pi\f$  (or @ref Nan if and only if the input is @ref Nan)
    according to the input @ref sign bit,

    - arg(x, use_signbit_) always returns \f$+0\f$ or \f$\pi\f$ taking into account the bit of sign
    even in the Nan case and is quicker.

    - This function implement the restriction to real numbers of the complex arg(ument) function.

    - This function is not defined for integral types.

    @see is_negative,  is_ltz,  Pi

  **/
  Value arg(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/arg.hpp>
#include <boost/simd/function/simd/arg.hpp>

#endif


#ifndef SKYLARK_WZT_HPP
#define SKYLARK_WZT_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <boost/random.hpp>

namespace skylark { namespace sketch {

/**
 * Woodruff-Zhang Transform (data)
 *
 * Woodruff-Zhang Transform is very similar to the Clarkson-Woodruff Transform:
 * it replaces the +1/-1 diagonal with reciprocal exponentia random enteries.
 * It is sutiable for lp regression with 1 <= p <= 2.
 *
 * Reference:
 * D. Woodruff and Q. Zhang
 * Subspace Embeddings and L_p Regression Using Exponential Random
 * COLT 2013
 *
 * TODO current implementation is only one sketch index, when for 1 <= p <= 2
 *      you want more than one.
 */

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct WZT_t :
        public WZT_data_t,
        virtual public sketch_transform_t<InputMatrixType, OutputMatrixType > {

public:

    // We use composition to defer calls to hash_transform_t
    typedef hash_transform_t< InputMatrixType, OutputMatrixType,
                              boost::random::uniform_int_distribution,
                              boost::random::exponential_distribution > transform_t;

    typedef WZT_data_t data_type;
    typedef data_type::params_t params_t;

    WZT_t(int N, int S, double p, base::context_t& context)
        : data_type(N, S, p, context), _transform(*this) {

    }

    WZT_t(int N, int S, const params_t& params, base::context_t& context)
        : data_type(N, S, params, context),
          _transform(*this) {

    }

    WZT_t(const boost::property_tree::ptree &pt)
        : data_type(pt), _transform(*this) {

    }

    template< typename OtherInputMatrixType,
              typename OtherOutputMatrixType >
    WZT_t(const WZT_t<OtherInputMatrixType,OtherOutputMatrixType>& other)
        : data_type(other), _transform(*this) {

    }

    WZT_t(const data_type& other)
        : data_type(other), _transform(*this) {

    }

    /**
     * Apply columnwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                columnwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    /**
     * Apply rowwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const typename transform_t::matrix_type& A,
                typename transform_t::output_matrix_type& sketch_of_A,
                rowwise_tag dimension) const {
        _transform.apply(A, sketch_of_A, dimension);
    }

    int get_N() const { return this->_N; } /**< Get input dimesion. */
    int get_S() const { return this->_S; } /**< Get output dimesion. */

    const sketch_transform_data_t* get_data() const { return this; }

private:
    transform_t _transform;
};

template<>
class WZT_t<boost::any, boost::any> :
  public WZT_data_t,
  virtual public sketch_transform_t<boost::any, boost::any > {

public:

    typedef WZT_data_t data_type;
    typedef data_type::params_t params_t;

    WZT_t(int N, int S, double p, base::context_t& context)
        : data_type(N, S, p, context) {

    }

    WZT_t(int N, int S, const params_t& params, base::context_t& context)
        : data_type(N, S, params, context) {

    }

    WZT_t(const boost::property_tree::ptree &pt)
        : data_type(pt) {

    }

    /**
     * Copy constructor
     */
    template <typename OtherInputMatrixType,
              typename OtherOutputMatrixType>
    WZT_t (const WZT_t<OtherInputMatrixType, OtherOutputMatrixType>& other)
        : data_type(other) {

    }

    /**
     * Constructor from data
     */
    WZT_t (const data_type& other)
        : data_type(other) {

    }

    /**
     * Apply columnwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply(const boost::any &A, const boost::any &sketch_of_A,
                columnwise_tag dimension) const {

#if     !(defined SKYLARK_NO_ANY) || (defined SKYLARK_WITH_WZT_ANY)

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::matrix_t, mdtypes::matrix_t,
            WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::sparse_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::shared_matrix_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::root_matrix_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::dist_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::dist_matrix_vc_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::dist_matrix_vr_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
            mdtypes::dist_matrix_star_vc_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
            mdtypes::dist_matrix_star_vr_t, WZT_t);

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::matrix_t, mftypes::matrix_t,
            WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::sparse_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::shared_matrix_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::root_matrix_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::dist_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::dist_matrix_vc_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::dist_matrix_vr_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
            mftypes::dist_matrix_star_vc_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
            mftypes::dist_matrix_star_vr_t, WZT_t);

#endif

        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "This combination has not yet been implemented for WZT"));

    }

    /**
     * Apply rowwise the sketching transform that is described by the
     * the transform with output sketch_of_A.
     */
    void apply (const boost::any &A, const boost::any &sketch_of_A,
        rowwise_tag dimension) const {

#if     !(defined SKYLARK_NO_ANY) || (defined SKYLARK_WITH_WZT_ANY)

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::matrix_t, mdtypes::matrix_t,
            WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::sparse_matrix_t,
            mdtypes::sparse_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::shared_matrix_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::root_matrix_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_t,
            mdtypes::dist_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vc_star_t,
            mdtypes::dist_matrix_vc_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_vr_star_t,
            mdtypes::dist_matrix_vr_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vc_t,
            mdtypes::dist_matrix_star_vc_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mdtypes::dist_matrix_star_vr_t,
            mdtypes::dist_matrix_star_vr_t, WZT_t);

        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::matrix_t, mftypes::matrix_t,
            WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::sparse_matrix_t,
            mftypes::sparse_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::shared_matrix_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::root_matrix_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::root_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::shared_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_t,
            mftypes::dist_matrix_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vc_star_t,
            mftypes::dist_matrix_vc_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_vr_star_t,
            mftypes::dist_matrix_vr_star_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vc_t,
            mftypes::dist_matrix_star_vc_t, WZT_t);
        SKYLARK_SKETCH_ANY_APPLY_DISPATCH(mftypes::dist_matrix_star_vr_t,
            mftypes::dist_matrix_star_vr_t, WZT_t);

#endif

        SKYLARK_THROW_EXCEPTION (
          base::sketch_exception()
              << base::error_msg(
                 "This combination has not yet been implemented for WZT"));

    }

    int get_N() const { return this->_N; } /**< Get input dimesion. */
    int get_S() const { return this->_S; } /**< Get output dimesion. */

    const sketch_transform_data_t* get_data() const { return this; }
};

} } /** namespace skylark::sketch */

#endif // SKYLARK_WZT_HPP


#include <boost/python/class.hpp>
#include <boost/python/def.hpp>

#include <scitbx/matrix/cholesky.h>

namespace scitbx { namespace matrix { namespace boost_python {

  struct matrix_cholesky_gill_murray_wright_decomposition_in_place_wrappers
  {
    typedef matrix::cholesky::gill_murray_wright_decomposition_in_place<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>("gill_murray_wright_cholesky_decomposition_in_place",
                  no_init)
        .def(init<af::shared<double> const &, optional<double> >(
             (arg("packed_u"), arg("epsilon"))))
        .def_readonly("epsilon", &w_t::epsilon)
        .add_property("packed_u", make_getter(&w_t::packed_u, rbv()))
        .add_property("e", make_getter(&w_t::e, rbv()))
        .add_property("pivots", make_getter(&w_t::pivots, rbv()))
        .def("solve", &w_t::solve, (arg("b")))
      ;
    }
  };

  struct cholesky_failure_info_wrapper
  {
    typedef cholesky::failure_info<double> wt;

    static bool nonzero(wt const &self) {
      return self.failed;
    }

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name, no_init)
        .def_readonly("index", &wt::index)
        .def_readonly("value", &wt::value)
        .def("__nonzero__", nonzero)
        ;
    }
  };

  template <class DecompositionType>
  struct cholesky_decomposition_for_python : DecompositionType
  {
    typedef typename DecompositionType::scalar_t scalar_t;
    typedef typename DecompositionType::accessor_type accessor_type;
    typedef af::ref<scalar_t, accessor_type> packed_ref_t;

    af::shared<scalar_t> a_;

    cholesky_decomposition_for_python(af::shared<scalar_t> const &a)
      : DecompositionType(packed_ref_t(const_cast<scalar_t *>(a.begin()),
                                       af::dimension_from_packed_size(a.size()))),
        a_(a) // so as to make a lives longer than this
    {}
  };

  template <class DecompositionType>
  struct cholesky_decomposition_wrapper
  {
    typedef cholesky_decomposition_for_python<DecompositionType> wt;
    typedef typename wt::scalar_t scalar_t;

    static void wrap(char const *name) {
      using namespace boost::python;
      class_<wt>(name, no_init)
        .def(init<af::shared<scalar_t> const &>())
        .def_readonly("failure", &wt::failure)
        .def("solve", &wt::solve)
        ;
    }
  };

  void wrap_cholesky() {
    using namespace boost::python;

    matrix_cholesky_gill_murray_wright_decomposition_in_place_wrappers::wrap();

    cholesky_decomposition_wrapper<
      cholesky::l_l_transpose_decomposition_in_place<double>
    >::wrap("l_l_transpose_cholesky_decomposition_in_place");
    cholesky_decomposition_wrapper<
      cholesky::u_transpose_u_decomposition_in_place<double>
    >::wrap("u_transpose_u_cholesky_decomposition_in_place");

    cholesky_failure_info_wrapper::wrap("cholesky_failure_info");

    def("inverse_of_u_transpose_u",
        cholesky::inverse_of_u_transpose_u<double>);
  }
}}}