/*
 ISC License

 Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder

 Permission to use, copy, modify, and/or distribute this software for any
 purpose with or without fee is hereby granted, provided that the above
 copyright notice and this permission notice appear in all copies.

 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

 */

#include <stdio.h>
#include <stdlib.h>
#include "gaussMarkovCheck.h"
#include <Eigen/Dense>
#include "utilities/avsEigenSupport.h"
#include "utilities/gauss_markov.h"
#include <iostream>

uint64_t testGaussMarkov()
{
    uint64_t failures = 0;
    uint64_t seedIn = 1000;
    Eigen::Matrix2d propIn;
    propIn << 1,1,0,1;
    Eigen::Matrix2d covar;
    covar << 1500,0,0,1.5;
    Eigen::Vector2d bounds;
    bounds << 1e-15, 1e-15; //small but non-zero required for "white" noise
    GaussMarkov errorModel;
    errorModel = GaussMarkov(2);
    errorModel.setRNGSeed(seedIn);
    errorModel.setPropMatrix(propIn);
    errorModel.setNoiseMatrix(covar);
    errorModel.setUpperBounds(bounds);
    uint64_t numPts = 100000;

    Eigen::MatrixXd noiseOut;
    noiseOut.resize(2, numPts);

    for(uint64_t i = 0; i < numPts; i++){
        errorModel.computeNextState();
        noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState();
    }

    //Test if the std deviation is what we asked for
    Eigen::Vector2d stds = calculateSD(noiseOut, numPts);
    Eigen::Vector2d stdsIn;
    stdsIn(0) = covar(0,0) / 1.5;
    stdsIn(1) = covar(1,1) / 1.5;
    failures += (stdsIn(0) - stds(0))/(stdsIn(0)) > 0.1 ? 1 : 0;
    failures += (stdsIn(1) - stds(1))/(stdsIn(1)) > 0.1 ? 1 : 0;

    //Test if the mean is zero
    Eigen::Vector2d means = noiseOut.rowwise().mean();
    Eigen::Vector2d meansIn;
    meansIn << 0, 0;
    failures += fabs(meansIn(0) - means(0)) > 5 ? 1 : 0;
    failures += fabs(meansIn(1) - means(1)) > .05 ? 1 : 0;
    seedIn = 1500;
    propIn << 1,0,0,1;
    covar << 1.5,0,0,0.015;
    bounds << 10., 0.1;
    errorModel = GaussMarkov(2);
    errorModel.setRNGSeed(seedIn);
    errorModel.setPropMatrix(propIn);
    errorModel.setNoiseMatrix(covar);
    errorModel.setUpperBounds(bounds);

    Eigen::Vector2d maxOut;
    maxOut.fill(0.0);
    Eigen::Vector2d minOut;
    minOut.fill(0.0);

    numPts = 1e6;

    noiseOut.resize(2,numPts);

    for(uint64_t i = 0; i < numPts; i++){
        errorModel.computeNextState();
        noiseOut.block(0, i, 2, 1) = errorModel.getCurrentState();
        if (noiseOut(0,i) > maxOut(0)){
            maxOut(0) = noiseOut(0,i);
        }
        if (noiseOut(0,i) < minOut(0)){
            minOut(0) = noiseOut(0,i);
        }
        if (noiseOut(1,i) > maxOut(1)){
            maxOut(1) = noiseOut(1,i);
        }
        if (noiseOut(1,i) < minOut(1)){
            minOut(1) = noiseOut(1,i);
        }
    }

    //Test the bounds
    failures += fabs(12.481655180914322 - maxOut(0)) / 12.481655180914322 > 5e-1 ? 1 : 0;
    failures += fabs(0.12052269089286843 - maxOut(1)) / 0.12052269089286843 > 5e-1 ? 1 : 0;
    failures += fabs(-12.230618182796439 - minOut(0)) / -12.230618182796439 > 5e-1 ? 1 : 0;
    failures += fabs(-0.12055787311661936 - minOut(1)) / -0.12055787311661936 > 5e-1 ? 1 : 0;

    return failures;

}

