#include <iostream>

#include <boost/multiprecision/cpp_dec_float.hpp>

typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<50>> Precise_Decimal;

/*
    Solves two conic sections equations simultaneously.

    It is used to find the intersection points of two conic sections.

    An arbitrary conic section (equation) is of the following form:

    Ax^2 + Bxy + Cy^2 + Dx + Ey + F = 0

    where A, B, C, D, and E are arbitrary coefficients, and F is an arbitrary constant.

    - by Melwyn Francis Carlo <carlo.melwyn@outlook.com>

    - Written for LibreCAD software.
*/
RS_VectorSolutions RS_Math::simultaneousQuadraticSolverFull(const std::vector<std::vector<double>>& input_conicSections)
{
    RS_VectorSolutions intersectionPoints;

    if (input_conicSections.size() != 2) return intersectionPoints;

    if ((input_conicSections[0].size() == 3) || (input_conicSections[1].size() == 3))
    {
        return simultaneousQuadraticSolverMixed(input_conicSections);
    }

    if ((input_conicSections[0].size() != 6) || (input_conicSections[1].size() != 6))
    {
        return intersectionPoints;
    }

    std::vector<std::vector<double>> conicSections = input_conicSections;

    for (int i = 0; i < 2; i++)
    {
        unsigned int max_orderOfMagnitudeFactor = 0;

        for (int j = 0; j < 6; j++)
        {
            if (std::fabs(conicSections[i][j]) < RS_TOLERANCE15)
            {
                conicSections[i][j] = 0.0;
            }

            if (conicSections[i][j] < 1.0)
            {
                const unsigned int orderOfMagnitudeFactor = (unsigned int) std::trunc(std::fabs(std::log10(conicSections[i][j]))) + 1;

                if (orderOfMagnitudeFactor > max_orderOfMagnitudeFactor)
                {
                    max_orderOfMagnitudeFactor = orderOfMagnitudeFactor;
                }
            }
        }

        for (int j = 0; j < 6; j++)
        {
            const double orderOfMagnitude = std::pow(10, max_orderOfMagnitudeFactor);

            conicSections[i][j] = std::trunc(conicSections[i][j] * orderOfMagnitude * 1.0E+10) * 1.0E-10;
        }
    }

    const Precise_Decimal a1 = conicSections[0][0];
    const Precise_Decimal b1 = conicSections[0][1];
    const Precise_Decimal c1 = conicSections[0][2];
    const Precise_Decimal d1 = conicSections[0][3];
    const Precise_Decimal e1 = conicSections[0][4];
    const Precise_Decimal f1 = conicSections[0][5];

    const Precise_Decimal a2 = conicSections[1][0];
    const Precise_Decimal b2 = conicSections[1][1];
    const Precise_Decimal c2 = conicSections[1][2];
    const Precise_Decimal d2 = conicSections[1][3];
    const Precise_Decimal e2 = conicSections[1][4];
    const Precise_Decimal f2 = conicSections[1][5];

    if ((a1 == 0.0) || (a2 == 0.0) || (c1 == 0.0) || (c2 == 0.0)) return intersectionPoints;

    const Precise_Decimal a1_sq = a1 * a1;
    const Precise_Decimal b1_sq = b1 * b1;
    const Precise_Decimal c1_sq = c1 * c1;
    const Precise_Decimal d1_sq = d1 * d1;
    const Precise_Decimal e1_sq = e1 * e1;
    const Precise_Decimal f1_sq = f1 * f1;

    const Precise_Decimal a2_sq = a2 * a2;
    const Precise_Decimal b2_sq = b2 * b2;
    const Precise_Decimal c2_sq = c2 * c2;
    const Precise_Decimal d2_sq = d2 * d2;
    const Precise_Decimal e2_sq = e2 * e2;
    const Precise_Decimal f2_sq = f2 * f2;

    const Precise_Decimal c1_cu = c1_sq * c1;

    std::vector<double> x_n_terms(5, 0.0);

    /* The x^4 term. */
    x_n_terms[4] = ((a1_sq * c1 * c2_sq)  + (a2_sq * c1_cu)  - (2.0 * a1 * a2 * c1_sq * c2)
                 + (a2 * b1_sq * c1 * c2) - (a2 * b1 * b2 * c1_sq)
                 + (a1 * b2_sq * c1_sq)  - (a1 * b1 * b2 * c1 * c2)).convert_to<double>();

    /* The x^3 term. */
    x_n_terms[3] = ((2.0 * a1 * c1 * c2_sq * d1) + (2.0 * a2 * c1_cu * d2) - (2.0 * a1 * c1_sq * c2 * d2) - (2.0 * a2 * c1_sq * c2 * d1)
                 + (2.0 * a2 * b1 * c1 * c2 * e1) + (b1_sq * c1 * c2 * d2) - (a2 * b1 * c1_sq * e2) - (a2 * b2 * c1_sq * e1) - (b1 * b2 * c1_sq * d2)
                 + (2.0 * a1 * b2 * c1_sq * e2) + (b2_sq * c1_sq * d1) - (a1 * b1 * c1 * c2 * e2) - (a1 * b2 * c1 * c2 * e1) - (b1 * b2 * c1 * c2 * d1)).convert_to<double>();

    /* The x^2 term. */
    x_n_terms[2] = ((2.0 * a1 * c1 * c2_sq * f1) + (2.0 * a2 * c1_cu * f2) + (c1_cu * d2_sq)  + (c1 * c2_sq * d1_sq)  - (2.0 * a1 * c1_sq * c2 * f2) - (2.0 * a2 * c1_sq * c2 * f1) - (2.0 * c1_sq * c2 * d1 * d2)
                 + (a2 * c1 * c2 * e1_sq)  + (2.0 * b1 * c1 * c2 * d2 * e1) + (b1_sq * c1 * c2 * f2) - (a2 * c1_sq * e1 * e2) - (b1 * b2 * c1_sq * f2) - (b1 * c1_sq * d2 * e2) - (b2 * c1_sq * d2 * e1)
                 + (a1 * c1_sq * e2_sq)  + (b2_sq * c1_sq * f1) + (2.0 * b2 * c1_sq * d1 * e2) - (a1 * c1 * c2 * e1 * e2) - (b1 * b2 * c1 * c2 * f1) - (b1 * c1 * c2 * d1 * e2) - (b2 * c1 * c2 * d1 * e1)).convert_to<double>();

    /* The x term. */
    x_n_terms[1] = ((2.0 * c1_cu * d2 * f2) + (2.0 * c1 * c2_sq * d1 * f1) - (2.0 * c1_sq * c2 * d1 * f2) - (2.0 * c1_sq * c2 * d2 * f1)
                 + (2.0 * b1 * c1 * c2 * e1 * f2) + (c1 * c2 * d2 * e1_sq)  - (b1 * c1_sq * e2 * f2) - (b2 * c1_sq * e1 * f2) - (c1_sq * d2 * e1 * e2)
                 + (c1_sq * d1 * e2_sq)  + (2.0 * b2 * c1_sq * e2 * f1) - (b1 * c1 * c2 * e2 * f1) - (b2 * c1 * c2 * e1 * f1) - (c1 * c2 * d1 * e1 * e2)).convert_to<double>();

    /* The constant term. */
    x_n_terms[0] = ((c1_cu * f2_sq)  + (c1 * c2_sq * f1_sq)  - (2.0 * c1_sq * c2 * f1 * f2)
                 + (c1 * c2 * e1_sq * f2) - (c1_sq * e1 * e2 * f2)
                 + (c1_sq * e2_sq * f1) - (c1 * c2 * e1 * e2 * f1)).convert_to<double>();

    if (RS_DEBUG->getLevel() >= RS_Debug::D_INFORMATIONAL)
    {
        DEBUG_HEADER
        std::cout << std::endl << std::endl
                  << " (" << x_n_terms[4] << ")x^4 + "
                  <<  "(" << x_n_terms[3] << ")x^3 + "
                  <<  "(" << x_n_terms[2] << ")x^2 + "
                  <<  "(" << x_n_terms[1] << ")x + "
                  <<  "(" << x_n_terms[0] << ") = 0"
                  << std::endl << std::endl;

        const double a = x_n_terms[4];
        const double b = x_n_terms[3];
        const double c = x_n_terms[2];
        const double d = x_n_terms[1];
        const double e = x_n_terms[0];

        const double a_sq = a * a;
        const double b_sq = b * b;
        const double c_sq = c * c;
        const double d_sq = d * d;
        const double e_sq = e * e;

        const double a_cu = a * a * a;
        const double b_cu = b * b * b;
        const double c_cu = c * c * c;
        const double d_cu = d * d * d;
        const double e_cu = e * e * e;

        std::cout << " Discriminant (Delta) = "
                  << (256.0 * a_cu * e_cu) - (192.0 * a_sq * b * d * e_sq) - (128.0 * a_sq * c_sq * e_sq)
                   + (144.0 * a_sq * c * d_sq * e) - (27.0 * a_sq * d_cu * d) + (144.0 * a * b_sq * c * e_sq)
                   - (6.0 * a * b_sq * d_sq * e) - (80.0 * a * b * c_sq * d * e) + (18.0 * a * b * c * d_cu)
                   + (16.0 * a * c_cu * c * e) - (4.0 * a * c_cu * d_sq) - (27.0 * b_cu * b * e_sq)
                   + (18.0 * b_cu * c * d * e) - (4.0 * b_cu * d_cu) - (4.0 * b_sq * c_cu * e) + (b_sq * c_sq * d_sq)
                  << std::endl << std::endl;

        std::cout << " P Factor = "
                  << (8.0 * a * c) - (3.0 * b_sq)
                  << std::endl << std::endl;

        std::cout << " D Factor = "
                  << (64.0 * a_cu * e) - (16.0 * a_sq * c_sq) + (16.0 * a * b_sq * c) - (16.0 * a_sq * b * d) - (3 * b_cu * b)
                  << std::endl << std::endl;
    }

	std::vector<double> rootsAbscissae = quarticSolverFull(x_n_terms);

    if (RS_DEBUG->getLevel() >= RS_Debug::D_INFORMATIONAL)
    {
        std::cout << " Number of quartic roots = " << rootsAbscissae.size()
                  << std::endl << std::endl;
    }

    for (double& rootX : rootsAbscissae)
    {
        const Precise_Decimal rootXPrecise = rootX;

        const Precise_Decimal sqrtTerm1 = boost::multiprecision::trunc(((((b1 * rootXPrecise) + e1) * ((b1 * rootXPrecise) + e1))
                                        - (4.0 * c1 * ((a1 * rootXPrecise * rootXPrecise) + (d1 * rootXPrecise) + f1))) * 1.0E+4) * 1.0E-4;

        const Precise_Decimal sqrtTerm2 = boost::multiprecision::trunc(((((b2 * rootXPrecise) + e2) * ((b2 * rootXPrecise) + e2))
                                        - (4.0 * c2 * ((a2 * rootXPrecise * rootXPrecise) + (d2 * rootXPrecise) + f2))) * 1.0E+4) * 1.0E-4;

        if (RS_DEBUG->getLevel() >= RS_Debug::D_INFORMATIONAL)
        {
            std::cout << " RootX square root terms : "
                      << sqrtTerm1 << ", " << sqrtTerm2
                      << std::endl << std::endl;
        }

        if ((sqrtTerm1 < 0.0) || (sqrtTerm2 < 0.0)) continue;

        const Precise_Decimal numeratorTerm1 = (-b1 * rootXPrecise) - e1;
        const Precise_Decimal numeratorTerm2 = (-b2 * rootXPrecise) - e2;

        const Precise_Decimal denominatorTerm1 = 2.0 * c1;
        const Precise_Decimal denominatorTerm2 = 2.0 * c2;

        const RS_Vector conic_1_points[2] =
        {
            RS_Vector(rootX, ((numeratorTerm1 + boost::multiprecision::sqrt(sqrtTerm1)) / denominatorTerm1).convert_to<double>()),
            RS_Vector(rootX, ((numeratorTerm1 - boost::multiprecision::sqrt(sqrtTerm1)) / denominatorTerm1).convert_to<double>())
        };

        const RS_Vector conic_2_points[2] =
        {
            RS_Vector(rootX, ((numeratorTerm2 + boost::multiprecision::sqrt(sqrtTerm2)) / denominatorTerm2).convert_to<double>()),
            RS_Vector(rootX, ((numeratorTerm2 - boost::multiprecision::sqrt(sqrtTerm2)) / denominatorTerm2).convert_to<double>())
        };

        if (((fabs(conic_1_points[0].x - conic_2_points[0].x) < 1.0E-4)
        &&   (fabs(conic_1_points[0].y - conic_2_points[0].y) < 1.0E-4))
        ||  ((fabs(conic_1_points[0].x - conic_2_points[1].x) < 1.0E-4)
        &&   (fabs(conic_1_points[0].y - conic_2_points[1].y) < 1.0E-4)))
        {
            intersectionPoints.push_back(conic_1_points[0]);
        }

        if (((fabs(conic_1_points[1].x - conic_2_points[0].x) < 1.0E-4)
        &&   (fabs(conic_1_points[1].y - conic_2_points[0].y) < 1.0E-4))
        ||  ((fabs(conic_1_points[1].x - conic_2_points[1].x) < 1.0E-4)
        &&   (fabs(conic_1_points[1].y - conic_2_points[1].y) < 1.0E-4)))
        {
            intersectionPoints.push_back(conic_1_points[1]);
        }

        if (RS_DEBUG->getLevel() >= RS_Debug::D_INFORMATIONAL)
        {
            std::cout << " Available roots : " << std::endl
                      << " 1.1. " << conic_1_points[0] << std::endl
                      << " 1.2. " << conic_1_points[1] << std::endl
                      << " 2.1. " << conic_2_points[0] << std::endl
                      << " 2.2. " << conic_2_points[1] << std::endl << std::endl;

            std::cout << " Chosen root : " << intersectionPoints [intersectionPoints.size() - 1]
                      << std::endl << std::endl;
        }
    }

    if (RS_DEBUG->getLevel() >= RS_Debug::D_INFORMATIONAL)
    {
        std::cout << " Number of intersection points = " << intersectionPoints.size()
                  << std::endl << std::endl;
    }

    return intersectionPoints;
}