Eigen::Vector2d calculateSD(Eigen::MatrixXd dat, uint64_t numPts)
{

    Eigen::Vector2d sum = dat.rowwise().sum();

    Eigen::Vector2d means = sum / numPts;
    Eigen::MatrixXd mean;
    mean.resize(2, numPts);
    mean.block(0, 0, 1, numPts).fill(means(0) / numPts);
    mean.block(1, 0, 1, numPts).fill(means(1) / numPts);

    Eigen::MatrixXd resid = dat - mean;
    resid = resid.cwiseProduct(resid);
    Eigen::MatrixXd stnd = resid.rowwise().sum();
    stnd = stnd / numPts;
    stnd = stnd.array().sqrt().matrix();

    return stnd;
}


/**
 * @file visual_ekf.hpp
 * @author Ziyou Zhang (ziyou.zhang@outlook.com)
 * @brief Design of visual-based EKF estimation of dynamic object state.
 * @version 0.1
 * @date 2020-09-18
 *
 * @copyright Copyright (c) 2020
 *
 */

#ifndef VISUAL_EKF_HPP_
#define VISUAL_EKF_HPP_

#include <ros/ros.h>
#include <Eigen/Dense>

#include <iostream>
#include <fstream>

struct ObjectState
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    double mass;
    double timestamp;                            // Time stamp in seconds.
    Eigen::Vector3d r_W;                         // The position relative to the W frame.
    Eigen::Quaterniond q_WO;                     // The quaternion of rotation W-O.
    Eigen::Vector3d v_O;                         // The velocity expressed in object frame.
    Eigen::Vector3d omega_O;                     // The angular velocity expressed in object frame.
    Eigen::Matrix<double, 3, 3> inertia;         //The moment of inertia.
    Eigen::Matrix<double, 3, 3> inertia_inverse; // The inverse of moment of inertia, stored for better efficiency.

    ObjectState()
    {
        inertia << 0.5 / 12, 0, 0,
            0, 0.5 / 12, 0,
            0, 0, 0.5 / 12;

        inertia_inverse = inertia.inverse();
    }
};

struct ObjectStateDerivative
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    double timestamp;            // Time stamp in seconds.
    Eigen::Vector3d r_W_dot;     // The position relative to the W frame.
    Eigen::Quaterniond q_WO_dot; // The quaternion of rotation W-O.
    Eigen::Vector3d v_O_dot;     // The velocity expressed in object frame.
    Eigen::Vector3d omega_O_dot; // The angular velocity expressed in object frame.
};

struct ApriltagMeasurement
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    double timestamp;        // Time stamp in seconds.
    Eigen::Vector3d r_W;     // The position relative to the W frame.
    Eigen::Quaterniond q_WO; // The quaternion of rotation W-O.
};

class VisualEKF
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VisualEKF();
    ~VisualEKF();

    /**
     * @brief The initialisation state.
     *
     * @return true if initialised
     * @return false if not initialised
     */
    bool isInitialsed();

    /**
     * @brief Initialise the EKF with the initial state.
     *
     * @param initialState Initial object state.
     * @return true if successfully executed.
     */
    bool initialise(const ObjectState initialState);

    /**
     * @brief The predict step in EKF.
     *
     * @param dt The time difference.
     * @param fromState The previous state.
     * @param toState The predicted state.
     * @return true if successfully executed.
     */
    bool predict(const double dt,
                 const ObjectState &fromState,
                 ObjectState &toState);

    /**
     * @brief The update step in EKF.
     *
     * @param apriltagMeasurement The apriltag measurement (pose and oritation of the object).
     * @return true if successfully executed.
     */
    bool update(const ApriltagMeasurement apriltagMeasurement);

    /**
     * @brief The state transition using trapezoidal numerical integration.
     *
     * @param object_state_k_minues_1 The state at previous step.
     * @param object_state_1 The resulting state after trapezoidal numerical integration.
     * @param dt The time difference.
     * @param jacobian The jacobian matrix to be calculated.
     * @return true if successfully executed.
     */
    bool stateTransition(const ObjectState &object_state_k_minues_1,
                         ObjectState &object_state_1,
                         const double dt,
                         Eigen::Matrix<double, 13, 13> &jacobian);

    /**
     * @brief Calculate the derivatives based on the motion model.
     *
     * @param state The object state.
     * @return ObjectStateDerivative The object state derivatives.
     */
    ObjectStateDerivative calcStateDerivative(const ObjectState &state);

    /**
     * @brief Calculate the jacobian matrix.
     *
     * @param state The object state.
     * @param dt The time difference.
     * @param jacobian The jacobian matrix to be calculated.
     * @return true if successfully executed.
     */
    bool calcJacobian(const ObjectState &state,
                      const double &dt,
                      Eigen::Matrix<double, 13, 13> &jacobian);

private:
    // The object states.
    ObjectState x_;
    ObjectState x_predicted_;
    ObjectState x_temp_;

    // The initialsation information.
    bool initialised = false;

    // The error matrix and jacobian matrix.
    Eigen::Matrix<double, 13, 13> P_;
    Eigen::Matrix<double, 13, 13> jacobian;

    // Process noise params.
    double sigma_c_r_W = 0.2;     // m, location error
    double sigma_c_q_WO = 0.4;    // N/A, for quoternion error
    double sigma_c_v_O = 3;     // m/s, velocity error
    double sigma_c_omega_O = 10; // rad/s, amgular velocity error

    //Measurement noise params.
    double sigma_z_r_W = 0.02;  // m, pose measurement error
    double sigma_z_q_WO = 0.02; // N/A, quaternion measurement error

    friend class PoseDetector;
};

#endif // VISUAL_EKF_HPP_


// 以下代码参考
// Eigen提取block：https://blog.csdn.net/jiahao62/article/details/80655542
// Eigen提取block:https://www.cnblogs.com/newneul/p/8306430.html
// 求解Ax=b：https://www.cnblogs.com/newneul/p/8306442.html

#include <iostream>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

#define EQUATION_NUM 3
#define VARIABLE_NUM 3