//
// Created by alex on 26/05/20.
//

#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <numeric>
#include <boost/algorithm/string.hpp>
#include <fstream>
#include "Utils.h"

using namespace std;

vector<vector<int>> Utils::loadGEucSpace(const string &file_path) {
    auto vertices = readVList(file_path);
    int n = vertices.size();
    vector<vector<int>> D(n, vector<int>(n));

    for (size_t i = 0; i < n; ++i) {
        for (size_t j = i + 1; j < n; ++j) {

            auto v1 = vertices[i];
            auto v2 = vertices[j];

            // computing euclidean distance
            vector<float> subtracted;
            subtracted.reserve(v1.size());
            transform(v1.begin(), v1.end(), v2.begin(), back_inserter(subtracted),
                           [](float a, float b) { return pow(a - b, 2); });
            double d = sqrt(accumulate(subtracted.begin(), subtracted.end(), 0.0));
            D[j][i] = D[i][j] = (int)(d+0.5);
        }
    }
    return D;
}

std::vector<std::vector<float>> Utils::readVList(const std::string &file_path) {

    std::vector<std::vector<float>> vertices;
    std::string line;
    std::ifstream file(file_path);

    if (file.is_open()) {

        getline(file, line);
        boost::trim_right(line);
        boost::trim_left(line);

        std::vector<std::string> line_vec;
        boost::split(line_vec, line, boost::is_any_of(" "));

        if (line_vec.size() != 1 && line_vec.size() != 3) {
            std::cerr << "Error in line " << line << std::endl;
            throw;
        }

        std::vector<float> xy(2);
        if (line_vec.size() == 1) {
            int n = stoi(line);
            vertices.reserve(n);
        } else {
            std::transform(line_vec.begin() + 1, line_vec.end(), xy.begin(),
                           [](std::string const &val) { return stof(val); });
            vertices.push_back(xy);
        }

        while (getline(file, line)) {
            boost::trim_right(line);
            boost::trim_left(line);
            boost::split(line_vec, line, boost::is_any_of(" "));

            if (line_vec.size() != 3) {
                std::cerr << "Error in line " << line << std::endl;
                throw;
            }

            std::transform(line_vec.begin() + 1, line_vec.end(), xy.begin(),
                           [](std::string const &val) { return stof(val); });
            vertices.push_back(xy);
        }
        file.close();
    } else {
        std::cerr << "Unable to open file" << std::endl;
    }
    return vertices;
}

std::vector<std::vector<float>> Utils::loadGMetricSpace(int n, const std::string &file_path) {
    std::vector<std::vector<float>> G(n, std::vector<float>(n));
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < n; ++j) {
            G[i][j] = i != j ? +INFINITY : 0;
        }
    }

    std::string line;
    std::ifstream file(file_path);

    if (file.is_open()) {

        std::vector<std::string> line_vec;
        while (getline(file, line)) {
            boost::trim_right(line);
            boost::trim_left(line);
            boost::split(line_vec, line, boost::is_any_of(" "));

            if (line_vec.size() != 3) {
                std::cerr << "Error in line " << line << std::endl;
                throw;
            }

            int v1 = stoi(line_vec[0]) - 1;
            int v2 = stoi(line_vec[1]) - 1;
            float w = stof(line_vec[2]);

            G[v1][v2] = w;
            G[v2][v1] = w;
        }
        file.close();
    } else {
        std::cerr << "Unable to open file" << std::endl;
    }
    floydWarshall(G);
    return G;
}

void Utils::floydWarshall(std::vector<std::vector<float>> &G) {
    int n = G.size();
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j < n; ++j) {
            for (int l = 0; l < n; ++l) {
                float cost = (G[i][j] == +INFINITY || G[i][l] == +INFINITY) ?
                             +INFINITY : G[i][j] + G[i][l];
                if (cost < G[j][l]) {
                    G[j][l] = cost;
                }
            }
        }
    }
}

float Utils::stdDev(std::vector<int> &items, float average) {
    float std = 0;
    for (float item : items) {
        std += pow(item - average, 2);
    }
    int n = items.size() > 2 ? items.size() - 1 : items.size();
    return sqrt(std / n);
}

bool Utils::save(std::string &output_path, std::string &content) {
    std::ofstream output_file(output_path);
//    output_file.write(&content, content.size());
    output_file << content;
    output_file.close();
    return true;
}


#include <armadillo>
#include "zeroSumGame.h"
#include "gurobi_c++.h"

using namespace arma;
using namespace std;

bool ZeroSumGame::solveGame(const Mat<double> &payoffMat, double &gameValue, Col<double> &p, Col<double> &q, double diffThreshold, bool checkNonnegative)
{
    double maximumGain = 0;
    double minimumLoss = 0;
    if(!maximizePayoff(payoffMat,maximumGain,p,checkNonnegative)) return false;
    if(!minimizePayoff(payoffMat,minimumLoss,q,checkNonnegative)) return false;

    Col<double> absValues(2);
    absValues(0) = abs(maximumGain);
    absValues(1) = abs(minimumLoss);

    double abs_gain_loss_diff = abs(maximumGain-minimumLoss);
    double diff_portion = abs_gain_loss_diff/max(absValues);
    if (diff_portion < diffThreshold) {
        gameValue = (maximumGain+minimumLoss)/2;
        return true;
    }
    return false;
}

bool ZeroSumGame::maximizePayoff(const Mat<double> &payoffMat, double &maximumGain, Col<double> &p, bool checkNonnegative)
{
    if (checkNonnegative==true) {
        double minValuePayoffMat = min(min(payoffMat));
        if (minValuePayoffMat <= 0) {
            cout << "all elements of the payoffMat must be positive!" << endl;
            throw 2;
        }
    }

    int numRows = payoffMat.n_rows;
    int numCols = payoffMat.n_cols;

    bool success = false;
    GRBEnv* env = 0;
    try{
    env = new GRBEnv();
    GRBModel model = GRBModel(*env);
    /* No solving information printed */
    model.getEnv().set(GRB_IntParam_OutputFlag, 0);
    /* Add varialbe to the model */
    /* first NULL means low bound is 0.0 and seconde NULL means up bound is infinite by default */
    GRBVar* vars = model.addVars(NULL,NULL,NULL,NULL,NULL,numRows);
    model.update();

    /* Populate payoffMat matrix */
    for (int j = 0; j < numCols; j++) {
        GRBLinExpr lhs = 0;
        for (int i = 0; i < numRows; i++)
            if(payoffMat(i,j) != 0)
                lhs += payoffMat(i,j)*vars[i];
        model.addConstr(lhs,GRB_GREATER_EQUAL,1.0);
    }

    GRBLinExpr obj = 0;

    for (int i = 0; i<numRows; i++)
        obj += 1.0*vars[i];

    model.setObjective(obj,GRB_MINIMIZE);
    model.update();
    model.optimize();

    if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL) {
        maximumGain = 1.0/model.get(GRB_DoubleAttr_ObjVal);
        p = zeros< Col<double> >(numRows);
        for (int i = 0; i < numRows; i++)
            p(i) = vars[i].get(GRB_DoubleAttr_X)*maximumGain;
        success = true;
    }

    //cout << "maximum gain:" << maximumGain << endl;
    delete [] vars;

    }catch(GRBException e) {
        cout << "Error code = " << e.getErrorCode() << endl;
        cout << e.getMessage() << endl;
    } catch(...) {
        cout << "Exception during optimization" << endl;
    }
    delete env;
    return success;
}

bool ZeroSumGame::minimizePayoff(const Mat<double> &payoffMat, double &minimumLoss, Col<double> &q, bool checkNonnegative)
{
    if (checkNonnegative==true) {
        double minValuePayoffMat = min(min(payoffMat));
        if (minValuePayoffMat <= 0) {
            cout << "all elements of the payoffMat must be positive!" << endl;
            throw 2;
        }
    }
    int numRows = payoffMat.n_rows;
    int numCols = payoffMat.n_cols;

    bool success = false;
    GRBEnv* env = 0;
    try{
        env = new GRBEnv();
        GRBModel model = GRBModel(*env);
        /* No solving information printed */
        model.getEnv().set(GRB_IntParam_OutputFlag, 0);
        /* Add varialbe to the model */
        /* first NULL means low bound is 0.0 and seconde NULL means up bound is infinite by default */
        GRBVar* vars = model.addVars(NULL,NULL,NULL,NULL,NULL,numCols);
        model.update();

        /* Populate A matrix */
        for (int i = 0; i < numRows; i++) {
            GRBLinExpr lhs = 0;
            for (int j = 0; j < numCols; j++)
                if(payoffMat(i,j) != 0)
                    lhs += payoffMat(i,j)*vars[j];
            model.addConstr(lhs,GRB_LESS_EQUAL,1.0);
        }

        GRBLinExpr obj = 0;

        for (int j = 0; j<numCols; j++)
            obj += 1.0*vars[j];

        model.setObjective(obj,GRB_MAXIMIZE);
        model.update();
        model.optimize();

        if (model.get(GRB_IntAttr_Status) == GRB_OPTIMAL) {
            minimumLoss = 1.0/model.get(GRB_DoubleAttr_ObjVal);
            q = zeros< Col<double> >(numCols);
            for (int j = 0; j < numCols; j++)
                q(j) = vars[j].get(GRB_DoubleAttr_X)*minimumLoss;
            success = true;
        }

        //cout << "minimum loss:" << minimumLoss << endl;

        delete [] vars;

    }catch(GRBException e) {
        cout << "Error code = " << e.getErrorCode() << endl;
        cout << e.getMessage() << endl;
    } catch(...) {
        cout << "Exception during optimization" << endl;
    }
    delete env;
    return success;
}


#include <iostream>
#include <vector>
#include <functional>
#include <cmath>
#include <Eigen/Dense>

#include "Derivative.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;

using Eigen::Derivative;

using std::function;
using std::vector;

typedef function<double(VectorXd)> FuncDV;
typedef function<VectorXd(VectorXd)> FuncVV;
typedef function<MatrixXd(VectorXd)> FuncMV;

MatrixXd MakePositiveSemidefinite(MatrixXd mat, int dim){
    double left = 0, right = std::max(-mat.minCoeff(), .0) + 1;
    while(right - left >= 1e-4){
        double mid = (left+right)/2;
        auto test = mat + mid*MatrixXd::Identity(dim, dim);
        // From https://stackoverflow.com/questions/35227131/eigen-check-if-matrix-is-positive-semi-definite

        Eigen::LLT<Eigen::MatrixXd> lltOfA(test);
        if(lltOfA.info() == Eigen::NumericalIssue)
            left = mid;
        else
            right = mid;
    }
    return mat + right*MatrixXd::Identity(dim, dim);
}

VectorXd doIPM(
    FuncDV F, FuncVV DelF, FuncMV LaplaceF,
    FuncVV H, FuncMV DelH, vector<FuncMV> LaplaceH,
    int dimX, int dimH, VectorXd initX
){
    double mu = 0.0001;

    std::function<bool(VectorXd)> feasible = [dimH](VectorXd _f){
        for(int lx = 0;lx < dimH;lx++)
            if(_f[lx] < 0)
                return false;
        return true;
    };

    std::function<double(VectorXd, VectorXd, VectorXd)>
        L = [mu, F, H, dimH](VectorXd x, VectorXd y, VectorXd w){
        double barrier = 0;
        for(int lx = 0;lx < dimH;lx++)
            barrier += log(w[lx]);
        return F(x) - mu*barrier - y.transpose()*(H(x) - w);
    };

    VectorXd x = initX, y, w = H(initX);

    {
        MatrixXd A = DelH(x).transpose();
        MatrixXd invA = A.completeOrthogonalDecomposition().pseudoInverse();
        y = invA*DelF(x);
    }

    for(;;){
        VectorXd h = H(x), e = VectorXd::Ones(dimH);
        MatrixXd W = w.asDiagonal(), invY = y.asDiagonal().inverse();

        MatrixXd Hess = LaplaceF(x);
        for(int lx = 0;lx < dimH;lx++)
            Hess -= y[lx]*LaplaceH[lx](x);

        // Find ~H = H + lambda * I
        Hess = MakePositiveSemidefinite(Hess, dimX);

        MatrixXd A = DelH(x);
        MatrixXd M1(dimX + dimH, dimX + dimH);

        M1 << -Hess, A.transpose(),
                  A, W*invY       ;

        VectorXd V1(dimX + dimH);

        V1 << DelF(x) - A.transpose()*y,
                  - h + mu*invY*e      ;

        VectorXd dxy = M1.inverse()*V1;
        VectorXd dx, dy, dw;

        dx = dxy.head(dimX);
        dy = dxy.tail(dimH);
        dw = invY*mu*e - W*e - invY*W*dy;

        x += dx, y += dy, w += dw;

        if(dx.norm() <= 0.001)
            break;
    }

    return x;
}