int main( int argc, char** argv )
{
    // Eigen提取block
    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    cout << "before assign \n" << matrix_NN.block(0, 0, 3, 3) << endl;
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Identity();
    matrix_NN.block(0, 0, 3, 3) = matrix_33;
    cout << "after assign \n"  << matrix_NN.block(0, 0, 3, 3) << endl;

    //-------------------------------------------------------------------------------------
    // 求解Ax=b
    Eigen::Matrix<double, EQUATION_NUM, VARIABLE_NUM> A = Eigen::MatrixXd::Random(EQUATION_NUM, VARIABLE_NUM);
    Eigen::Matrix<double, EQUATION_NUM, 1> b = Eigen::MatrixXd::Random(EQUATION_NUM, 1);
    // 测试用例
    A << 10,3,1,2,-10,3,1,3,10;
    b << 14,-5,14;
    // 设置解变量
    Eigen::Matrix<double, VARIABLE_NUM, 1> x;

    clock_t time_stt = clock(); // 计时

    // 方法一：直接求逆，适用条件：方阵
    x = A.inverse()*b;
    cout <<"time use in normal inverse is "   << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;

	  // 方法二：QR分解，适用条件：方阵和非方阵
    // 当方程组有解时的出的是真解，若方程组无解得出的是近似解
    time_stt = clock();
    x = A.colPivHouseholderQr().solve(b);
    cout << "x^T = " << x.transpose() <<endl;
    cout <<"time use in Qr decomposition is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // 方法三：最小二乘法，适用条件：方阵和非方阵
    // 方程组有解时得出真解，否则是最小二乘解(在求解过程中可以用QR分解 分解最小二成的系数矩阵)
    time_stt = clock();
    x = (A.transpose() * A ).inverse() * (A.transpose() * b);
    cout << "x^T = " << x.transpose() <<endl;
    cout <<"time use in least square is "     << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // 方法四：LU分解法，适用条件：方阵
    // 满足分解的条件才行
    time_stt = clock();
    x = A.lu().solve(b);
    cout << "x^T = " << x.transpose() <<endl;
    cout <<"time use in LU decomposition is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // 方法五：Cholesky分解法，适用条件：方阵
    // 结果与其他的方法差好多，如此简单的也错了。。
    time_stt = clock();
    x = A.llt().solve(b);
    cout << "x^T = " << x.transpose() <<endl;
    cout <<"time use in Cholesky decomposition is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    // 方法六：Jacobi迭代法，挖坑待补充。。

    //-------------------------------------------------------------------------------------
    // 坐标变换
    Eigen::Quaterniond q1(0.35, 0.2,  0.3, 0.1);
    cout << "quaternion 1 = \n" << q1.coeffs() << endl;
    Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
    cout << "quaternion 2 = \n" << q2.coeffs() << endl;
    q1.normalize(); // remember to normalize
    q2.normalize();
    cout << "normalized quaternion 1 = \n" << q1.coeffs() << endl;
    cout << "normalized quaternion 2 = \n" << q2.coeffs() << endl;
    Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);

    // 注意下标，T1w表示world to 1！
    Eigen::Isometry3d T1w(q1); // 初始化方法一
    // Eigen::Isometry3d T1w(q1.toRotationMatrix()); // 初始化方法二
    // Eigen::Isometry3d T1w = Eigen::Isometry3d::Identity();
    // T1w.rotate(q1.toRotationMatrix()); // 初始化方法三
    T1w.pretranslate(t1);
    cout << "Transform matrix 1 = \n" << T1w.matrix() <<endl;
    // world to 2
    Eigen::Isometry3d T2w(q2);
    T2w.pretranslate(t2);
    cout << "Transform matrix 2 = \n" << T2w.matrix() <<endl;

    Eigen::Vector3d p1(0.5, 0, 0.2);
    // first from 1 to world, then world to 2
    Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;
    cout << "coordinate in camera 2 is \n" << p2.transpose() << endl;

    return 0;
}


#include <iostream>
#include <vector>
//#include <cstdint>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/QR>

class Poly1d
{
    protected:
         std::vector<double> coefficients_;
    public:
        //============================================================================
        // Method Description:
        Poly1d() = default;

        //============================================================================
        // Method Description:
        std::vector<double> getCoeffs()
        {
            return coefficients_;
        }

        void init (const std::vector<double>& inValues)
        {
            // std::vector<double> coefficients_;
            for (auto value : inValues)
            {
                coefficients_.push_back(value);
            }
        }

        std::vector<double> evaluate (std::vector<double> inValues)
        {
            std::vector<double> result;

            for (int i = 0; i < inValues.size(); i++)
            {
                double polyValue = 0.0;
                int power = inValues.size() - 1;
                for (auto coeff : coefficients_)
                {
                    polyValue += coeff * pow(inValues[i], power);
                    power--;
                }
                result.push_back(polyValue);
            }

            return result;
        }

        void fit (std::vector<double> &x_data, std::vector<double> &y_data, std::vector<double> &coeff, int order)
        {
            Eigen::MatrixXd W(x_data.size(), order + 1);
            Eigen::VectorXd I = Eigen::VectorXd::Map(&y_data.front(), y_data.size());
            Eigen::VectorXd result;

            assert(x_data.size() == y_data.size());
            assert(x_data.size() >= order + 1);

            for(std::size_t i = 0; i < x_data.size(); ++i)
            {
                for (std::size_t j = 0; j < order + 1; ++j)
                {
                    W(i, j) = std::pow(x_data.at(i), j);
                }
            }

//            std::cout<< W << std::endl;

            result = W.householderQr().solve(I);
            coeff.resize(order + 1);

            for(std::size_t k = 0; k < order + 1; k++)
            {
                coeff[k] = result[k];
            }
        }
};