// Solve : min  obj_f
//         sub  con_hs >= 0

VectorXd Derivative_IPM(Derivative obj_f, vector<Derivative> con_hs, VectorXd start_guess){
    int x_size = start_guess.size(), h_size = con_hs.size();

    vector<Derivative> v1w(x_size);
    vector< vector<Derivative> > v2w(x_size, v1w);

    vector<Derivative> f_gradient = v1w;
    vector< vector<Derivative> > f_hess = v2w;

    vector< vector<Derivative> > hs_gradient(h_size, v1w);
    vector< vector< vector<Derivative> > > hs_hess(h_size, v2w);

    // Prebuild differiential function

    for(int lx = 0;lx < x_size;lx++)
        f_gradient[lx] = obj_f.diffPartial(lx);

    for(int lx = 0;lx < x_size;lx++)
        for(int ly = 0;ly < x_size;ly++)
            f_hess[lx][ly] = f_gradient[lx].diffPartial(ly);

    for(int lh = 0;lh < h_size;lh++){
        for(int lx = 0;lx < x_size;lx++)
            hs_gradient[lh][lx] = con_hs[lh].diffPartial(lx);

        for(int lx = 0;lx < x_size;lx++)
            for(int ly = 0;ly < x_size;ly++)
                hs_hess[lh][lx][ly] = hs_gradient[lh][lx].diffPartial(ly);
    }

    FuncDV F = [obj_f](VectorXd x){
        return obj_f(x);
    };

    FuncVV DelF = [f_gradient, x_size](VectorXd x){
        VectorXd ret(x_size);
        for(int lx = 0;lx < x_size;lx++)
            ret[lx] = f_gradient[lx](x);
        return ret;
    };

    FuncMV LaplaceF = [f_hess, x_size](VectorXd x){
        MatrixXd ret(x_size, x_size);
        for(int lx = 0;lx < x_size;lx++)
            for(int ly = 0;ly < x_size;ly++)
                ret(lx, ly) = f_hess[lx][ly](x);
        return ret;
    };

    FuncVV H = [con_hs, h_size](VectorXd x){
        VectorXd ret(h_size);
        for(int lx = 0;lx < h_size;lx++)
            ret[lx] = con_hs[lx](x);
        return ret;
    };

    FuncMV DelH = [hs_gradient, h_size, x_size](VectorXd x){
        MatrixXd A(h_size, x_size);
        for(int lh = 0;lh < h_size;lh++)
            for(int lx = 0;lx < x_size;lx++)
                A(lh, lx) = hs_gradient[lh][lx](x);
        return A;
    };

    vector<FuncMV> LaplaceH(h_size);

    for(int lh = 0;lh < h_size;lh++){
        LaplaceH[lh] = [hs_hess, lh, x_size](VectorXd x){
            MatrixXd ret(x_size, x_size);
            for(int lx = 0;lx < x_size;lx++)
                for(int ly = 0;ly < x_size;ly++)
                    ret(lx, ly) = hs_hess[lh][lx][ly](x);
            return ret;
        };
    };

    return doIPM(F, DelF, LaplaceF, H, DelH, LaplaceH, start_guess.size(), con_hs.size(), start_guess);
}

int main(){
    // Reference : http://www.princeton.edu/~rvdb/tex/talks/MLSS_LaPalma/LaPalma3.pdf
    // Solve : min  x+y
    //         sub  xx + yy >= 1
    //              x >= 0
    //              y >= 0

    Derivative par_x = Derivative::Variable(0), par_y = Derivative::Variable(1);
    Derivative obj_f = par_x + par_y;

    Derivative con_h1 = par_x*par_x + par_y*par_y - 1,
               con_h2 = par_x,
               con_h3 = par_y;

    // Multiple initial value
    VectorXd x(2);

    x << 1, 1;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << 1, 2;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << 2, 1;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << 4, -1;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << -1, 4;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << 0.2, 0.7;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;

    x << 0.5, 0.5;
    std::cout << "x initial as " << x.transpose() << std::endl;
    std::cout << Derivative_IPM(obj_f, {con_h1, con_h2, con_h3}, x).transpose() << std::endl;
    return 0;
}


#ifndef TRIUMF_SRIM_PDF_HPP
#define TRIUMF_SRIM_PDF_HPP

#include <boost/math/constants/constants.hpp>
// #include <boost/math/distributions/lognormal.hpp>
// #include <boost/math/distributions/skew_normal.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

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

#include <cmath>

// TRIUMF: Canada's accelerator centre
namespace triumf {

// Stopping and Range of Ions in Matter (SRIM)
namespace srim {

// probability density functions (PDF)
namespace pdf {

/// see Eq. (5.6) in Md Masrur Hossain's MSc thesis, UBC, 2006, p. 52.
template <typename T> T custom(T x, T alpha, T beta, T sigma, T x_max, T N) {
  T y = x / x_max;
  return y < 0.0 or y > 1.0 ? 0.0
                            : N * std::pow(y, alpha) * std::pow(1.0 - y, beta) *
                                  std::exp(-std::pow((x - x_max) / sigma, 2));
}

/// see Eq. (5.6) in Md Masrur Hossain's MSc thesis, UBC, 2006, p. 52.
template <typename T> T custom(const T *x, const T *par) {
  // parameters
  T alpha = par[0];
  T beta = par[1];
  T sigma = par[2];
  T x_max = par[3];
  T N = par[4];
  T y = *x / x_max;

  // ensure function is always properly normalized!
  boost::math::quadrature::tanh_sinh<T> integrator;
  auto integrand = [&](T z) { return custom(z, alpha, beta, sigma, x_max, N); };
  T I = integrator.integrate(integrand, 0.0, x_max);

  // return the normalized function
  return custom(*x, alpha, beta, sigma, x_max, N) / I;
}

/// beta distribution - x in [0, 1]
template <typename T> T _beta(T x, T alpha, T beta) {
  if ((x < 0.0) or (x > 1.0)) {
    return 0.0;
  } else {
    return std::pow(x, alpha - 1.0) * std::pow(1.0 - x, beta - 1.0) /
           boost::math::beta<T, T>(alpha, beta);
  }
}

/// modified beta distribution - x in [0, x_max]
template <typename T> T modified_beta(T x, T alpha, T beta, T x_max) {
  T y = x / x_max;
  return _beta<T>(y, alpha, beta) / x_max;
}

/// modified beta distribution - x in [0, x_max]
/// interface for ROOT TF1
template <typename T> T modified_beta(const T *x, const T *par) {
  T alpha = par[0];
  T beta = par[1];
  T x_max = par[2];
  return modified_beta<T>(*x, alpha, beta, x_max);
}

} // namespace pdf

} // namespace srim

} // namespace triumf

#endif // TRIUMF_SRIM_PDF_HPP


#include <rct_optimizations/validation/homography_validation.h>
#include <Eigen/Dense>
#include <rct_optimizations/types.h>

namespace rct_optimizations
{
Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspondences,
                                         const CorrespondenceSampler &correspondence_sampler)
{
  /* There is a 3x3 homography matrix, H, that can transform a point from one plane onto a different plane
   * | u | = k * | H00 H01 H02 | * | x |
   * | v |       | H10 H11 H12 |   | y |
   * | 1 |       | H20 H12  1  |   | 1 |
   *
   * In our case we have 2 sets of known corresponding planar points: points on the planar target, and points in the image plane
   * Therefore, there is some matrix, H, which can transform target points into the image plane.
   * If the target points and camera points actually match, we should be able to:
   *   1. Calculate H for a subset of corresponding points
   *   2. Transform the remaining target points by H to obtain estimates of their locations in the image plane
   *   3. Compare the calculated estimations to the actual image points to make sure they are very close. If they are not close, we know that the correspondences are not valid
   *
   * The matrix H has 8 unique values.
   * These 8 values of the homography matrix can be solved for, given a set of (at least) 8 corresponding planar vectors, by rearranging the above equations:
   *
   * A * H = b, where
   * H = inv(A) * b
   *   - A is matrix (size 2*n x 8), where n is the number of corresponding vectors
   *   - H is a vector (size 8 x 1) of the unknown elements of the homography matrix
   *   - B is a vector (size 2*n x 1) representing the elements of one set of planar vectors
   *
   *                  A               *              H    =    b
   * |-x0 -y0  -1   0    0    0   u0*x0   u0*y0 | * | H00 | = | -u0 |
   * | 0   0   0  -x0   -y0  -1   v0*x0   v0*y0 | * | H01 | = | -v0 |
   *                              ...
   * |-x7 -y7  -1   0    0    0   u7*x7   u7*y7 | * | H20 | = | -u7 |
   * | 0   0   0  -x7   -y7  -1   v7*x7   v7*y7 | * | H21 | = | -v7 |
   *
   */

  // Select the points that we want to use to create the H matrix
  std::vector<std::size_t> sample_correspondence_indices = correspondence_sampler
                                                             .getSampleCorrespondenceIndices();
  std::size_t n_samples = sample_correspondence_indices.size();

  // Ensure that there are enough points for testing outside of the sampled set
  if (correspondences.size() < 2 * n_samples)
  {
    std::stringstream ss;
    ss << "Correspondences size is not more than 2x sample size (" << correspondences.size()
       << " correspondences vs. " << n_samples << ")";
    throw std::runtime_error(ss.str());
  }

  // Create the A and b matrices
  Eigen::MatrixXd A(2 * n_samples, 8);
  Eigen::MatrixXd b(2 * n_samples, 1);

  // Fill the A and B matrices with data from the selected correspondences
  for (std::size_t i = 0; i < n_samples; ++i)
  {
    std::size_t corr_idx = sample_correspondence_indices.at(i);
    const Correspondence2D3D &corr = correspondences.at(corr_idx);

    //assign A row-th row:
    const double x = corr.in_target.x();
    const double y = corr.in_target.y();
    const double u = corr.in_image.x();
    const double v = corr.in_image.y();
    A.row(2 * i) << -x, -y, -1.0, 0.0, 0.0, 0.0, u * x, u * y;
    A.row(2 * i + 1) << 0.0, 0.0, 0.0, -x, -y, -1.0, v * x, v * y;

    b.block<2, 1>(2 * i, 0) = -1.0 * corr.in_image;
  }

  // Create the homography matrix
  Eigen::Matrix<double, 3, 3, Eigen::RowMajor> H = Eigen::Matrix3d::Ones();

  // Map the elements of the H matrix into a column vector and solve for the first 8
  {
    Eigen::Map<Eigen::VectorXd> hv(H.data(), 9);
    hv.head<8>() = A.fullPivLu().solve(b);
  }

  // Estimate the image locations of all the target observations and compare to the actual image locations
  Eigen::VectorXd error(correspondences.size());
  for (std::size_t i = 0; i < correspondences.size(); ++i)
  {
    const Correspondence2D3D &corr = correspondences[i];

    // Calculate the scaling factor
    double ki = 1.0 / (H(2, 0) * corr.in_target.x() + H(2, 1) * corr.in_target.y() + 1.0);

    // Replace the z-element of the point with 1
    Eigen::Vector3d xy(corr.in_target);
    xy(2) = 1.0;

    // Estimate the point in the image plane
    Eigen::Vector3d in_image_estimate = ki * H * xy;

    // Calculate the error
    Eigen::Vector2d image_error = corr.in_image - in_image_estimate.head<2>();
    error(i) = image_error.norm();
  }

  return error;
}

} // namespace rct_optimizations


#ifndef LINEAR_ELASTICITY_HPP
#define LINEAR_ELASTICITY_HPP

#include "C0_triangles.hpp"
#include "Galerkin/Galerkin.hpp"
#include "mesh_traits.hpp"
#include "SymSparse.hpp"

using Galerkin::get;

#include <Eigen/Core>

#include <exception>

namespace Elasticity
{

namespace TwoD
{

template <class Element, class U, class V>
Eigen::Matrix2d pair_stiffness_integral(const Element &el, const U &u, const V &v,
    double lambda, double mu)
{
    auto uxvx = el.integrate(el.template partial<0>(u) * el.template partial<0>(v));
    auto uxvy = el.integrate(el.template partial<0>(u) * el.template partial<1>(v));
    auto uyvy = el.integrate(el.template partial<1>(u) * el.template partial<1>(v));
    auto uyvx = el.integrate(el.template partial<1>(u) * el.template partial<0>(v));

    Eigen::Matrix2d K;
    K(0, 0) = (lambda + 2*mu) * uxvx + mu * uyvy;
    K(1, 0) = lambda * uyvx + mu * uxvy;
    K(0, 1) = lambda * uxvy + mu * uyvx;
    K(1, 1) = (lambda + 2*mu) * uyvy + mu * uxvx;

    return K;
}

template <class Element>
constexpr size_t basis_size = Element::basis.size();

template <auto I, auto J, class Element>
void add_contribution(const Element &el, double lambda, double mu,
                      Eigen::Matrix<double, 2*basis_size<Element>, 2*basis_size<Element>> &K)
{
    constexpr auto u = get<I>(Element::basis);
    constexpr auto v = get<J>(Element::basis);
    auto local_K = pair_stiffness_integral(el, u, v, lambda, mu);
    auto row = I*2;
    auto col = J*2;
    K.block(row, col, 2, 2) = local_K;
    K.block(col, row, 2, 2) = local_K.transpose();
}

template <class Element>
Eigen::Matrix<double, 2*basis_size<Element>, 2*basis_size<Element>>
element_stiffness_matrix(const Element &el, double lambda, double mu)
{
    Eigen::Matrix<double, 2*basis_size<Element>, 2*basis_size<Element>> K;
    Galerkin::static_for<0, basis_size<Element>, 1>(
        [&](auto I)
        {
            Galerkin::static_for<I(), basis_size<Element>, 1>(
                [&](auto J)
                {
                    add_contribution<I(), J()>(el, lambda, mu, K);
                }
            );
        }
    );
    return K;
}

template <class Mesh>
auto instantiate_element(const Mesh &mesh, size_t which)
{
    constexpr int order = msh::element_order<Mesh>;
    const auto &el = mesh.element(which);
    return fem::c0::C0Triangle<order>(
        mesh.coord(el.control_nodes[0]),
        mesh.coord(el.control_nodes[1]),
        mesh.coord(el.control_nodes[2])
    );
}

constexpr auto lame_parameters(double E, double nu) noexcept
{
    double lambda = E * nu / ((1+nu) * (1-nu));
    double mu     = E / (2 * (1+nu));
    return std::make_pair(lambda, mu);
}

template <class Mesh>
auto assemble_stiffness(const Mesh &mesh, double E, double nu)
{
    SymSparse::SymmetricSparseMatrix<double, 2 * msh::max_node_adjacencies<Mesh>> K(mesh.num_nodes() * 2);
    auto [lambda, mu] = lame_parameters(E, nu);

    for (size_t i = 0; i < mesh.num_elements(); ++i)
    {
        const auto &el_info = mesh.element(i);
        const auto nn = el_info.node_numbers();
        const auto el = instantiate_element(mesh, i);
        const auto local_K = element_stiffness_matrix(el, lambda, mu);
        for (size_t j = 0; j < nn.size(); ++j)
        {
            for (size_t k = j; k < nn.size(); ++k)
            {
                K.insert_entry(nn[j]*2, nn[k]*2, local_K(2*j, 2*k));
                if (k != j)
                {
                    K.insert_entry(nn[j]*2+1, nn[k]*2, local_K(2*j+1, 2*k));
                }
                K.insert_entry(nn[j]*2, nn[k]*2+1, local_K(2*j, 2*k+1));
                K.insert_entry(nn[j]*2+1, nn[k]*2+1, local_K(2*j+1, 2*k+1));
            }
        }
    }
    return K;
}

template <class Mesh>
using StiffnessType = decltype(assemble_stiffness(std::declval<const Mesh &>(), 1.0, 1.0));

/*
 * Add a homogeneous Dirichlet condition on the boundary segment of the mesh
 * indexed by `which`. K is the stiffness matrix obtained from `assemble_stiffness`,
 * and `rhs` is the forcing vector.
 */
template <class Mesh, class RHS>
void impose_homogeneous_condition(const Mesh &mesh, StiffnessType<Mesh> &K, RHS &rhs, size_t which,
                                  double scale = 1.0)
{
    std::vector<size_t> adjacent;
    adjacent.reserve(2 * msh::max_node_adjacencies<Mesh>);
    const auto &boundary = mesh.boundary(which);

    for (auto n: boundary.nodes)
    {
        adjacent.clear();
        // Get all of the adjacent DOFs to this one; since there are two components
        // of displacement there are 2 degrees of freedom (2*n, 2*n+1) corresponding
        // to each node.
        for (auto n2: mesh.adjacent_nodes(n))
        {
            adjacent.push_back(2*n2);
            adjacent.push_back(2*n2+1);
        }
        K.eliminate_dof(2*n, 0.0, scale, rhs, adjacent);
        K.eliminate_dof(2*n+1, 0.0, scale, rhs, adjacent);
    }
}

template <class Mesh>
void impose_homogeneous_condition(
    const Mesh &mesh, StiffnessType<Mesh> &K, size_t which, double scale = 1.0)
{
    std::vector<size_t> adjacent;
    adjacent.reserve(2 * msh::max_node_adjacencies<Mesh>);
    const auto &boundary = mesh.boundary(which);

    for (auto n : boundary.nodes)
    {
        adjacent.clear();
        // Get all of the adjacent DOFs to this one; since there are two components
        // of displacement there are 2 degrees of freedom (2*n, 2*n+1) corresponding
        // to each node.
        for (auto n2 : mesh.adjacent_nodes(n))
        {
            adjacent.push_back(2 * n2);
            adjacent.push_back(2 * n2 + 1);
        }
        K.eliminate_dof(2 * n, 0.0, scale, adjacent);
        K.eliminate_dof(2 * n + 1, 0.0, scale, adjacent);
    }
}

struct OutOfBoundsIndex : public std::exception
{
    const char *msg;
    OutOfBoundsIndex(const char *m) : msg(m) {}
    const char *what() const noexcept
    {
        return msg;
    }
};

template <class Mesh, class RHS, class Vector>
void impose_dirichlet_condition(const Mesh &mesh, StiffnessType<Mesh> &K, RHS &rhs, size_t which,
                                const Vector &value, double scale = 1.0)
{
    std::vector<size_t> adjacent;
    adjacent.reserve(2 * msh::max_node_adjacencies<Mesh>);
    const auto &boundary = mesh.boundary(which);

    if (value.size() != 2*boundary.nodes.size())
    {
        throw OutOfBoundsIndex("Vector given for Dirichlet condition has wrong number of values");
    }

    size_t i = 0;
    for (auto n: boundary.nodes)
    {
        adjacent.clear();
        for (auto n2: mesh.adjacent_nodes(n))
        {
            adjacent.push_back(2*n2);
            adjacent.push_back(2*n2+1);
        }
        K.eliminate_dof(2*n, value[2*i], scale, rhs, adjacent);
        K.eliminate_dof(2*n+1, value[2*i+1], scale, rhs, adjacent);
        i += 1;
    }
}

template <class Force, class RHS>
void add_point_force(const Force &force, size_t node, RHS &F)
{
    if (2*node+1 > F.size())
    {
        throw OutOfBoundsIndex("Node is out of bounds for given forcing vector");
    }
    F[2*node] += force[0];
    F[2*node+1] += force[1];
}

template <class T, class RHS, int N, class IndexContainer, int... Options>
void add_point_forces(const Eigen::Matrix<T, 2, N, Options...> &force, const IndexContainer &nodes,
                      RHS &F)
{
    static_assert(sizeof...(Options) == 3);
    size_t col = 0;
    for (size_t n: nodes)
    {
        add_point_force(force.col(col), n, F);
        col += 1;
    }
}

} // namespace 2D

} // namespace Elasticity

#endif // LINEAR_ELASTICITY_HPP


// Copyright (C) Microsoft. All rights reserved.
// Licensed under the MIT License. See LICENSE in the project root for license information.

#include <pch.h>
#include <Eigen_Z.h>

////////////////////////////////////////////////////////////////////////////////
// File: jacobi_cyclic_method.c                                               //
// Routines:                                                                  //
//    Jacobi_Cyclic_Method                                                    //
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
//  void Jacobi_Cyclic_Method                                                 //
//            (double eigenvalues[], double *eigenvectors, double *A, int n)  //
//                                                                            //
//  Description:                                                              //
//     Find the eigenvalues and eigenvectors of a symmetric n x n matrix A    //
//     using the Jacobi method. Upon return, the input matrix A will have     //
//     been modified.                                                         //
//     The Jacobi procedure for finding the eigenvalues and eigenvectors of a //
//     symmetric matrix A is based on finding a similarity transformation     //
//     which diagonalizes A.  The similarity transformation is given by a     //
//     product of a sequence of orthogonal (rotation) matrices each of which  //
//     annihilates an off-diagonal element and its transpose.  The rotation   //
//     effects only the rows and columns containing the off-diagonal element  //
//     and its transpose, i.e. if a[i][j] is an off-diagonal element, then    //
//     the orthogonal transformation rotates rows a[i][] and a[j][], and      //
//     equivalently it rotates columns a[][i] and a[][j], so that a[i][j] = 0 //
//     and a[j][i] = 0.                                                       //
//     The cyclic Jacobi method considers the off-diagonal elements in the    //
//     following order: (0,1),(0,2),...,(0,n-1),(1,2),...,(n-2,n-1).  If the  //
//     the magnitude of the off-diagonal element is greater than a treshold,  //
//     then a rotation is performed to annihilate that off-diagnonal element. //
//     The process described above is called a sweep.  After a sweep has been //
//     completed, the threshold is lowered and another sweep is performed     //
//     with the new threshold. This process is completed until the final      //
//     sweep is performed with the final threshold.                           //
//     The orthogonal transformation which annihilates the matrix element     //
//     a[k][m], k != m, is Q = q[i][j], where q[i][j] = 0 if i != j, i,j != k //
//     i,j != m and q[i][j] = 1 if i = j, i,j != k, i,j != m, q[k][k] =       //
//     q[m][m] = cos(phi), q[k][m] = -sin(phi), and q[m][k] = sin(phi), where //
//     the angle phi is determined by requiring a[k][m] -> 0.  This condition //
//     on the angle phi is equivalent to                                      //
//               cot(2 phi) = 0.5 * (a[k][k] - a[m][m]) / a[k][m]             //
//     Since tan(2 phi) = 2 tan(phi) / (1.0 - tan(phi)^2),                    //
//               tan(phi)^2 + 2cot(2 phi) * tan(phi) - 1 = 0.                 //
//     Solving for tan(phi), choosing the solution with smallest magnitude,   //
//       tan(phi) = - cot(2 phi) + sgn(cot(2 phi)) sqrt(cot(2phi)^2 + 1).     //
//     Then cos(phi)^2 = 1 / (1 + tan(phi)^2) and sin(phi)^2 = 1 - cos(phi)^2 //
//     Finally by taking the sqrts and assigning the sign to the sin the same //
//     as that of the tan, the orthogonal transformation Q is determined.     //
//     Let A" be the matrix obtained from the matrix A by applying the        //
//     similarity transformation Q, since Q is orthogonal, A" = Q'AQ, where Q'//
//     is the transpose of Q (which is the same as the inverse of Q).  Then   //
//         a"[i][j] = Q'[i][p] a[p][q] Q[q][j] = Q[p][i] a[p][q] Q[q][j],     //
//     where repeated indices are summed over.                                //
//     If i is not equal to either k or m, then Q[i][j] is the Kronecker      //
//     delta.   So if both i and j are not equal to either k or m,            //
//                                a"[i][j] = a[i][j].                         //
//     If i = k, j = k,                                                       //
//        a"[k][k] =                                                          //
//           a[k][k]*cos(phi)^2 + a[k][m]*sin(2 phi) + a[m][m]*sin(phi)^2     //
//     If i = k, j = m,                                                       //
//        a"[k][m] = a"[m][k] = 0 =                                           //
//           a[k][m]*cos(2 phi) + 0.5 * (a[m][m] - a[k][k])*sin(2 phi)        //
//     If i = k, j != k or m,                                                 //
//        a"[k][j] = a"[j][k] = a[k][j] * cos(phi) + a[m][j] * sin(phi)       //
//     If i = m, j = k, a"[m][k] = 0                                          //
//     If i = m, j = m,                                                       //
//        a"[m][m] =                                                          //
//           a[m][m]*cos(phi)^2 - a[k][m]*sin(2 phi) + a[k][k]*sin(phi)^2     //
//     If i= m, j != k or m,                                                  //
//        a"[m][j] = a"[j][m] = a[m][j] * cos(phi) - a[k][j] * sin(phi)       //
//                                                                            //
//     If X is the matrix of normalized eigenvectors stored so that the ith   //
//     column corresponds to the ith eigenvalue, then AX = X Lamda, where     //
//     Lambda is the diagonal matrix with the ith eigenvalue stored at        //
//     Lambda[i][i], i.e. X'AX = Lambda and X is orthogonal, the eigenvectors //
//     are normalized and orthogonal.  So, X = Q1 Q2 ... Qs, where Qi is      //
//     the ith orthogonal matrix,  i.e. X can be recursively approximated by  //
//     the recursion relation X" = X Q, where Q is the orthogonal matrix and  //
//     the initial estimate for X is the identity matrix.                     //
//     If j = k, then x"[i][k] = x[i][k] * cos(phi) + x[i][m] * sin(phi),     //
//     if j = m, then x"[i][m] = x[i][m] * cos(phi) - x[i][k] * sin(phi), and //
//     if j != k and j != m, then x"[i][j] = x[i][j].                         //
//                                                                            //
//  Arguments:                                                                //
//     double  eigenvalues                                                    //
//        Array of dimension n, which upon return contains the eigenvalues of //
//        the matrix A.                                                       //
//     double* eigenvectors                                                   //
//        Matrix of eigenvectors, the ith column of which contains an         //
//        eigenvector corresponding to the ith eigenvalue in the array        //
//        eigenvalues.                                                        //
//     double* A                                                              //
//        Pointer to the first element of the symmetric n x n matrix A. The   //
//        input matrix A is modified during the process.                      //
//     int     n                                                              //
//        The dimension of the array eigenvalues, number of columns and rows  //
//        of the matrices eigenvectors and A.                                 //
//                                                                            //
//  Return Values:                                                            //
//     Function is of type void.                                              //
//                                                                            //
//  Example:                                                                  //
//     #define N                                                              //
//     double A[N][N], double eigenvalues[N], double eigenvectors[N][N]       //
//                                                                            //
//     (your code to initialize the matrix A )                                //
//                                                                            //
//     Jacobi_Cyclic_Method(eigenvalues, (double*)eigenvectors,               //
//                                                          (double *) A, N); //
////////////////////////////////////////////////////////////////////////////////
//                                                                            //