#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);
}


/**
 * @date Thu July 19 12:27:15 2012 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * @brief This file provides a class to process images with a weighted
 *        Gaussian kernel (Used by the Self Quotient Image)
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/format.hpp>
#include <stdexcept>
#include <bob.ip.base/IntegralImage.h>
#include <bob.ip.base/WeightedGaussian.h>

bob::ip::base::WeightedGaussian::WeightedGaussian(
  const size_t radius_y, const size_t radius_x,
  const double sigma_y, const double sigma_x,
  const bob::sp::Extrapolation::BorderType border_type
):
  m_radius_y(radius_y),
  m_radius_x(radius_x),
  m_sigma_y(sigma_y),
  m_sigma_x(sigma_x),
  m_conv_border(border_type)
{
  computeKernel();
}

bob::ip::base::WeightedGaussian::WeightedGaussian(const WeightedGaussian& other)
:
  m_radius_y(other.m_radius_y),
  m_radius_x(other.m_radius_x),
  m_sigma_y(other.m_sigma_y),
  m_sigma_x(other.m_sigma_x),
  m_conv_border(other.m_conv_border)
{
  computeKernel();
}

void bob::ip::base::WeightedGaussian::computeKernel()
{
  m_kernel.resize(2 * m_radius_y + 1, 2 * m_radius_x + 1);
  m_kernel_weighted.resize(2 * m_radius_y + 1, 2 * m_radius_x + 1);
  // Computes the kernel
  const double inv_sigma_y = 1.0 / (m_sigma_y*m_sigma_y);
  const double inv_sigma_x = 1.0 / (m_sigma_x*m_sigma_x);
  for (int i = -(int)m_radius_y; i <= (int)m_radius_y; ++i)
    for (int j = -(int)m_radius_x; j <= (int)m_radius_x; ++j)
      m_kernel(i + (int)m_radius_y, j + (int)m_radius_x) =
        exp( -0.5 * (inv_sigma_y * (i * i) + inv_sigma_x * (j * j)));
  // Normalizes the kernel
  m_kernel /= blitz::sum(m_kernel);
}

void bob::ip::base::WeightedGaussian::reset(
  const size_t radius_y, const size_t radius_x,
  const double sigma_y, const double sigma_x,
  const bob::sp::Extrapolation::BorderType border_type
){
  m_radius_y = radius_y;
  m_radius_x = radius_x;
  m_sigma_y = sigma_y;
  m_sigma_x = sigma_x;
  m_conv_border = border_type;
  computeKernel();
}

bob::ip::base::WeightedGaussian& bob::ip::base::WeightedGaussian::operator=(const bob::ip::base::WeightedGaussian& other)
{
  if (this != &other)
  {
    m_radius_y = other.m_radius_y;
    m_radius_x = other.m_radius_x;
    m_sigma_y = other.m_sigma_y;
    m_sigma_x = other.m_sigma_x;
    m_conv_border = other.m_conv_border;
    computeKernel();
  }
  return *this;
}

bool bob::ip::base::WeightedGaussian::operator==(const bob::ip::base::WeightedGaussian& b) const
{
  return (this->m_radius_y == b.m_radius_y && this->m_radius_x == b.m_radius_x &&
          this->m_sigma_y == b.m_sigma_y && this->m_sigma_x == b.m_sigma_x &&
          this->m_conv_border == b.m_conv_border);
}

bool bob::ip::base::WeightedGaussian::operator!=(const bob::ip::base::WeightedGaussian& b) const
{
  return !(this->operator==(b));
}

void bob::ip::base::WeightedGaussian::filter_(const blitz::Array<double,2>& src, blitz::Array<double,2>& dst)
{
  // Checks input
  bob::core::array::assertZeroBase(src);
  bob::core::array::assertZeroBase(dst);
  bob::core::array::assertSameShape(src, dst);
  if(src.extent(0)<m_kernel.extent(0)) {
    boost::format m("The convolutional kernel has the first dimension larger than the corresponding one of the array to process (%d > %d). Our convolution code does not allows. You could try to revert the order of the two arrays.");
    m % src.extent(0) % m_kernel.extent(0);
    throw std::runtime_error(m.str());
  }
  if(src.extent(1)<m_kernel.extent(1)) {
    boost::format m("The convolutional kernel has the second dimension larger than the corresponding one of the array to process (%d > %d). Our convolution code does not allows. You could try to revert the order of the two arrays.");
    m % src.extent(1) % m_kernel.extent(1);
    throw std::runtime_error(m.str());
  }

  // 1/ Extrapolation of src
  // Resize temporary extrapolated src array
  blitz::TinyVector<int,2> shape = src.shape();
  shape(0) += 2 * (int)m_radius_y;
  shape(1) += 2 * (int)m_radius_x;
  m_src_extra.resize(shape);

  // Extrapolate
  if(m_conv_border == bob::sp::Extrapolation::Zero)
    bob::sp::extrapolateZero(src, m_src_extra);
  else if(m_conv_border == bob::sp::Extrapolation::NearestNeighbour)
    bob::sp::extrapolateNearest(src, m_src_extra);
  else if(m_conv_border == bob::sp::Extrapolation::Circular)
    bob::sp::extrapolateCircular(src, m_src_extra);
  else
    bob::sp::extrapolateMirror(src, m_src_extra);

  // 2/ Integral image then mean values
  shape += 1;
  m_src_integral.resize(shape);
  bob::ip::base::integral(m_src_extra, m_src_integral, true);

  // 3/ Convolution
  double n_elem = m_kernel.numElements();
  for(int y=0; y<src.extent(0); ++y)
    for(int x=0; x<src.extent(1); ++x)
    {
      // Computes the threshold associated to the current location
      // Integral image is used to speed up the process
      blitz::Array<double,2> src_slice = m_src_extra(
        blitz::Range(y,y+2*(int)m_radius_y), blitz::Range(x,x+2*(int)m_radius_x));
      double threshold = (m_src_integral(y,x) +
          m_src_integral(y+2*(int)m_radius_y+1,x+2*(int)m_radius_x+1) -
          m_src_integral(y,x+2*(int)m_radius_x+1) -
          m_src_integral(y+2*(int)m_radius_y+1,x)
        ) / n_elem;
      // Computes the weighted Gaussian kernel at this location
      // a/ M1 is the set of pixels whose values are above the threshold
      if( blitz::sum(src_slice >= threshold) >= n_elem/2.)
        m_kernel_weighted = blitz::where(src_slice >= threshold, m_kernel, 0.);
      // b/ M1 is the set of pixels whose values are below the threshold
      else
        m_kernel_weighted = blitz::where(src_slice < threshold, m_kernel, 0.);
      // Normalizes the kernel
      m_kernel_weighted /= blitz::sum(m_kernel_weighted);
      // Convolves: This is indeed not a real convolution but a multiplication,
      // as it seems that the authors aim at exclusively using the M1 part
      dst(y,x) = blitz::sum(src_slice * m_kernel_weighted);
    }
}


#pragma once

#include <Eigen/Dense>
#include <avatar_locomanipulation/helpers/IOUtilities.hpp>
#include <iostream>
#include <vector>

enum ActivationFunction { None = 0, Tanh = 1, ReLU = 2, LeakyReLU = 3, Sigmoid = 4};

class Layer {
   public:
    Layer(Eigen::MatrixXd weight, Eigen::MatrixXd bias,
          ActivationFunction act_fn);
    virtual ~Layer();
    Eigen::MatrixXd GetOutput(const Eigen::MatrixXd& input);
    int GetNumInput() { return num_input_; }
    int GetNumOutput() { return num_output_; }
    Eigen::MatrixXd GetWeight() { return weight_; }
    Eigen::MatrixXd GetBias() { return bias_; }
    ActivationFunction GetActivationFunction() { return act_fn_; }

   private:
    Eigen::MatrixXd weight_;
    Eigen::MatrixXd bias_;
    int num_input_;
    int num_output_;
    ActivationFunction act_fn_;
};

class NeuralNetModel {
   public:
    NeuralNetModel(std::vector<Layer> layers);
    NeuralNetModel(std::vector<Layer> layers, Eigen::VectorXd logstd);
    NeuralNetModel(const myYAML::Node& node, bool b_stochastic);

    virtual ~NeuralNetModel();

    std::vector<Layer> GetLayers() { return layers_; }
    Eigen::MatrixXd GetOutput(const Eigen::MatrixXd& input);
    Eigen::MatrixXd GetOutput(const Eigen::MatrixXd& input, int idx);
    void GetOutput(const Eigen::MatrixXd& _input,
                   const Eigen::VectorXd& _lb,
                   const Eigen::VectorXd& _ub,
                   Eigen::MatrixXd& _output,
                   Eigen::MatrixXd& _mean,
                   Eigen::VectorXd& _neglogp);

    int GetNumInput() { return num_input_; }
    int GetNumOutput() { return num_output_; }
    void PrintInfo();

    Eigen::MatrixXd CropMatrix(Eigen::MatrixXd value, Eigen::MatrixXd min,
                               Eigen::MatrixXd max, std::string source);

   private:
    void Initialize_(std::vector<Layer> layers, bool b_stoch = false,
                     Eigen::VectorXd logstd = Eigen::VectorXd::Zero(1));
    int num_input_;
    int num_output_;
    int num_layer_;
    std::vector<Layer> layers_;
    bool b_stochastic_;
    Eigen::VectorXd logstd_;
    Eigen::VectorXd std_;
};


// 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;
}


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

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE dataset_example68
#include <boost/test/included/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
#include <sstream>

namespace bdata = boost::unit_test::data;

// Dataset generating a Fibonacci sequence
class fibonacci_dataset : public bdata::monomorphic::dataset<int>
{
public:
  // Samples type is int
  typedef int data_type;
  enum { arity = 1 };

private:
  typedef bdata::monomorphic::dataset<int> base;

  struct iterator : base::iterator
  {
    typedef bdata::monomorphic::traits<int>::ref_type ref_type;
    int a;
    int b; // b is the output

    iterator() : a(0), b(0)
    {}

    ref_type operator*()
    {
      return b;
    }

    virtual void operator++()
    {
      a = a + b;
      std::swap(a, b);

      if(!b)
        b = 1;
    }
  };

public:
  fibonacci_dataset()
  {}

  // size is infinite
  bdata::size_t size() const
  {
    return bdata::BOOST_TEST_DS_INFINITE_SIZE;
  }

  // iterator
  virtual iter_ptr begin() const
  {
    return boost::make_shared<iterator>();
  }
};

namespace boost { namespace unit_test { namespace data { namespace monomorphic {
  // registering fibonacci_dataset as a proper dataset
  template <>
  struct is_dataset<fibonacci_dataset> : boost::mpl::true_ {};
}}}}

// Creating a test-driven dataset
BOOST_DATA_TEST_CASE(
  test1,
  fibonacci_dataset() ^ bdata::xrange(10),
  fib_sample, index)
{
  std::cout << "test 1: "
    << fib_sample
    << " / index: " << index
    << std::endl;
  BOOST_TEST(fib_sample <= 13);
}
//]


#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