#define VAL_FLT_EPSILON     2.2204460492503131e-016f//1.0e-4f//1.192092896e-07F        /* smallest such that 1.0+FLT_EPSILON != 1.0 */

void Jacobi_Cyclic_Method(float eigenvalues[], float *eigenvectors,float *A, int n)
{
   int i, j, k, m;
   float *pAk, *pAm, *p_r, *p_e;
   double threshold_norm;
   double threshold;
   double tan_phi, sin_phi, cos_phi, tan2_phi, sin2_phi, cos2_phi;
   double sin_2phi, cos_2phi, cot_2phi;
   double dum1;
   double dum2;
   double dum3;
   double max;

				  // Take care of trivial cases

   if ( n < 1) return;
   if ( n == 1) {
	  eigenvalues[0] = *A;
	  *eigenvectors = 1.f;
	  return;
   }

		  // Initialize the eigenvalues to the identity matrix.

   for (p_e = eigenvectors, i = 0; i < n; i++)
	  for (j = 0; j < n; p_e++, j++)
		 if (i == j) *p_e = 1.f; else *p_e = 0.f;

			// Calculate the threshold and threshold_norm.

   for (threshold = 0.f, pAk = A, i = 0; i < ( n - 1 ); pAk += n, i++)
	  for (j = i + 1; j < n; j++) threshold += *(pAk + j) * *(pAk + j);
   threshold = sqrt(threshold + threshold);
   threshold_norm = threshold * VAL_FLT_EPSILON;
   max = threshold + 1.f;
   while (threshold > threshold_norm)
   {
//   for (int il=0; il<100; il++) {
	  threshold /= 10.f;
	  if (max < threshold)
		  continue;
	  max = 0.f;
	  for (pAk = A, k = 0; k < (n-1); pAk += n, k++) {
		 for (pAm = pAk + n, m = k + 1; m < n; pAm += n, m++) {
			double fabspAkpm=fabs(*(pAk + m));
			if ( fabspAkpm < threshold )
				continue;

				 // Calculate the sin and cos of the rotation angle which
				 // annihilates A[k][m].

			cot_2phi = 0.5f * ( *(pAk + k) - *(pAm + m) ) / *(pAk + m);
			dum1 = sqrt( cot_2phi * cot_2phi + 1.f);
			if (cot_2phi < 0.f) dum1 = -dum1;
			tan_phi = -cot_2phi + dum1;
			tan2_phi = tan_phi * tan_phi;
			sin2_phi = tan2_phi / (1.f + tan2_phi);
			cos2_phi = 1.f - sin2_phi;
			sin_phi = sqrt(sin2_phi);
			if (tan_phi < 0.f) sin_phi = - sin_phi;
			cos_phi = sqrt(cos2_phi);
			sin_2phi = 2.f * sin_phi * cos_phi;
			cos_2phi = cos2_phi - sin2_phi;

					 // Rotate columns k and m for both the matrix A
					 //     and the matrix of eigenvectors.

			p_r = A;
			dum1 = *(pAk + k);
			dum2 = *(pAm + m);
			dum3 = *(pAk + m);
			*(pAk + k) = dum1 * cos2_phi + dum2 * sin2_phi + dum3 * sin_2phi;
			*(pAm + m) = dum1 * sin2_phi + dum2 * cos2_phi - dum3 * sin_2phi;
			*(pAk + m) = 0.f;
			*(pAm + k) = 0.f;
			for (i = 0; i < n; p_r += n, i++) {
			   if ( (i == k) || (i == m) ) continue;
			   if ( i < k ) dum1 = *(p_r + k); else dum1 = *(pAk + i);
			   if ( i < m ) dum2 = *(p_r + m); else dum2 = *(pAm + i);
			   dum3 = dum1 * cos_phi + dum2 * sin_phi;
			   if ( i < k ) *(p_r + k) = dum3; else *(pAk + i) = dum3;
			   dum3 = - dum1 * sin_phi + dum2 * cos_phi;
			   if ( i < m ) *(p_r + m) = dum3; else *(pAm + i) = dum3;
			}
			for (p_e = eigenvectors, i = 0; i < n; p_e += n, i++) {
			   dum1 = *(p_e + k);
			   dum2 = *(p_e + m);
			   *(p_e + k) = dum1 * cos_phi + dum2 * sin_phi;
			   *(p_e + m) = - dum1 * sin_phi + dum2 * cos_phi;
			}
		 }
		 for (i = 0; i < n; i++)
			if ( i == k ) continue;
			else if ( max < fabs(*(pAk + i))) max = fabs(*(pAk + i));
	  }
   }
   for (pAk = A, k = 0; k < n; pAk += n, k++) eigenvalues[k] = *(pAk + k);
}

void GenericEigen3D(const Vec3f *vectors,S32 nbvectors,Vec3f eigenvectors[3],Float eigenvalues[3])
{
	int		i;

	if (nbvectors<1)
		return;

	Vec3f	BaryCentre=VEC4F_NULL;

	// Calculate mean (barycentre)
	for (i=0; i<nbvectors; i++)
	{
		BaryCentre+=vectors[i];
	}

	BaryCentre/=(Float)nbvectors;

	Float	MatCovariance[3][3];
	// Compute cloud point directing vector
	// 1st compute covariance matrix
	for (int x=0; x<3; x++)
	{
		for (int y=0; y<3; y++)
		{
			Float	totalcovariance=0.f;
			for (i=0; i<nbvectors; i++)
			{
				Float	valx=(Float)(((Float*)&vectors[i])[x]);
				Float	valy=(Float)(((Float*)&vectors[i])[y]);
				valx-=((Float*)&BaryCentre)[x];
				valy-=((Float*)&BaryCentre)[y];
				Float	covariance=valx*valy;
				totalcovariance+=covariance;
			}
			Float	covariance=totalcovariance/nbvectors;
			MatCovariance[x][y]=covariance;
		}
	}
	// Compute EigenVectors
	Jacobi_Cyclic_Method(eigenvalues,(float *)eigenvectors,(Float*)&MatCovariance,3);

	Swap(eigenvectors[0].y, eigenvectors[1].x);
	Swap(eigenvectors[0].z, eigenvectors[2].x);
	Swap(eigenvectors[1].z, eigenvectors[2].y);
}


// Example implementation of an objective function class for linear regression
// and usage of the L-BFGS optimizer.
//
// Compilation:
// g++ example.cpp -o example -O3 -larmadillo

#include <iostream>
#include <armadillo>
#include <ensmallen.hpp>

class LinearRegressionFunction
{
  public:

  LinearRegressionFunction(arma::mat& X, arma::vec& y) : X(X), y(y) { }

  double EvaluateWithGradient(const arma::mat& theta, arma::mat& gradient)
  {
    const arma::vec tmp = X.t() * theta - y;
    gradient = 2 * X * tmp;
    return arma::dot(tmp,tmp);
  }

  private:

  const arma::mat& X;
  const arma::vec& y;
};

int main(int argc, char** argv)
{
  if (argc < 3)
  {
    std::cout << "usage: " << argv[0] << " n_dims n_points" << std::endl;
    return -1;
  }

  int n_dims   = atoi(argv[1]);
  int n_points = atoi(argv[2]);

  // generate noisy dataset with a slight linear pattern
  arma::mat X(n_dims, n_points, arma::fill::randu);
  arma::vec y(        n_points, arma::fill::randu);

  for (size_t i = 0; i < n_points; ++i)
  {
    double a = arma::randu();
    X(1, i) += a;
    y(i)    += a;
  }

  LinearRegressionFunction lrf(X, y);

  // create a Limited-memory BFGS optimizer object with default parameters
  ens::L_BFGS opt;
  opt.MaxIterations() = 10;

  // initial point (uniform random)
  arma::vec theta(n_dims, arma::fill::randu);

  opt.Optimize(lrf, theta);

  // theta now contains the optimized parameters
  theta.print("theta:");

  return 0;
}


#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <array>
#include "/Users/drewlewis/software/install/tiledarray/sparse_new_summa_debug/include/tiledarray.h"

using Tensor = TiledArray::Tensor<double>;
using Perm = TiledArray::Permutation;
using Range = TiledArray::Range;
using Matrix =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

template <typename... Args>
TiledArray::Range make_range(Args... args) {
    return TiledArray::Range(args...);
}

Tensor vectors(Tensor const &a) {
    auto const &size = a.range().size();
    Eigen::Map<const Matrix> map(a.data(), size[0], size[1] * size[2]);

    Eigen::JacobiSVD<Matrix> svd(map,
                                 Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto const &vals = svd.singularValues();

    auto rank = 0;
    for (auto i = 0; i < vals.size(); ++i) {
        if (1e-12 < vals[i]) {
            ++rank;
        }
    }

    auto matrixU = svd.matrixU().leftCols(rank);
    Tensor U{Range(matrixU.rows(), matrixU.cols())};

    for (auto i = 0; i < matrixU.size(); ++i) {
        U[i] = *(matrixU.data() + i);
    }

    return U;
}

Tensor Kron(Tensor const &a, Tensor const &b) {
    auto const &asize = a.range().size();
    auto const &bsize = b.range().size();
    const auto ab0 = asize[0] * bsize[0];
    const auto ab1 = asize[1] * bsize[1];

    Tensor out(Range(ab0, ab1));

    Eigen::Map<Matrix> outm(out.data(), ab0, ab1);
    Eigen::Map<const Matrix> am(a.data(), asize[0], asize[1]);
    Eigen::Map<const Matrix> bm(b.data(), bsize[0], bsize[1]);

    outm = Matrix::Zero(ab0, ab1);
    std::cout << "outm = \n" << outm << std::endl;

    auto row_start = 0;
    for (auto i = 0; i < am.rows(); ++i) {
        auto col_start = 0;
        for (auto j = 0; j < am.cols(); ++j) {
            outm.block(row_start, col_start, bsize[0], bsize[1]) = am(i, j) * bm;
            col_start += bsize[1];
        }
        row_start += bsize[0];
        std::cout << "outm = \n" << std::endl;
    }

    return out;
}

Tensor FormS(Tensor const &U1, Tensor const &A, Tensor const &K) {
    auto const &usize = U1.range().size();
    auto const &asize = A.range().size();
    auto const &ksize = K.range().size();
    Eigen::Map<const Matrix> um(U1.data(), usize[0], usize[1]);
    Eigen::Map<const Matrix> am(A.data(), asize[0], asize[1] * asize[2]);
    Eigen::Map<const Matrix> km(K.data(), ksize[0], ksize[1]);

    Matrix outtemp(usize[1], asize[1] * asize[2]);
    outtemp = um.transpose() * am;

    Tensor out{Range(usize[1], asize[1], asize[2])};
    Eigen::Map<Matrix> outm(out.data(), usize[1], asize[1] * asize[2]);
    outm = outtemp * km;
    return out;
}

Tensor FormA(Tensor const &U1, Tensor const &S, Tensor const &K) {
    auto const &usize = U1.range().size();
    auto const &ssize = S.range().size();
    auto const &ksize = K.range().size();
    Eigen::Map<const Matrix> um(U1.data(), usize[0], usize[1]);
    Eigen::Map<const Matrix> sm(S.data(), ssize[0], ssize[1] * ssize[2]);
    Eigen::Map<const Matrix> km(K.data(), ksize[0], ksize[1]);

    Matrix outtemp(usize[0], ssize[1] * ssize[2]);
    outtemp = um * sm;
    std::cout << outtemp << std::endl;

    Tensor out{Range(usize[0], ssize[1], ssize[2])};
    Eigen::Map<Matrix> outm(out.data(), usize[0], ssize[1] * ssize[2]);
    outm = outtemp * km.transpose();
    return out;
}

int main(int argc, char **argv) {
    auto tensor = Tensor{make_range(3, 2, 2)};
    for (auto i = 1; i <= 12; ++i) {
        tensor[i - 1] = i;
    }
    auto u1 = vectors(tensor);
    auto u2 = vectors(Tensor{tensor, Perm{1, 0, 2}});
    auto u3 = vectors(Tensor{tensor, Perm{2, 1, 0}});

    std::cout << tensor << std::endl;
    std::cout << u1 << std::endl;
    std::cout << u2 << std::endl;
    std::cout << u3 << std::endl;

    auto K = Kron(u3,u2);
    std::cout << "K = \n" << K << std::endl;
    auto S = FormS(u1, tensor, K);
    std::cout << "S = \n" << S << std::endl;
    auto Tapprox = FormA(u1, S, K);
    std::cout << "Aapprox = \n" << Tapprox << std::endl;

    return 0;
}


// Adapted from:
// * http://www.stanford.edu/~acoates/quaternion.h
// * http://www.euclideanspace.com/maths/geometry/rotations/conversions

#pragma once

#include "vector-2.hpp"
#include "vector-3.hpp"
#include "vector-4.hpp"
#include <cmath>
#include <iostream>

#include <Eigen/Dense>

namespace perceive
{
#pragma pack(push, 1)
template<typename T> class QuaternionT
{
 public:
   using value_type = T;

   T x, y, z, w;

   QuaternionT() noexcept
       : x(0.0)
       , y(0.0)
       , z(0.0)
       , w(1.0)
   {}

   QuaternionT(const Vector3T<T>& v, T w_) noexcept
       : x(v.x)
       , y(v.y)
       , z(v.z)
       , w(w_)
   {}

   QuaternionT(const Vector4T<T>& v) noexcept { from_axis_angle(v); }

   QuaternionT(const T array[4]) noexcept
   {
      x = array[0];
      y = array[1];
      z = array[2];
      w = array[3];
   }

   QuaternionT(T x_, T y_, T z_, T w_) noexcept
       : x(x_)
       , y(y_)
       , z(z_)
       , w(w_)
   {}

   QuaternionT(const Vector3T<T>& a, const Vector3T<T>& b) noexcept
   {
      *this = between_vectors(a, b);
   }

   bool operator==(const QuaternionT& rhs) const noexcept
   {
      const auto o       = this->normalised();
      const auto p       = rhs.normalised();
      const auto cos_phi = p.x * o.x + p.y * o.y + p.z * o.z + p.w * o.w;
      return fabs(cos_phi) > 1.0 - 1e-9;
   }

   bool operator!=(const QuaternionT& rhs) const noexcept
   {
      return !(*this == rhs);
   }

   static inline QuaternionT identity() noexcept
   {
      return QuaternionT(0.0, 0.0, 0.0, 1.0);
   }

   static QuaternionT nan() noexcept
   {
      constexpr T val = T(NAN);
      return QuaternionT(val, val, val, val);
   }

   // Create from between vectors
   static inline QuaternionT between_vectors(const Vector3T<T>& a,
                                             const Vector3T<T>& b) noexcept
   {
      auto u     = a.normalised();
      auto v     = b.normalised();
      auto n     = cross(u, v);
      auto cos_t = dot(u, v);
      if(cos_t < (1e-6 - 1.0)) {
         // 180 degree rotation... about arbitrary axis
         Vector3T<T> axis;
         axis = cross(Vector3T<T>(1, 0, 0), u);
         if(axis.quadrance() < 1e-3) axis = cross(Vector3T<T>(0, 1, 0), u);
         axis.normalise();
         return QuaternionT(axis, 0);
      } else if(cos_t >= 1.0 - 1e-6) {
         return QuaternionT(0, 0, 0, 1);
      }
      QuaternionT q(n, 1.0 + cos_t);
      q.normalise();
      return q;
   }

   // Create from between rotations: r * a = b
   static inline QuaternionT between_rotations(const QuaternionT<T>& a,
                                               const QuaternionT<T>& b) noexcept
   {
      return b * a.conjugate();
   }

   Vector3T<T>& complex() noexcept
   {
      return *(reinterpret_cast<Vector3T<T>*>(this));
   }
   const Vector3T<T>& complex() const noexcept
   {
      return *(reinterpret_cast<const Vector3T<T>*>(this));
   }
   T& real() noexcept { return w; }
   const T& real() const noexcept { return w; }
   QuaternionT conjugate() const noexcept { return QuaternionT(-x, -y, -z, w); }

   QuaternionT inverse() const noexcept { return conjugate().normalised(); }
   QuaternionT& invert() const noexcept
   {
      x = -x;
      y = -y;
      z = -z;
      return normalise();
   }

   T quadrance() const noexcept { return x * x + y * y + z * z + w * w; }
   T norm() const noexcept { return std::sqrt(quadrance()); }

   QuaternionT normalised(T epsilon = T(1e-9)) const noexcept
   {
      QuaternionT ret(*this);
      ret.normalise(epsilon);
      return ret;
   }

   QuaternionT& normalise(T epsilon = T(1e-9)) noexcept
   {
      auto mag2 = quadrance();
      if(std::fabs(mag2 - T(1.0)) > epsilon) {
         auto inv = T(1.0) / T(std::sqrt(mag2));
         x *= inv;
         y *= inv;
         z *= inv;
         w *= inv;
      }
      return *this;
   }

   unsigned size() const noexcept { return 4; }

   T* copy_to(T a[4]) const noexcept
   {
      a[0] = x;
      a[1] = y;
      a[2] = z;
      a[3] = w;
      return a;
   }

   T* ptr() noexcept { return &x; }
   const T* ptr() const noexcept { return &x; }
   T& operator[](int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator()(int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   static void product(const QuaternionT<T>& a,
                       const QuaternionT<T>& b,
                       QuaternionT<T>& c) noexcept
   {
      c.x = a.y * b.z - a.z * b.y + a.x * b.w + a.w * b.x;
      c.y = a.z * b.x - a.x * b.z + a.y * b.w + a.w * b.y;
      c.z = a.x * b.y - a.y * b.x + a.z * b.w + a.w * b.z;
      c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
   }

   QuaternionT<T> operator*(const QuaternionT<T>& rhs) const noexcept
   {
      QuaternionT<T> q;
      product(*this, rhs, q);
      return q;
   }
   QuaternionT<T>& operator*=(const QuaternionT<T>& rhs) noexcept
   {
      *this = *this * rhs;
      return *this;
   }

   QuaternionT<T> operator*(T s) const noexcept
   {
      auto q = *this;
      q *= s;
      return q;
   }
   QuaternionT<T>& operator*=(T s) noexcept
   {
      x *= s;
      y *= s;
      z *= s;
      w *= s;
      return *this;
   }
   QuaternionT<T> operator/(T s) const noexcept { return *this * T(1.0) /= s; }
   QuaternionT<T>& operator/=(T s) noexcept { return *this *= T(1.0) / s; }

   QuaternionT<T> operator+(const QuaternionT<T>& o) const noexcept
   {
      auto q = *this;
      q += o;
      return q;
   }
   QuaternionT<T>& operator+=(const QuaternionT<T>& o) noexcept
   {
      x += o.x;
      y += o.y;
      z += o.z;
      w += o.w;
      return *this;
   }
   QuaternionT<T> operator-(const QuaternionT<T>& o) const noexcept
   {
      auto q = *this;
      q -= o;
      return q;
   }
   QuaternionT<T>& operator-=(const QuaternionT<T>& o) noexcept
   {
      x -= o.x;
      y -= o.y;
      z -= o.z;
      w -= o.w;
      return *this;
   }

   // Same as rotation-maxtrix3x3 * v
   Vector3T<T> rotate(const Vector3T<T>& v) const noexcept
   {
      return (((*this) * QuaternionT(v, 0)) * inverse()).complex();
   }

   Vector3T<T> inverse_rotate(const Vector3T<T>& v) const noexcept
   {
      return conjugate().rotate(v);
   }

   Vector3T<T> apply(const Vector3T<T>& v) const noexcept { return rotate(v); }
   Vector3T<T> inverse_apply(const Vector3T<T>& v) const noexcept
   {
      return inverse_rotate(v);
   }

   // -- spherical-axis-angle
   Vector3T<T> to_spherical_axis_angle() const noexcept
   {
      Vector3T<T> a;
      auto aa = to_axis_angle();
      a.x     = std::acos(aa.z);        // inclination
      a.y     = std::atan2(aa.y, aa.x); // azimuth
      a.z     = aa.w;                   // theta
      return a;
   }

   QuaternionT<T>& from_spherical_axis_angle(const Vector3T<T>& a) noexcept
   {
      // a(inclination, azimuth, amount-of-rotation)
      const T half_theta     = T(0.5) * a.z;
      const T sin_half_theta = std::sin(half_theta);
      x                      = std::sin(a.x) * std::cos(a.y) * sin_half_theta;
      y                      = std::sin(a.x) * std::sin(a.y) * sin_half_theta;
      z                      = std::cos(a.x) * sin_half_theta;
      w                      = std::cos(half_theta);
      return *this;
   }

   QuaternionT& from_axis_angle(const Vector3T<T>& a,
                                const double theta) noexcept
   {
      const T sin_t = std::sin(0.5 * theta);
      const T cos_t = std::cos(0.5 * theta);
      auto b        = a.normalised();
      x             = b.x * sin_t;
      y             = b.y * sin_t;
      z             = b.z * sin_t;
      w             = cos_t;
      normalise();
      return *this;
   }

   QuaternionT& from_axis_angle(const Vector4T<T>& a) noexcept
   {
      return from_axis_angle(a.xyz(), a.d());
   }

   Vector4T<T> to_axis_angle() const noexcept
   {
      if(!is_finite()) return Vector4T<T>::nan();
      auto mag = quadrance();
      if(std::fabs(mag) < T(1e-9)) return Vector4T<T>(1, 0, 0, 0);
      T n_inv = T(1.0) / std::sqrt(mag);

      Vector4T<T> a;
      if(std::fabs(w - T(1.0)) < T(1e-9)) {
         // There is no rotation, so set the axis
         // to an arbitrary value, and theta to 0
         a = Vector4T<T>(1, 0, 0, 0);
      } else {
         auto ww = w * n_inv;
         T theta = T(2.0) * std::acos(ww); // angle of rotation
         T s_inv = T(1.0) / std::sqrt(T(1.0) - ww * ww);
         a.xyz() = complex() * s_inv * n_inv;
         a.w     = T(2.0) * std::acos(ww);
      }
      return a;
   }

   T theta() const noexcept { return to_axis_angle().w; }
   Vector3T<T> axis() const noexcept { return to_axis_angle().xyz(); }

   void set_theta(T f) noexcept { from_axis_angle(axis(), f); }
   void set_axis(const Vector3T<T>& a) noexcept { from_axis_angle(a, theta()); }

   bool is_nan() const noexcept
   {
      return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w);
   }
   bool is_finite() const noexcept
   {
      return std::isfinite(x) && std::isfinite(y) && std::isfinite(z)
             && std::isfinite(w);
   }
   bool is_unit_quaternion(T epsilon = 1e-9) const noexcept
   {
      return is_finite() && fabs(quadrance() - 1.0) < epsilon;
   }

   // -- String shim --
   std::string to_string(const char* fmt = "[{}, {}, {}, {}]") const
   {
      return format(fmt, x, y, z, w);
   }
   std::string to_str() const { return to_string("[{}, {}, {}, {}]"); }

   void print(const char* msg = NULL, bool newline = true) const
   {
      printf("{}{}{}{}",
             (msg == NULL ? "" : msg),
             (msg == NULL ? "" : " "),
             to_string().c_str(),
             (newline ? "\n" : ""));
      fflush(stdout);
   }

   std::string to_readable_str() const
   {
      auto aa = to_axis_angle();
      return format("aa = [{:7.5f}, {:7.5f}, {:7.5f}], theta = {:7.3f} degrees",
                    aa(0),
                    aa(1),
                    aa(2),
                    to_degrees(aa(3)));
   }
};
#pragma pack(pop)

// Scalar multiplication
template<typename T> QuaternionT<T> operator*(float a, const QuaternionT<T>& v)
{
   return v * a;
}
template<typename T> QuaternionT<T> operator/(float a, const QuaternionT<T>& v)
{
   return v / a;
}
template<typename T> QuaternionT<T> operator*(double a, const QuaternionT<T>& v)
{
   return v * a;
}
template<typename T> QuaternionT<T> operator/(double a, const QuaternionT<T>& v)
{
   return v / a;
}

// ------------------- String shim
template<typename T> std::string str(const QuaternionT<T>& v)
{
   return v.to_string();
}
template<typename T>
std::ostream& operator<<(std::ostream& out, const QuaternionT<T>& v)
{
   out << v.to_string();
   return out;
}

} // namespace perceive


// neural_network.cpp example program using posit arithmetic for a neural network simulation
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.
#include "common.hpp"
// include the posit number system
#include <universal/posit/posit>
#define MTL_WITH_INITLIST 1
#include <boost/numeric/mtl/mtl.hpp>

#undef NOW
#ifdef NOW
template<size_t nbits, size_t es>
sw::unum::posit<nbits, es> Sigmoid_(sw::unum::posit<nbits, es>& x, bool derivative = false) {
	if (derivative) return x*(1-x);
	return (1);
}

template<typename Scalar>
Scalar sigmoid(Scalar x) {
	return (1.0f / (1.0f + exp(-x)));
}

template<typename Vector, typename Scalar>
Scalar f_theta(Scalar x, Scalar b, const Vector& V, const Vector& W) {
	double result = b;
	for (int i = 0; i < N; ++i) {
		result += V[i] * sigmoid(c[i] + W[i] * x);
	}
	return result;
}

template<typename Vector, typename Scalar>
void train(Scalar x, Scalar y, Vector& W, Vector& V, Vector& c) {
	for (int i = 0; i < size(W); ++i) {
		W[i] = W[i] - epsilon * 2 * (f_theta(x) - y) * V[i] * x *
			(1 - sigmoid(c[i] + W[i] * x)) * sigmoid(c[i] + W[i] * x);
	}
	for (int i = 0; i < size(V); ++i) {
		V[i] = V[i] - epsilon * 2 * (f_theta(x) - y) * sigmoid(c[i] + W[i] * x);
	}
	b = b - epsilon * 2 * (f_theta(x) - y);
	for (int i = 0; i < size(c); ++i) {
		c[i] = c[i] - epsilon * 2 * (f_theta(x) - y) * V[i] *
			(1 - sigmoid(c[i] + W[i] * x)) * sigmoid(c[i] + W[i] * x);
	}
}

// Multi-Level Perceptron
// one input layer
// one hidden layer with NrOfNeurons.
// A \(sigmoid\\) function as activation function
// one output layer
int main(int argc, char** argv)
try {
	using namespace std;
	using namespace mtl;
	using namespace sw::unum;

	const size_t nbits = 16;
	const size_t es = 1;
	const size_t vecSize = 32;

	using Scalar = float;
	using Matrix = mtl::dense2D<Scalar>;
	using Vector = mtl::dense_vector<Scalar>;
	using Pair   = std::pair<Scalar, Scalar>;
	using VoP    = mtl::dense_vector<Pair>;

#if defined(MTL_WITH_INITLIST) && defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
	Matrix X = {
		{0,0,1},
		{0,1,1},
		{1,0,1},
		{1,1,1}
	};
#endif

	constexpr int NrOfTrainingSamples = 20;
	constexpr int NrOfNeurons = 5;
	constexpr float epsilon = 0.05f;
	constexpr int epoch = 50000;

	Vector c(NrOfNeurons), W(NrOfNeurons), V(NrOfNeurons);
	c = W = V = 0;
	Scalar b = 0;

	// fill initial state with random values
	srand((unsigned int)time(NULL));
	for (int i = 0; i < NrOfNeurons; i++) {
		W[i] = Scalar(2 * rand() / RAND_MAX - 1);
		V[i] = Scalar(2 * rand() / RAND_MAX - 1);
		c[i] = Scalar(2 * rand() / RAND_MAX - 1);
	}
	VoP trainingSet(NrOfTrainingSamples);

	for (int i = 0; i < NrOfTrainingSamples; i++) {
		trainingSet[i] = make_pair(i * 2 * m_pi / NrOfTrainingSamples, sin(i * m_2_pi / NrOfTrainingSamples));
	}

	// train for epoch number of cycles
	for (int k = 0; k < epoch; ++k) {
		for (int i = 0; i < NrOfTrainingSamples; i++) {
			auto x = trainingSet[i].first;
			auto y = trainingSet[i].second;
			train(x,y, W, V, c);
		}
		std::cout << k << "\r";
	}

	// Plot the results
	int nrVisualSamples = 1000;
	Vector x(nrVisualSamples);
	Vector y1(nrVisualSamples), y2(nrVisualSamples);

	auto scaling_factor = m_2pi / nrVisualSamples;
	for (int i = 0; i < nrVisualSamples; i++) {
		x[i] =  i * scaling_factor;
		y1[i] = sin(i * scaling_factor);
		y2[i] = f_theta(Scalar(i * scaling_factor), b, V, W);
	}

	FILE * gp = _popen("gnuplot", "w");
	fprintf(gp, "set terminal wxt size 600,400 \n");
	fprintf(gp, "set grid \n");
	fprintf(gp, "set title '%s' \n", "f(x) = sin (x)");
	fprintf(gp, "set style line 1 lt 3 pt 7 ps 0.1 lc rgb 'green' lw 1 \n");
	fprintf(gp, "set style line 2 lt 3 pt 7 ps 0.1 lc rgb 'red' lw 1 \n");
	fprintf(gp, "plot '-' w p ls 1, '-' w p ls 2 \n");

	// Exact f(x) = sin(x) -> Green Graph
	for (int k = 0; k < nrVisualSamples; ++k) {
		fprintf(gp, "%f %f \n", x[k], y1[k]);
	}
	fprintf(gp, "e\n");

	//Neural Network Approximate f(x) = sin(x) -> Red Graph
	for (int k = 0; k < nrVisualSamples; ++k) {
		fprintf(gp, "%f %f \n", x[k], y2[k]);
	}
	fprintf(gp, "e\n");

	fflush(gp);

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}

#else
int main() {}
#endif


#include <Eigen/Core>
#include <Eigen/Dense>
#include <armadillo>
#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include "Sampling_functions.h"

using namespace std;
using namespace Eigen;
using namespace arma;
using namespace boost;

//boost::random::mt19937 gen(0);
boost::random::mt19937 gen(time(0));
//distributions
double runif(double lower, double higher)
{
	boost::random::uniform_real_distribution<> dist(lower, higher);
	return dist(gen);
}

double rnorm(double mean, double sd)
{
	boost::random::normal_distribution<> dist(mean, sd);
	return dist(gen);
}

double rgamma(double alpha, double beta)
{

	boost::math::gamma_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rbeta(double alpha, double beta)
{

	boost::math::beta_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rinvchisq(double df, double scale)
{

	boost::math::inverse_chi_squared_distribution<> dist(df, scale);
	double q = quantile(dist, runif(0,1));

	return(q);
}
int rbernoulli(double p)
{
	std::bernoulli_distribution dist(p);
	return dist(gen);
}

//switch between armadillo and eigen
Eigen::MatrixXd cast_eigen(arma::mat arma_A)
{
	Eigen::MatrixXd eigen_B = Eigen::Map<Eigen::MatrixXd>(arma_A.memptr(),
			arma_A.n_rows,
			arma_A.n_cols);

	return eigen_B;
}

arma::mat cast_arma(Eigen::MatrixXd& eigen_A)
{
	arma::mat arma_B = arma::mat(eigen_A.data(), eigen_A.rows(), eigen_A.cols(),false, false);
	return arma_B;
}

//sampling functions
double sample_hyper(const MatrixXd& w1_M1_sample, const MatrixXd& WI_m, double b0_m, const VectorXd& mu0_m, int df_m,VectorXd& mu_m,MatrixXd& lambda_m)
{
	//Sample from individual or SNP hyperparams (equation 14)
	int N = w1_M1_sample.rows();
	int num_feat=w1_M1_sample.cols();
	VectorXd u_bar(num_feat);
	MatrixXd S_bar(num_feat,num_feat);

	u_bar = w1_M1_sample.colwise().mean().transpose();
	//covariance to correlation
	S_bar = (w1_M1_sample.transpose()*w1_M1_sample)/N;

	//Gaussian-Wishard sampling
	//1.sample lambda from wishard distribution (lambda_m or lambda_u)
	//2.sample mu (mu_m or mu_u) from multivariate normal distribution with mean mu_0 (mu_temp) and variance (lambda*Lambda)^-1

	//Wishard-covariance matrix
	MatrixXd WI_post(num_feat,num_feat);
	WI_post = WI_m.inverse() + N*S_bar+((N*b0_m)/(b0_m+N))*((mu0_m - u_bar)*(mu0_m - u_bar).transpose());

	//WI_post = (WI_post + WI_post.transpose())/2;

	WI_post=WI_post.inverse();

	//Wishard-degrees of freedom
	int df_mpost = df_m+N;

	//Wishard draw, using the armadillo function. So I cast to an armadillo matrix, draw wishart then cast back to eigen matrix.
	//should just code the wishart function.
	mat arma_lambda =cast_arma(WI_post);
	mat W = wishrnd(arma_lambda, df_mpost);
	lambda_m=cast_eigen(W);

	//multivariate normal mean
	VectorXd mu_temp(num_feat);
	mu_temp = (b0_m*mu0_m + N*u_bar)/(b0_m+N);

	//Multivariate normal with mean mu_temp and cholesky decomposed covariance lam
	MatrixXd lam = (((b0_m+N)*lambda_m).inverse());

	lam=lam.llt().matrixU();
	lam.transposeInPlace();

	MatrixXd normV(num_feat,1);
	for (int i=0;i<num_feat;i++){
		normV(i,0)=rnorm(0,1);
	}

	mu_m = lam*normV+mu_temp;

	return 0;
}

double sample_ind (double Xglobalmean,const MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample, const MatrixXd& X,int num_p,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const VectorXd& alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).
	MatrixXd covarprod = w1_P1_sample.transpose()*w1_P1_sample;
	MatrixXd lambda_mu_prod=lambda_u*mu_u;

	for (int i=0;i<num_p;i++){
		//observed data row
		VectorXd rr = X.row(I[i]).array();
		//equation 12
		MatrixXd covar = alpha[i]*covarprod+lambda_u;
		covar=covar.inverse();
		//equation 13
		VectorXd mean_u = covar * (alpha[i]*(w1_M1_sample.transpose()*rr)+lambda_mu_prod);
		//multivariate normal with mean mean_u and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}

		w1_P1_sample.row(I[i]) = (lam*normV+mean_u).transpose();

	}

	return 0;
}

double sample_SNP (double Xglobalmean,const MatrixXd& w1_P1_sample, MatrixXd& w1_M1_sample, const MatrixXd& X,int num_m,int num_feat,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of markers.
	std::vector<int> I;
	for (int i=0; i<num_m; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());
	//equation 12
	MatrixXd covar = ((alpha*(w1_P1_sample.transpose()*w1_P1_sample)+lambda_m));
	covar=covar.inverse();
	MatrixXd lam = covar.llt().matrixU();
	lam.transposeInPlace();
	MatrixXd lambda_mu_prod=lambda_m*mu_m;

	for (int i=0;i<num_m;i++){
		//observed data column
		VectorXd rr = X.col(I[i]).array();

		//equation 13
		VectorXd mean_m = covar * (alpha*(w1_P1_sample.transpose()*rr)+lambda_mu_prod);
		//multivariate normal with mean mean_m and cholesky decomposed variance lam
		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}
		w1_M1_sample.row(I[i]) = (lam*normV+mean_m).transpose();
	}

	return 0;
}

double sample_ind_missing (double Xglobalmean,const MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample,const MatrixXd& Indicator, const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const VectorXd& alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).

	for (int i=0;i<num_p;i++){
		//observed data row
		VectorXd rr = X.row(I[i]).array();
		//equation 12
		MatrixXd covarprod(num_feat,num_feat);
		VectorXd obsprod(num_feat);
		MatrixXd lambda_mu_prod=lambda_u*mu_u;

		covarprod.setZero();
		obsprod.setZero();
		for (int j=0;j<num_m;j++){
			covarprod=covarprod + (w1_M1_sample.row(j).transpose()*w1_M1_sample.row(j))*Indicator(i,j);
			obsprod=obsprod + (w1_M1_sample.row(j).transpose()*rr[j])*Indicator(i,j);
		}

		MatrixXd covar = alpha[i]*covarprod+lambda_u;
		covar=covar.inverse();
		//equation 13
		VectorXd mean_u = covar * (alpha[i]*obsprod+lambda_mu_prod);
		//multivariate normal with mean mean_u and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}

		w1_P1_sample.row(I[i]) = (lam*normV+mean_u).transpose();

	}

	return 0;
}

double sample_SNP_missing (double Xglobalmean,const MatrixXd& w1_P1_sample, MatrixXd& w1_M1_sample, const MatrixXd& Indicator,const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of markers.
	std::vector<int> I;
	for (int i=0; i<num_m; ++i) {
		I.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());

	for (int i=0;i<num_m;i++){
		//observed data column
		VectorXd rr = X.col(I[i]).array();
		//equation 12
		MatrixXd covarprod(num_feat,num_feat);
		VectorXd obsprod(num_feat);
		MatrixXd lambda_mu_prod=lambda_m*mu_m;

		covarprod.setZero();
		obsprod.setZero();
		for (int j=0;j<num_p;j++){
			covarprod=covarprod + (w1_P1_sample.row(j).transpose()*w1_P1_sample.row(j))*Indicator(j,i);
			obsprod=obsprod + (w1_P1_sample.row(j).transpose()*rr[j])*Indicator(j,i);
		}

		MatrixXd covar = alpha*covarprod+lambda_m;
		covar=covar.inverse();

		//equation 13
		VectorXd mean_m= covar * (alpha*obsprod+lambda_mu_prod);
		//multivariate normal with mean mean_m and cholesky decomposed variance lam
		MatrixXd lam = covar.llt().matrixU();
		lam.transposeInPlace();

		MatrixXd normV(num_feat,1);
		for (int j=0;j<num_feat;j++){
			normV(j,0)=rnorm(0,1);
		}
		w1_M1_sample.row(I[i]) = (lam*normV+mean_m).transpose();
	}

	return 0;
}

double sample_ind_SNP (MatrixXd& w1_M1_sample, MatrixXd& w1_P1_sample, const MatrixXd& X,int num_p,int num_m,int num_feat,const MatrixXd& lambda_u,const VectorXd& mu_u,const MatrixXd& lambda_m,const VectorXd& mu_m,double alpha)
{
	//random shuffling of individuals.
	std::vector<int> I;
	for (int i=0; i<num_p; ++i) {
		I.push_back(i);
	}
	std::vector<int> J;
	for (int i=0; i<num_m; ++i) {
		J.push_back(i);
	}

	std::random_shuffle(I.begin(), I.end());
	std::random_shuffle(J.begin(), J.end());

	// Gibbs updates over individual latent vectors given hyperparams.
	// Infer posterior distribution over individual latent vectors (equation 11).
	int upd_row=0;
	int upd_col=0;
	int i=0;
	while (i<num_p+num_m){

		if(upd_row<num_p){
			//observed data row
			VectorXd rr = X.row(I[upd_row]);
			//equation 12
			MatrixXd covar = ((alpha*(w1_M1_sample.transpose()*w1_M1_sample)+lambda_u));
			covar=covar.inverse();
			//equation 13
			VectorXd mean_u = covar * (alpha*w1_M1_sample.transpose()*rr+lambda_u*mu_u);
			//multivariate normal with mean mean_u and cholesky decomposed variance lam
			MatrixXd lam = covar.llt().matrixU();
			lam.transposeInPlace();

			MatrixXd normV(num_feat,1);
			for (int j=0;j<num_feat;j++){
				normV(j,0)=rnorm(0,1);
			}

			w1_P1_sample.row(I[upd_row]) = (lam*normV+mean_u).transpose();
			upd_row=upd_row+1;
			i=i+1;
		}

		if(upd_col<num_m){
			//observed data column
			VectorXd rr = X.col(J[upd_col]);
			//equation 12
			MatrixXd covar = ((alpha*(w1_P1_sample.transpose()*w1_P1_sample)+lambda_m));
			covar=covar.inverse();
			//equation 13
			VectorXd mean_m = covar * (alpha*w1_P1_sample.transpose()*rr+lambda_m*mu_m);
			//multivariate normal with mean mean_m and cholesky decomposed variance lam
			MatrixXd lam = covar.llt().matrixU();
			lam.transposeInPlace();

			MatrixXd normV(num_feat,1);
			for (int j=0;j<num_feat;j++){
				normV(j,0)=rnorm(0,1);
			}
			w1_M1_sample.row(J[upd_col]) = (lam*normV+mean_m).transpose();
			upd_col=upd_col+1;
			i=i+1;
		}

	}

	return 0;
}

double sample_residual_variance_gamma(const MatrixXd& epsilon)
{
	int N=epsilon.rows()*epsilon.cols();
	double res=epsilon.squaredNorm();
	double sigma2=1/rgamma((N-1)/2,1/(0.5*res));
	//cout<<"residual info"<<endl;
	//cout<<N<<"\t"<<res<<"\t"<<sigma2<<endl;
	return sigma2;
}

double sample_residual_row_variance_gamma(const MatrixXd& epsilon, VectorXd& alpha)
{
	int N=epsilon.rows();
	int M=epsilon.cols();
	double res=0;
	for (int i=0;i<N;i++){
	res=epsilon.row(i).squaredNorm();
	alpha[i]=1/rgamma((M-1)/2,1/(0.5*res));
	}

	return 0;
}


#include <iostream>
#include <ctime>
#include <Eigen/Core>  // Eigen 部分
#include <Eigen/Dense> // 稠密矩阵的代数运算
using namespace std;

#define MATRIX_SIZE 50

/********************************************
 * 本程序演示Eigen的基本类型的使用
 * *****************************************/

int main(int argc, char** argv)
{
    Eigen::Matrix<float,2,3> matrix_23;
    Eigen::Vector3d v_3d;
    Eigen::Matrix<float,3,1> vd_3d;

    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic;
    Eigen::MatrixXd matrix_x;

    matrix_23 << 1, 2, 3, 4, 5, 6;
    cout<< matrix_23<<endl;

    for(int i=0; i<2; i++)
    {
        for(int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    v_3d << 3, 2, 1;
    vd_3d << 4, 5, 6;
    Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    // Eigen::Matrix<double,2,1> result2 = matrix_23 * vd_3d;
    // cout << result2 << endl;

    // 四则运算直接用+-*/
    // 下面为其他矩阵运算
    matrix_33  = Eigen::Matrix3d::Random();
    cout << matrix_33 << endl;

    cout << matrix_33.transpose() << endl;
    cout << matrix_33.sum() << endl;
    cout << matrix_33.trace() << endl;
    cout << 10*matrix_33 <<endl;
    cout << matrix_33.inverse() << endl;
    cout << matrix_33.determinant() << endl;

    // 特征值
    //实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver
        (matrix_33.transpose()*matrix_33);
    cout << "Eigen values = \n"<< eigen_solver.eigenvalues()<<endl;
    cout << "Eigen vectors = \n"<< eigen_solver.eigenvectors()<<endl;

    // 解方程
    // 求解matrix_NN * x = v_Nd这个方程
    // N 由前面的宏定义
    // 直接求解最直接,但是求逆运算量大

    Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,MATRIX_SIZE,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    clock_t time_stt = clock(); // start

    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout << "time use in normal inverse is " << 1000*(clock()-time_stt)/
        (double)CLOCKS_PER_SEC << "ms" <<endl;

    // 通常用矩阵分解的方式求
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time use in Qr decomposition is " << 1000*(clock()-time_stt)/
        (double)CLOCKS_PER_SEC << "ms" <<endl;

    return 0;

}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    double           array[][3]= {{1., 0., 3.}, {4., 0., 6.}, {7., 0., 9.}, {0., 0., 0.}};
    dense2D<double>  A(array), B2, B3;

    // Creating a compression (reordering) matrix from a vector (or an array respectively)
    int non_zero_rows[]= {0, 1, 2}, non_zero_columns[]= {0, 2};
    // To be sure, we give the number of columns for a consistent size!
    mat::traits::reorder<>::type RR= mat::reorder(non_zero_rows, 4), RC= mat::reorder(non_zero_columns);
    std::cout << "A =\n" << A << "\nRR =\n" << RR << "\nRC =\n" << RC;

    // Compress rows
    B2= RR * A;
    std::cout << "\nRR * A, i.e. compress row of A =\n" << B2;

    // Compress columns
    B3= B2 * trans(RC);
    std::cout << "\nB2 * trans(RC), i.e. compress columns of B2 =\n" << B3;

    // Decompress rows
    dense2D<double>  C1(trans(RR) * B3);
    std::cout << "\ntrans(RR) * B3, i.e. row decompression of B3 =\n" << C1;

    // Decompress columns
    dense2D<double>  C2(C1 * RC);
    std::cout << "\nC1 * RC, i.e. column decompression of C1 (should be A) =\n" << C2;

    return 0;
}


#include <iostream>
#include <tuple>
#include <Eigen/Dense>
#include "linear_algebra_addon.hpp"
using namespace Eigen;
using namespace std;

MatrixXcd bl_cocg_rq(const MatrixXcd& A, const MatrixXcd& B, const double& tol, const int& itermax)
{
// only for complex-symmetric matrix
// Gu et al 2016, arXiv, Block variants of COCG and COCR methods
// for solving complex symmetric linear systems with multiple right-hand sides

  double Bnorm= B.norm();
  MatrixXcd X= MatrixXcd::Zero(B.rows(),B.cols()); // Initial guess of X (zeros)

  MatrixXcd Q;
  MatrixXcd xi;
  tie(Q,xi)= qr_reduced(B-A*X);
  MatrixXcd S= Q;

  for(int k= 0; k < itermax; ++k){
      MatrixXcd AS= A*S;
      MatrixXcd alpha= (S.transpose()*AS).fullPivLu().solve(Q.transpose()*Q);
      X= X+S*alpha*xi;
      MatrixXcd Qnew;
      MatrixXcd tau;
      tie(Qnew,tau)= qr_reduced(Q-AS*alpha);
      xi= tau*xi;
      double err= xi.norm()/Bnorm;
      cout << "bl_cocg_rq: " << "iter= " << k << " relative err= " << err << endl;
      if(err < tol) break;
      MatrixXcd beta= (Q.transpose()*Q).fullPivLu().solve(tau.transpose()*Qnew.transpose()*Qnew);
      Q= Qnew;
      S= Q+S*beta;
  }

  if((A*X-B).norm()/Bnorm > 10*tol){
      cerr << "bl_cocg_rq did not converge to solution within error tolerance !" << endl;
     // exit(EXIT_FAILURE);
  }

  return X;
}


#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;
using namespace mtl;

typedef dense2D<double> Matrix;
// using Matrix= dense2D<double>; // C++11

dense_vector<double> unit_vector(unsigned k, unsigned n)
{
    dense_vector<double>  e_k(n, 0.0);
    e_k[k]= 1.0;

    // for (unsigned i= 0; i < n; ++i)
    // 	   e_k[k]= static_cast<double>(i == k);
    // for (unsigned i= 0; i < n; ++i)
    // 	   e_k[k]= i == k;

    return e_k;
}

Matrix inverse_upper(const Matrix& U)
{
    const unsigned n= num_rows(U);
    Matrix Inv(n, n);
    Inv= 0.0;
    for (unsigned k= 0; k < n; k++) {
	irange  r(0,k+1);
	Inv[r][k]= upper_trisolve(U[r][r], unit_vector(k, k+1));
    }
    return Inv;
}

Matrix inverse_lower(const Matrix& L)
{
    Matrix T(trans(L));
    return Matrix(trans(inverse_upper(T)));
}

Matrix inverse(const Matrix& A)
{
    const unsigned n= num_rows(A);
    if (num_cols(A) != n)
	throw "Matrix must be square";

    Matrix Inv(n, n);

    Matrix LU(A); // Kopie von A
    dense_vector<unsigned>  Pv(n);
    lu(LU, Pv);
    compressed2D<unsigned> P(permutation(Pv));
    Matrix I(mat::identity(n, n)), L(I + strict_lower(LU)), U(upper(LU));
    Inv= inverse_upper(U) * inverse_lower(L) * P;

    return Inv;
}

int main (int argc, char* argv[])
{
    const unsigned size= 3;
    const double   eps= 0.001; // C++11: constexpr double   eps= 0.001;
    Matrix   A(size, size);
    A= 4, 1, 2,
       1, 5, 3,
       2, 6, 9;

    cout << "A ist\n" << A;

    Matrix LU(A); // Kopie von A
    dense_vector<unsigned>  Pv(size);
    lu(LU, Pv);
    Matrix P(permutation(Pv));

    cout << "LU ist\n" << LU;
    cout << "Permutation vector is " << Pv << "\nPermutationsmatrix ist\n" << P;

    Matrix I(mat::identity(size, size)), L(I + strict_lower(LU)), U(upper(LU));
    cout << "L ist\n" << L << "U ist\n" << U;

    Matrix UI(inverse_upper(U));
    cout << "Inverse von U ist\n" << UI << "UI * U ist\n" << Matrix(UI * U);
    assert(one_norm(Matrix(UI * U - I)) < eps);

    Matrix LI(inverse_lower(L));
    cout << "Inverse von L ist\n" << LI << "LI * L ist\n" << Matrix(LI * L);
    assert(one_norm(Matrix(LI * L - I)) < eps);

    Matrix AI(UI * LI * P);
    cout << "Inverse von A ist\n" << AI << "AI * A ist\n" << Matrix(AI * A);
    assert(one_norm(Matrix(AI * A - I)) < eps);

    Matrix A_inverse(inverse(A));
    cout << "Inverse von A ist\n" << A_inverse << "A_inverse * A ist\n" << Matrix(A_inverse * A);
    assert(one_norm(Matrix(A_inverse * A - I)) < eps);

    return 0 ;
}


#include "triangulation.h"

#include "defines.h"

#include <Eigen/SVD>

// Используем DLT метод, составляем систему уравнений. Система похожа на систему для гомографии, там пары уравнений получались из выражений вида x (cross) Hx = 0, а здесь будет x (cross) PX = 0
// (см. Hartley & Zisserman p.312)
cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int count)
{
    using mat = Eigen::MatrixXd;
    using vec = Eigen::VectorXd;

    mat A(2 * count, 4);

    for (int i = 0; i < count; ++i) {

        const matrix34d &P = Ps[i];
        const vector3d &m = ms[i];

        double x = m[0];
        double y = m[1];
        double z = m[2];

        auto p0 = P.row(0);
        auto p1 = P.row(1);
        auto p2 = P.row(2);

        auto row0 = x * p2 - z * p0;
        auto row1 = y * p2 - z * p1;

        A.row(i * 2 + 0) << row0(0), row0(1), row0(2), row0(3);
        A.row(i * 2 + 1) << row1(0), row1(1), row1(2), row1(3);
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::VectorXd null_space = svd.matrixV().col(3);

    vector4d result;
    for (int i = 0; i < 4; ++i) {
        result(i) = null_space(i);
    }

    return result;
}