#include <boost/test/unit_test.hpp>

#include "diophantienne.h"

BOOST_AUTO_TEST_SUITE(test_diophantienne)

    BOOST_AUTO_TEST_CASE(pqa1) {
        auto resultat = diophantienne::PQa(11, 108, 13);
        std::vector<long long> fractions{0, 7, 2, 1, 1, 6, 1, 1};
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.first.begin(), resultat.first.end(), fractions.begin(), fractions.end());
        BOOST_CHECK_EQUAL(resultat.second, 5);
    }

    BOOST_AUTO_TEST_CASE(pqa2) {
        auto resultat = diophantienne::PQa(0, 1, 13);
        std::vector<long long> fractions{3, 1, 1, 1, 1, 6};
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.first.begin(), resultat.first.end(), fractions.begin(), fractions.end());
        BOOST_CHECK_EQUAL(resultat.second, 5);
    }

    BOOST_AUTO_TEST_CASE(pell1_min) {
        BOOST_CHECK_EQUAL(diophantienne::pell1_min(13, 1), std::make_pair(649, 180));
        BOOST_CHECK_EQUAL(diophantienne::pell1_min(13, -1), std::make_pair(18, 5));
    }

    BOOST_AUTO_TEST_CASE(pell4_min) {
        BOOST_CHECK_EQUAL(diophantienne::pell4_min(13, 1), std::make_pair(11, 3));
        BOOST_CHECK_EQUAL(diophantienne::pell4_min(13, -1), std::make_pair(3, 1));
    }

    BOOST_AUTO_TEST_CASE(pell1_1) {
        std::vector<std::pair<long long, long long>> resultat;
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(649, 180),
                std::make_pair(842401, 233640)
        };
        auto callback = [&resultat, &solution](long long x, long long y) {
            resultat.push_back(std::make_pair(x, y));
            return resultat.size() != solution.size();
        };

        diophantienne::pell1<long long>(13, 1, callback);
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell1_2) {
        std::vector<std::pair<long long, long long>> resultat;
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(18, 5),
                std::make_pair(23382, 6485),
                std::make_pair(30349818, 8417525)
        };
        auto callback = [&resultat, &solution](long long x, long long y) {
            resultat.push_back(std::make_pair(x, y));
            return resultat.size() != solution.size();
        };

        diophantienne::pell1<long long>(13, -1, callback);
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell4_1) {
        std::vector<std::pair<long long, long long>> resultat;
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(649, 180),
                std::make_pair(421200, 116820)
        };
        auto callback = [&resultat, &solution](long long x, long long y) {
            resultat.push_back(std::make_pair(x, y));
            return resultat.size() != solution.size();
        };

        diophantienne::pell4<long long>(13, 1, callback);
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell4_2) {
        std::vector<std::pair<long long, long long>> resultat;
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(18, 5),
                std::make_pair(5841, 1620),
                std::make_pair(1895386, 525685)
        };
        auto callback = [&resultat, &solution](long long x, long long y) {
            resultat.push_back(std::make_pair(x, y));
            return resultat.size() != solution.size();
        };

        diophantienne::pell4<long long>(13, -1, callback);
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_funds_bf1) {
        auto resultat = diophantienne::pell_funds_bf<long long>(13, 108);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(11, 1),
                std::make_pair(-11, 1),
                std::make_pair(15, 3),
                std::make_pair(-15, 3),
                std::make_pair(24, 6),
                std::make_pair(-24, 6),
                std::make_pair(41, 11),
                std::make_pair(-41, 11),
                std::make_pair(80, 22),
                std::make_pair(-80, 22),
                std::make_pair(141, 39),
                std::make_pair(-141, 39)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_funds_bf2) {
        auto resultat = diophantienne::pell_funds_bf<long long>(157, 12);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(13, 1),
                std::make_pair(-13, 1),
                std::make_pair(10663, 851),
                std::make_pair(-10663, 851),
                std::make_pair(579160, 46222),
                std::make_pair(-579160, 46222)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_funds_bf3) {
        auto resultat = diophantienne::pell_funds_bf<long long>(13, 27);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(12, 3),
                std::make_pair(-12, 3),
                std::make_pair(40, 11),
                std::make_pair(-40, 11)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_bf1) {
        auto resultat = diophantienne::pell_bf<long long>(13, 108, 1000);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(11, 1),
                std::make_pair(15, 3),
                std::make_pair(24, 6),
                std::make_pair(41, 11),
                std::make_pair(80, 22),
                std::make_pair(141, 39),
                std::make_pair(249, 69),
                std::make_pair(440, 122),
                std::make_pair(869, 241)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_bf2) {
        auto resultat = diophantienne::pell_bf<long long>(157, 12, 100000000);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(13, 1),
                std::make_pair(10663, 851),
                std::make_pair(579160, 46222)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(pell_bf3) {
        auto resultat = diophantienne::pell_bf<long long>(13, 27, 1000);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(12, 3),
                std::make_pair(40, 11),
                std::make_pair(220, 61),
                std::make_pair(768, 213)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(quad_s) {
        auto resultat = diophantienne::quad_s<long long>(8, 9, 72, 1000);
        std::vector<std::pair<long long, long long>> solution{
                std::make_pair(3, 0),
                std::make_pair(9, 8),
                std::make_pair(51, 48),
                std::make_pair(297, 280)
        };
        BOOST_CHECK_EQUAL_COLLECTIONS(resultat.begin(), resultat.end(), solution.begin(), solution.end());
    }

    BOOST_AUTO_TEST_CASE(iterateur_diophantienne1) {
        // 10x + 84y + 16 = 0
        long long int A = 10, B = 84, C = 16;
        size_t compteur = 0;
        for (auto s : diophantienne::equation_lineaire(A, B, C)) {
            ++compteur;
            if (compteur > 10) {
                break;
            }
            BOOST_CHECK_EQUAL(A * s.first + B * s.second + C, 0);
        }
    }

    BOOST_AUTO_TEST_CASE(iterateur_diophantienne2) {
        // 10x + 84y - 16 = 0
        long long int A = 10, B = 84, C = -16;
        size_t compteur = 0;
        for (auto s : diophantienne::equation_lineaire(A, B, C)) {
            ++compteur;
            if (compteur > 10) {
                break;
            }
            BOOST_CHECK_EQUAL(A * s.first + B * s.second + C, 0);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


using namespace std;
#include <cassert>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <algorithm>
#include <random>
#include <boost/random/discrete_distribution.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

using namespace Eigen;

class RNN
{
    private:
        struct loss_result
        {
            /* Structure for recording the result of the loss method */
            double loss;
            MatrixXd dWxh;
            MatrixXd dWhh;
            MatrixXd dWhy;
            MatrixXd dbh;
            MatrixXd dby;
            MatrixXd hs;
        };

        vector <string> data;                                           //vector that holds the corpus
        map <string, int> char_id;                                      //map holds corpus unique char and their id
        map <int, string> id_char;                                      //map id to unique char

        int hiddens;                                                    //hidden layers number
        int sequence_length;                                            //sequence length
        int data_size;                                                  //length of corpus
        int vocab_size;                                                 //vocabulary size for the network

        MatrixXd W_XH;                                                  //Weights from Input to Hiddens
        MatrixXd W_HH;                                                  //Weights from Hidden to Hidden
        MatrixXd W_HY;                                                  //Weights from Hidden to Output
        MatrixXd BH;                                                    //Bias of Hiddens
        MatrixXd BY;                                                    //Bias of Outputs

        double check (const double min, const double max, double &target)
        {
            /* Sub-method to use for Clip Method */
            if (target > max)
            {
                return max;
            }
            else if (target < min)
            {
                return min;
            }
            return target;
        }

        void clip(const double min, const double max, MatrixXd &target)
        {
            /* Clip elements' value inside the Matrix to leave them remain in between min and max */
            for (int r = 0; r < target.rows(); r ++)
            {
                for (auto item = target.row(r).data(); item < target.row(r).data() + target.size(); item += target.outerStride())
                {
                    *item = check(min, max, *item);
                }
            }
        }

        const vector <double> ravel(MatrixXd &target)
        {
            /* Generate a 1 by M Vector; Keep all values of target in 1D vector */
            vector <double> p;

            for (int r = 0; r < target.rows(); r ++)
            {
                for (auto item = target.row(r).data(); item < target.row(r).data() + target.size(); item += target.outerStride())
                {
                    p.push_back(*item);
                }
            }

            return p;
        }

        const int choice (vector <double> &p)
        {
            /* Random Choice Method*/
            random_device rng;                                                  //set rng
            mt19937 gen(rng());                                                 //use another generator to generate upon rng
            boost::random::discrete_distribution<> dist (p.begin(), p.end());   //apply discrete distrib upon probabilities vector

            int randomNumber = dist(gen);                                       //set random ID & return
            return randomNumber;
        }

        loss_result* loss(const vector <int> &inputs, const vector <int> &targets, const MatrixXd &hprev)
        {
            /* Inputs and Targets are list of IDs
               Hprev holds a matrix of initial Hidden States x 1
               Return Loss Result Structure
            */

            map <int, MatrixXd > xs, hs, ys, ps;     //map that holds iteration -> Matrix correspond to Inputs, Hiddens, Outputs, Probs
            MatrixXd original (hprev);               //Make a copy of hprev
            hs[-1] = original;                       //Set -1 as original; this is for the 1st iteration; hs[i - 1]
            double loss = 0.0;                       //Initialize loss

            //Forward Propagation
            for (int i = 0; i < inputs.size(); i ++)
            {
                xs[i] = MatrixXd::Zero(vocab_size, 1);             //Initalizes first X
                xs[i].row(inputs[i]).array() = 1;                  //encode X in 1 of K representation

                //Apply Activation Function; Tanh(X * Weights of X->H + Weights of Hidden * Hidden States + Bias Hidden)
                hs[i] = ((W_XH * xs[i]) + (W_HH * hs[i - 1]) + BH).unaryExpr<double(*)(double)>(&tanh);

                //Set Outputs; Weights of Y * Hidden States + Bias Y
                ys[i] = (W_HY * hs[i]) + BY;

                //Set Exp(current Y)
                MatrixXd ys_exp = ys[i].unaryExpr<double(*)(double)>(&exp);

                //Set Probabilities of the next Y
                ps[i] = ys_exp / ys_exp.sum();

                //Set new loss
                loss += -log(ps[i].coeff(targets[i], 0));
            }

            //Gradients
            MatrixXd dWxh = MatrixXd::Zero(W_XH.rows(), W_XH.cols());
            MatrixXd dWhy = MatrixXd::Zero(W_HY.rows(), W_HY.cols());
            MatrixXd dWhh = MatrixXd::Zero(W_HH.rows(), W_HH.cols());
            MatrixXd dbh = MatrixXd::Zero(BH.rows(), BH.cols());
            MatrixXd dby = MatrixXd::Zero(BY.rows(), BY.cols());
            MatrixXd dh_next = MatrixXd::Zero(hs[0].rows(), hs[0].cols());

            //Backpropagation: Computing Gradient
            for (int i = inputs.size() - 1; i >= 0; i --)
            {
                //copy current probabilities
                MatrixXd dy(ps[i]);
                //back prop into y
                dy.row(targets[i]) = dy.row(targets[i]).array() - 1.0;

                //grab gradients of weights and biases of y
                dWhy += (dy * hs[i].transpose());
                dby.noalias() += dy;

                //back prop into h
                MatrixXd dh = ((W_HY.transpose() * dy) + dh_next);
                //back prop into tanh nonlinear
                MatrixXd dhraw = ((1 - (hs[i].array() * hs[i].array())).array() * dh.array());

                //set gradients
                dbh.noalias() += dhraw;
                dWxh += (dhraw * xs[i].transpose());
                dWhh += (dhraw * hs[i - 1].transpose());
                dh_next = (W_HH.transpose() * dhraw);
            }

            /* Clip values in matrices to prevent gradient explosion */
            clip(-10, 10, dWxh);
            clip(-10, 10, dWhh);
            clip(-10, 10, dWhy);
            clip(-10, 10, dbh);
            clip(-10, 10, dby);

            /* Allocate necessary items into newly created Structure */
            loss_result* item = new loss_result();
            item->loss = loss; item->dbh = dbh; item->dby = dby;
            item->dWhh = dWhh; item->dWhy = dWhy; item->dWxh = dWxh;
            item->hs = hs[inputs.size() - 1];

            return item;
        }

        const vector <int> sample(const MatrixXd &h_prev, const int &seedID, const int &m)
        {
            /* Generate a Sample of Char ID for Text Generation */
            MatrixXd x = MatrixXd::Zero(vocab_size, 1);                      //generate a new bias called x
            x.row(seedID).array() = 1.0;                                     //set the entry at seedID equals to 1
            vector <int> samples;                                            //new samples of char IDs

            for (int i = 0; i < m; i ++)
            {
                MatrixXd h = (W_XH * x) + (W_HH * h_prev) + BH;             //current Hypothesis
                h = h.unaryExpr<double(*)(double)>(&tanh);                  //apply tanh

                MatrixXd y = W_HY * h + BY;                                 //Produce Y
                MatrixXd exp_y = y.unaryExpr<double(*)(double)>(&exp);      //Apply Exp

                MatrixXd p = exp_y / exp_y.sum();                           //Create Probabilities
                vector <double> probabilities = ravel(p);                   //Produce 1 by M vector

                int ix = choice(probabilities);                             //Select a choice based on probabilities

                x = MatrixXd::Zero(vocab_size, 1);                          //Reset X or Inputs

                x.row(ix).array() = 1.0;                                    //Set all elements in row SeedID equals to 1.0 again
                samples.push_back(ix);                                      //Push ID onto samples vector
            }

            return samples;
        }

        const vector <int> getID(const int min, const int max)
        {
            // Get a list of characters based on the range of p and sequence length
            vector <int> item;

            for (int c = min; c < max; c ++)
            {
                item.push_back(char_id[data[c]]);
            }

            return item;
        }

        const string genText(const vector <int> samples)
        {
            /* Generate Text With Parallelism Enabled */
            string output = "";

            #pragma omp declare reduction (merge : string : omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
            #pragma omp parallel for reduction (merge : output)
            for (int i = 0; i < samples.size(); i++)
            {
                #pragma omp critical
                output += id_char[samples[i]];
            }

            return output;
        }

    public:
        RNN(const int &hidden_size, const int &seq_length)
        {
            hiddens = hidden_size;
            sequence_length = seq_length;
        }

        void load(const string filename)
        {
            /* Load File and Enumerate through each character in the text file; get unique char and assign their ID */
            ifstream file(filename);    // grab file
            string line;                // a string for holding  current line
            int count = 0;              // count variable
            vector <string> corpus;

            /* Grab all lines in the text file */
            while(getline(file, line))
            {
                corpus.push_back(line);
            }

            file.close();               // close the file

            //set next line into the map and allocate the same ID into both map; then increment the ID
            char_id["\n"] = count; id_char[count] = "\n"; count ++;
            char_id[" "] = count; id_char[count] = " "; count ++;

            //Loop through the corpus
            //Find Unique characters and allocate it with a unique ID
            //Also, append each character in the line onto the data vector for RNN's feeding
            for (auto &line: corpus)
            {
                int line_size = line.size();
                if (line_size > 0)
                {
                    int c = 0;
                    istringstream word(line);
                    for (string current; word >> current;)
                    {
                        data.push_back(current);
                        if (c < line_size)
                        {
                            data.push_back(" ");
                        }

                        if (char_id.find(current) == char_id.end())
                        {
                            char_id[current] = count;
                            id_char[count] = current;
                            count ++;
                        }
                        c ++;
                    }

                    data.push_back("\n");
                }
                else
                {
                    data.push_back("\n");
                }
            }

            data_size = data.size();                //set data size
            vocab_size = char_id.size();            //set vocab size

            /* Set Weights and Biases */
            W_XH = MatrixXd::Random(hiddens, vocab_size) * 0.01;
            W_HH = MatrixXd::Random(hiddens, hiddens) * 0.01;
            W_HY = MatrixXd::Random(vocab_size, hiddens) * 0.01;
            BH = MatrixXd::Zero(hiddens, 1);
            BY = MatrixXd::Zero(vocab_size, 1);

            cout << "Create Topology: Done!" << endl;
        }

        const void learn()
        {
            /* Learn Method */

            //Initalizes memory variables for Adam Gradient Descent aka AdaGrad
            MatrixXd mWXH = MatrixXd::Zero(W_XH.rows(), W_XH.cols());
            MatrixXd mWHH = MatrixXd::Zero(W_HH.rows(), W_HH.cols());
            MatrixXd mWHY = MatrixXd::Zero(W_HY.rows(), W_HY.cols());
            MatrixXd mBH = MatrixXd::Zero(BH.rows(), BH.cols());
            MatrixXd mBY = MatrixXd::Zero(BY.rows(), BY.cols());

            int n = 0;  //iteration
            int p = 0;  //incrementer for input & target

            double smooth_loss = -log(1.0/ double(vocab_size)) * double(sequence_length);
            double learning_rate = 1 * exp(-1);

            MatrixXd h_prev; //saving previous hiddens

            while (true)
            {
                if ( ((p + sequence_length + 1) >= data_size) or (n == 0))
                {
                    h_prev = MatrixXd::Zero(hiddens, 1);  //Initalizes or Reset the memory of RNN
                    p = 0;                                //Set sequence incrementer back to 0
                }

                /* Grab Inputs and Targets */
                vector <int> inputs = getID(p, p + sequence_length);
                vector <int> targets = getID(p + 1, p + sequence_length + 1);

                // Grab Loss Function's Outputs
                loss_result* item = loss(inputs, targets, h_prev);

                // Set new loss
                smooth_loss = smooth_loss * 0.999 + item->loss * 0.001;

                if ((n % 100 == 0) && (n != 0))
                {
                    /* Generate Text and Display Current Loss */
                    auto samples = sample(h_prev, inputs[0], 200);
                    auto text = genText(samples);

                    cout << "\n" << text << endl;
                    cout << "\n";
                    cout << "Iteration #: "  << n << " Loss: " << smooth_loss << endl;
                }

                if ((n % 1000000 == 0) && (n != 0))
                {
                   // decreases learning rate as number of iteration increases
                   if (learning_rate >= 0.0000001)
                   {
                       learning_rate = learning_rate / (1.0 + (exp(-n/(n + 1))));
                   }
                   else
                   {
                       //restart
                       learning_rate = 0.1;
                   }

                }

                /* Update Weight using AdaGrad*/
                mWXH.noalias() += MatrixXd(item->dWxh.array() * item->dWxh.array());
                W_XH += MatrixXd(-learning_rate * item->dWxh.array() / sqrt(mWXH.array() + 1e-8));

                mWHH.noalias() += MatrixXd(item->dWhh.array() * item->dWhh.array());
                W_HH += MatrixXd(-learning_rate * item->dWhh.array() / sqrt(mWHH.array() + 1e-8));

                mWHY.noalias() += MatrixXd(item->dWhy.array() * item->dWhy.array());
                W_HY += MatrixXd(-learning_rate * item->dWhy.array() / sqrt(mWHY.array() + 1e-8));

                /* Update Biases*/
                mBH.noalias() += MatrixXd(item->dbh.array() * item->dbh.array());
                BH += MatrixXd(-learning_rate * item->dbh.array() / sqrt(mBH.array() + 1e-8));

                mBY.noalias() += MatrixXd(item->dby.array() * item->dby.array());
                BY += MatrixXd(-learning_rate * item->dby.array() / sqrt(mBY.array() + 1e-8));

                delete(item);                           //free the loss result structure

                p += sequence_length;                   //update current sequence length
                n ++;                                   //increment current iteration
            }
        }
};

int main(int argc, char* argv[])
{
    if (argc != 4)
    {
        cout << "Arguments: Corpus File, hidden size, sequence length" << endl;
        return 0;
    }
    //grab arguments and convert char to int for hidden size and sequence length
    string filename = argv[1]; int hidden_size = atoi(argv[2]); int seq_length = atoi(argv[3]);

    Eigen::initParallel();
    RNN rnn = RNN(hidden_size, seq_length);
    rnn.load(filename);
    rnn.learn();
}


#include "Triangle.h"
#include "Ray.h"
#include <Eigen/Geometry>

bool Triangle::intersect(
  const Ray & ray, const double min_t, double & t, Eigen::Vector3d & n) const
{
  ////////////////////////////////////////////////////////////////////////////
  // Replace with your code here
  double beta = 0;
  double gamma = 0;
  double t_prime = 0;

  // corners: a = corners[0], b = corners[1], c = corners[2]
  // Calculate dummy variables for Cramer's rule calculation (from textbook)
  double a = std::get<0>(corners)(0) - std::get<1>(corners)(0);
  double d = std::get<0>(corners)(0) - std::get<2>(corners)(0);
  double g = ray.direction(0);

  double b = std::get<0>(corners)(1) - std::get<1>(corners)(1);
  double e = std::get<0>(corners)(1) - std::get<2>(corners)(1);
  double h = ray.direction(1);

  double c = std::get<0>(corners)(2) - std::get<1>(corners)(2);
  double f = std::get<0>(corners)(2) - std::get<2>(corners)(2);
  double i = ray.direction(2);

  double j = std::get<0>(corners)(0) - ray.origin(0);
  double k = std::get<0>(corners)(1) - ray.origin(1);
  double l = std::get<0>(corners)(2) - ray.origin(2);

  // intermediate dummy variables for calculating beta, gamma, and t
  double ei_minus_hf = (e * i) - (h * f);
  double gf_minus_di = (g * f) - (d * i);
  double dh_minus_eg = (d * h) - (e * g);
  double ak_minus_jb = (a * k) - (j * b);
  double jc_minus_al = (j * c) - (a * l);
  double bl_minus_kc = (b * l) - (k * c);

  double M = (a * ei_minus_hf) + (b * gf_minus_di) + (c * dh_minus_eg);
  t_prime = -(((f * ak_minus_jb) + (e * jc_minus_al) + (d * bl_minus_kc)) / M);

  if (t_prime < min_t)
  {
    return false;
  }

  gamma = ((i * ak_minus_jb) + (h * jc_minus_al) + (g * bl_minus_kc)) / M;
  if (gamma < 0 || gamma > 1)
  {
    return false;
  }

  beta = ((j * ei_minus_hf) + (k * gf_minus_di) + (l * dh_minus_eg)) / M;
  if (beta < 0 || beta > (1 - gamma))
  {
    return false;
  }

  if (t_prime < t)
  {
    t = t_prime;
    // given two vectors u and v, the cross product (u X v) = n (normal vector)
    // u = b - a, v = c - a
    Eigen::Vector3d u = std::get<1>(corners) - std::get<0>(corners);
    Eigen::Vector3d v = std::get<2>(corners) - std::get<0>(corners);
    n = u.cross(v);
    n.normalize();

    return true;
  }
  else
  {
    return false;
  }
  ////////////////////////////////////////////////////////////////////////////
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE RasterizerTestSuite
#include <boost/test/unit_test.hpp>

#include <BilinearPatch.hpp>
#include <QuadraticSolver.hpp>
#include <RationalBilinearInverter.hpp>
#include <Vec.hpp>

BOOST_AUTO_TEST_CASE(TEST_QUADRATIC_SOLVER) {
	QuadraticSolver<float> q(1.0f, 0.0f, -4.0f);
	BOOST_CHECK_EQUAL(q.size(), 2);
	BOOST_CHECK_EQUAL(q[0], -2.0f);
	BOOST_CHECK_EQUAL(q[1], 2.0f);
}

BOOST_AUTO_TEST_CASE(TEST_BILINEAR_PATCH) {
	BilinearPatch<Vec2> patch(
		Vec2(-1, -1), Vec2(1, -1),
		Vec2(-1, 1), Vec2(1, 1)
	);

	BOOST_CHECK_EQUAL(patch(0.5f, 0.0f), Vec2(0, -1));
}

BOOST_AUTO_TEST_CASE(TEST_RATIONAL_BILINEAR_INVERTER) {
	BilinearPatch<Vec4> patch(
		Vec4(0, 0, 1, 1),
		Vec4(1, 1, 1, 1),
		Vec4(0, 1, 1, 1),
		Vec4(1, 2, 1, 1)
	);

	for (int j = 0; j <= 64; ++j) {
		float v = static_cast<float>(j) / 64.0f;

		for (int i = 0; i <= 64; ++i) {
			float u = static_cast<float>(i) / 64.0f;

			Vec4 p4 = patch(u, v);
			Vec2 p2 = Vec2(p4.x(), p4.y()) / (p4.z() * p4.w());

			RationalBilinearInverter rbi(p2,
				patch[0], patch[1], patch[2], patch[3]
			);

			BOOST_REQUIRE(rbi.size() > 0);
			BOOST_CHECK_EQUAL(rbi.front().second, Vec2(u, v));
		}
	}
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Jianwei Cui <thucjw@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <complex>
#include <cmath>
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <int DataLayout>
static void test_1D_fft_ifft_invariant(int sequence_length) {
  Tensor<double, 1, DataLayout> tensor(sequence_length);
  tensor.setRandom();

  array<int, 1> fft;
  fft[0] = 0;

  Tensor<std::complex<double>, 1, DataLayout> tensor_after_fft;
  Tensor<std::complex<double>, 1, DataLayout> tensor_after_fft_ifft;

  tensor_after_fft = tensor.template fft<Eigen::BothParts, Eigen::FFT_FORWARD>(fft);
  tensor_after_fft_ifft = tensor_after_fft.template fft<Eigen::BothParts, Eigen::FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(tensor_after_fft.dimension(0), sequence_length);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(0), sequence_length);

  for (int i = 0; i < sequence_length; ++i) {
    VERIFY_IS_APPROX(static_cast<float>(tensor(i)), static_cast<float>(std::real(tensor_after_fft_ifft(i))));
  }
}

template <int DataLayout>
static void test_2D_fft_ifft_invariant(int dim0, int dim1) {
  Tensor<double, 2, DataLayout> tensor(dim0, dim1);
  tensor.setRandom();

  array<int, 2> fft;
  fft[0] = 0;
  fft[1] = 1;

  Tensor<std::complex<double>, 2, DataLayout> tensor_after_fft;
  Tensor<std::complex<double>, 2, DataLayout> tensor_after_fft_ifft;

  tensor_after_fft = tensor.template fft<Eigen::BothParts, Eigen::FFT_FORWARD>(fft);
  tensor_after_fft_ifft = tensor_after_fft.template fft<Eigen::BothParts, Eigen::FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(tensor_after_fft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(1), dim1);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(1), dim1);

  for (int i = 0; i < dim0; ++i) {
    for (int j = 0; j < dim1; ++j) {
      //std::cout << "[" << i << "][" << j << "]" <<  "  Original data: " << tensor(i,j) << " Transformed data:" << tensor_after_fft_ifft(i,j) << std::endl;
      VERIFY_IS_APPROX(static_cast<float>(tensor(i,j)), static_cast<float>(std::real(tensor_after_fft_ifft(i,j))));
    }
  }
}

template <int DataLayout>
static void test_3D_fft_ifft_invariant(int dim0, int dim1, int dim2) {
  Tensor<double, 3, DataLayout> tensor(dim0, dim1, dim2);
  tensor.setRandom();

  array<int, 3> fft;
  fft[0] = 0;
  fft[1] = 1;
  fft[2] = 2;

  Tensor<std::complex<double>, 3, DataLayout> tensor_after_fft;
  Tensor<std::complex<double>, 3, DataLayout> tensor_after_fft_ifft;

  tensor_after_fft = tensor.template fft<Eigen::BothParts, Eigen::FFT_FORWARD>(fft);
  tensor_after_fft_ifft = tensor_after_fft.template fft<Eigen::BothParts, Eigen::FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(tensor_after_fft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(1), dim1);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(2), dim2);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(1), dim1);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(2), dim2);

  for (int i = 0; i < dim0; ++i) {
    for (int j = 0; j < dim1; ++j) {
      for (int k = 0; k < dim2; ++k) {
        VERIFY_IS_APPROX(static_cast<float>(tensor(i,j,k)), static_cast<float>(std::real(tensor_after_fft_ifft(i,j,k))));
      }
    }
  }
}

template <int DataLayout>
static void test_sub_fft_ifft_invariant(int dim0, int dim1, int dim2, int dim3) {
  Tensor<double, 4, DataLayout> tensor(dim0, dim1, dim2, dim3);
  tensor.setRandom();

  array<int, 2> fft;
  fft[0] = 2;
  fft[1] = 0;

  Tensor<std::complex<double>, 4, DataLayout> tensor_after_fft;
  Tensor<double, 4, DataLayout> tensor_after_fft_ifft;

  tensor_after_fft = tensor.template fft<Eigen::BothParts, Eigen::FFT_FORWARD>(fft);
  tensor_after_fft_ifft = tensor_after_fft.template fft<Eigen::RealPart, Eigen::FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(tensor_after_fft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(1), dim1);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(2), dim2);
  VERIFY_IS_EQUAL(tensor_after_fft.dimension(3), dim3);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(0), dim0);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(1), dim1);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(2), dim2);
  VERIFY_IS_EQUAL(tensor_after_fft_ifft.dimension(3), dim3);

  for (int i = 0; i < dim0; ++i) {
    for (int j = 0; j < dim1; ++j) {
      for (int k = 0; k < dim2; ++k) {
        for (int l = 0; l < dim3; ++l) {
          VERIFY_IS_APPROX(static_cast<float>(tensor(i,j,k,l)), static_cast<float>(tensor_after_fft_ifft(i,j,k,l)));
        }
      }
    }
  }
}

void test_cxx11_tensor_ifft() {
  CALL_SUBTEST(test_1D_fft_ifft_invariant<ColMajor>(4));
  CALL_SUBTEST(test_1D_fft_ifft_invariant<ColMajor>(16));
  CALL_SUBTEST(test_1D_fft_ifft_invariant<ColMajor>(32));
  CALL_SUBTEST(test_1D_fft_ifft_invariant<ColMajor>(1024*1024));

  CALL_SUBTEST(test_2D_fft_ifft_invariant<ColMajor>(4,4));
  CALL_SUBTEST(test_2D_fft_ifft_invariant<ColMajor>(8,16));
  CALL_SUBTEST(test_2D_fft_ifft_invariant<ColMajor>(16,32));
  CALL_SUBTEST(test_2D_fft_ifft_invariant<ColMajor>(1024,1024));

  CALL_SUBTEST(test_3D_fft_ifft_invariant<ColMajor>(4,4,4));
  CALL_SUBTEST(test_3D_fft_ifft_invariant<ColMajor>(8,16,32));
  CALL_SUBTEST(test_3D_fft_ifft_invariant<ColMajor>(16,4,8));
  CALL_SUBTEST(test_3D_fft_ifft_invariant<ColMajor>(256,256,256));

  CALL_SUBTEST(test_sub_fft_ifft_invariant<ColMajor>(4,4,4,4));
  CALL_SUBTEST(test_sub_fft_ifft_invariant<ColMajor>(8,16,32,64));
  CALL_SUBTEST(test_sub_fft_ifft_invariant<ColMajor>(16,4,8,12));
  CALL_SUBTEST(test_sub_fft_ifft_invariant<ColMajor>(64,64,64,64));
}


/*
A class to represent an extension field
*/

#ifndef EXTENSION_FIELD
#define EXTENSION_FIELD

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>

#include "polynomial.hpp"

using namespace std;

// extension field element (EFE)
template<class PFE> class EFE:
	boost::field_operators< EFE<PFE>,
	boost::equality_comparable< EFE<PFE>
	> >
{
public:
	//! an element of the extension field is a polynomial with coefficients in the prime field PFE
	polynomial<PFE> el;
	//! the degree of the extension
	static unsigned m;
	//! primitive polynomial defining the extension field
	static polynomial<PFE> prim_poly;
	//! constructors
	EFE(vector<PFE>& v): el(polynomial<PFE>(v)) {};
	EFE(polynomial<PFE>& el_): el(el_) {};
	EFE(unsigned x){ // set the coeff with exponent zero to x
		vector<PFE> tmpv(m, PFE(0) );
		tmpv[0] = PFE(x);
		el = polynomial<PFE>(tmpv);
	}
	EFE(){};

	EFE& operator += (const EFE& x){
		el += x.el;
		return *this;
	}

	EFE& operator -= (const EFE& x){
		el -= x.el;
		return *this;
	}

	EFE& operator *= (const EFE& x){
		// multiply
		polynomial<PFE> ptmp = el*x.el;
		// modulo the primitive polynomial

		int exp;
		while( (exp = ptmp.degree() - prim_poly.degree()) >= 0 ){
			// difference in exponents
			//unsigned exp = ptmp.degree() - prim_poly.degree();
			// primitive polynomial; to be muliplied..
			polynomial<PFE> primtmp = prim_poly;
			PFE co_mo = ptmp.poly[ptmp.degree()];
			co_mo /= prim_poly.poly[prim_poly.degree()];
			primtmp.multiply_monomial( co_mo , exp  );
			ptmp -= primtmp;
		}

		ptmp.poly.resize(ptmp.degree()+1);
		el = ptmp;
		return *this;
	}

	EFE& operator /= (const EFE& x){
		*this = (x.inverse() * (*this));
		return *this;
	}

	//! compute the multiplicative inverse via the extended Euclidean algorithm
	EFE inverse() const {

		polynomial<PFE> rem1 = prim_poly;
		polynomial<PFE> rem2 = el;
		polynomial<PFE> aux1 = polynomial<PFE>(0);
		polynomial<PFE> aux2 = polynomial<PFE>(1);

		while(! rem2.iszero() ){
			// res.first = quotient(rem1/rem2)
			// res.second = remainder(rem1/rem2)

			pair<polynomial<PFE>, polynomial<PFE> > res = divide<PFE>(rem1,rem2);

			polynomial<PFE> aux_new = polynomial<PFE>(0) - res.first*aux2 + aux1;

			// prepare for the next step
			rem1 = rem2;
			rem2 = res.second;
			aux1 = aux2;
			aux2 = aux_new;

		}

		aux1.multiply_monomial(pow(rem1.poly[0],-1), 0);

		return aux1;
	}

	unsigned order() const {
		// multiplicative neutral element = 1
		//PFE one = PFE(1);
		vector<PFE> vecone = vector<PFE>(1,PFE(1));
		EFE one = EFE(vecone);
		EFE tmp = *this;  //EFE(el);
		unsigned ord = 1;
		while(!(tmp == one)){
			ord++;
			tmp *= *this;
			//cout << tmp << endl;
		}
		return ord;
	}
	/*
	// not a good solution
	vector<unsigned> tovec() const {
		vector<unsigned> vec(m,0);
		for(unsigned i=0;i<el.poly.size();++i){
			vec[i] = el.poly[i].element;
		}
		return vec;
	}
	*/

	vector<PFE> tovec() const {
		if(el.poly.size()!=m){
			vector<PFE> tmp(m,PFE(0));
			for(unsigned i=0;i<el.poly.size();++i){
				tmp[i] = el.poly[i];
			}
			return tmp;
		}else
			return el.poly;
	}

	bool iszero() const {
		vector<PFE> veczero = vector<PFE>(1,PFE(0));
		EFE zero = EFE(veczero);
		return (zero == *this);
	}

	bool isempty() const {
		return (el.poly.size() == 0);
	}

	bool operator ==(const EFE& x) const {

		for(unsigned i = 0; i < min(el.poly.size(),x.el.poly.size()); ++i)
			if(!(x.el.poly[i] == el.poly[i]) ) return false;

		// need to verify if the coefficients of the larger vector that are necessarily zero in the smaller vector are also zero in the larger vector
		if(el.poly.size() > x.el.poly.size()){
			for(unsigned i = x.el.poly.size() ; i<el.poly.size(); ++i)
				if( !(el.poly[i].iszero()) ) return false;
		} else if(el.poly.size() < x.el.poly.size()){
			for(unsigned i = el.poly.size(); i<x.el.poly.size(); ++i)
				if( !(x.el.poly[i].iszero()) ) return false;
		}

		return true;
	}
};

 template<class PFE>
    ostream &operator<<(ostream &stream, const EFE<PFE> x)
    {
	  	stream <<  x.el;
      	return stream; // must return stream
	};

template<class PFE>
 	EFE<PFE> pow(const EFE<PFE>& a, int exp){

		if(a.iszero()) return a;
		if(exp == 0) {
			vector<PFE> vecone = vector<PFE>(1,PFE(1));
			return EFE<PFE>(vecone);
		}

		EFE<PFE> tmp;

		if(exp < 0){
			EFE<PFE> am = a.inverse();
			tmp = am;
			for(unsigned i =1; i < abs(exp); ++i) tmp *= am;
		} else { // exp > 0
			tmp = a;
			for(unsigned i =1; i < abs(exp); ++i) tmp *= a;
		}

		return tmp;
	};

#endif


/*
 * Copyright Nick Thompson, 2021
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include "math_unit_test.hpp"
#include <random>
#include <boost/math/tools/cubic_roots.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::tools::cubic_roots;
using boost::math::tools::cubic_root_residual;
using std::cbrt;

template<class Real>
void test_zero_coefficients()
{
    Real a = 0;
    Real b = 0;
    Real c = 0;
    Real d = 0;
    auto roots = cubic_roots(a,b,c,d);
    CHECK_EQUAL(roots[0], Real(0));
    CHECK_EQUAL(roots[1], Real(0));
    CHECK_EQUAL(roots[2], Real(0));

    a = 1;
    roots = cubic_roots(a,b,c,d);
    CHECK_EQUAL(roots[0], Real(0));
    CHECK_EQUAL(roots[1], Real(0));
    CHECK_EQUAL(roots[2], Real(0));

    a = 1;
    d = 1;
    // x^3 + 1 = 0:
    roots = cubic_roots(a,b,c,d);
    CHECK_EQUAL(roots[0], Real(-1));
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);
    d = -1;
    // x^3 - 1 = 0:
    roots = cubic_roots(a,b,c,d);
    CHECK_EQUAL(roots[0], Real(1));
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);

    d = -2;
    // x^3 - 2 = 0
    roots = cubic_roots(a,b,c,d);
    CHECK_ULP_CLOSE(roots[0], cbrt(Real(2)), 2);
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);

    d = -8;
    roots = cubic_roots(a,b,c,d);
    CHECK_EQUAL(roots[0], Real(2));
    CHECK_NAN(roots[1]);
    CHECK_NAN(roots[2]);

    // (x-1)(x-2)(x-3) = x^3 - 6x^2 + 11x - 6
    roots = cubic_roots(Real(1), Real(-6), Real(11), Real(-6));
    CHECK_ULP_CLOSE(roots[0], Real(1), 2);
    CHECK_ULP_CLOSE(roots[1], Real(2), 2);
    CHECK_ULP_CLOSE(roots[2], Real(3), 2);

    // Double root:
    // (x+1)^2(x-2) = x^3 - 3x - 2:
    // Note: This test is unstable wrt to perturbations!
    roots = cubic_roots(Real(1), Real(0), Real(-3), Real(-2));
    CHECK_ULP_CLOSE(Real(-1), roots[0], 2);
    CHECK_ULP_CLOSE(Real(-1), roots[1], 2);
    CHECK_ULP_CLOSE(Real(2), roots[2], 2);

    std::uniform_real_distribution<Real> dis(-2,2);
    std::mt19937 gen(12345);
    // Expected roots
    std::array<Real, 3> r;
    int trials = 10;
    for (int i = 0; i < trials; ++i) {
        // Mathematica:
        // Expand[(x - r0)*(x - r1)*(x - r2)]
        // - r0 r1 r2 + (r0 r1 + r0 r2 + r1 r2) x
        // - (r0 + r1 + r2) x^2 + x^3
        for (auto & root : r) {
            root = static_cast<Real>(dis(gen));
        }
        std::sort(r.begin(), r.end());
        Real a = 1;
        Real b = -(r[0] + r[1] + r[2]);
        Real c = r[0]*r[1] + r[0]*r[2] + r[1]*r[2];
        Real d = -r[0]*r[1]*r[2];

        auto roots = cubic_roots(a, b, c, d);
        // I could check the condition number here, but this is fine right?
        if(!CHECK_ULP_CLOSE(r[0], roots[0], 25)) {
            std::cerr << "  Polynomial x^3 + " << b << "x^2 + " << c << "x + " << d << " has roots {";
            std::cerr << r[0] << ", " << r[1] << ", " << r[2] << "}, but the computed roots are {";
            std::cerr << roots[0] << ", " << roots[1] << ", " << roots[2] << "}\n";
        }
        CHECK_ULP_CLOSE(r[1], roots[1], 25);
        CHECK_ULP_CLOSE(r[2], roots[2], 25);
        for (auto root : roots) {
            auto res = cubic_root_residual(a, b,c,d, root);
            CHECK_LE(abs(res[0]), 20*res[1]);
        }
    }
}

int main()
{
    test_zero_coefficients<float>();
    test_zero_coefficients<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_zero_coefficients<long double>();
#endif

#ifdef BOOST_HAS_FLOAT128
    // For some reason, the quadmath is way less accurate than the float/double/long double:
    //test_zero_coefficients<float128>();
#endif

    return boost::math::test::report_errors();
}


#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

int main()
{
  VectorXf v(6);

  v << 1, 2, 3, 4, 5, 6;

  //print first three elements
  cout << "-- head(3) --" << endl
    << v.head(3) << endl << endl;

  //print last three elements
  cout << "-- tail(3) --" << endl
    << v.tail(3) << endl << endl;

  //print between 2nd and 5th elem. inclusive
  cout << "-- segment(1,4) --" << endl
    << v.segment(1,4) << endl;
}


#include <Eigen/Core>
#include <Spectra/SymEigsSolver.h>

#include <iostream>

int main() {
    // We are going to calculate the eigenvalues of M
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(10, 10);
    Eigen::MatrixXd M = A + A.transpose();

    // Construct matrix operation object using the wrapper class DenseSymMatProd
    Spectra::DenseSymMatProd<double> op(M);

    // Construct eigen solver object, requesting the largest three eigenvalues
#ifdef SPECTRA_LESS_1_0_0
    Spectra::SymEigsSolver< double, Spectra::LARGEST_ALGE, Spectra::DenseSymMatProd<double> > eigs(&op, 3, 6);
#else
    Spectra::SymEigsSolver<Spectra::DenseSymMatProd<double>> eigs(op, 3, 6);
#endif

    // Initialize and compute
    eigs.init();
#ifdef SPECTRA_LESS_1_0_0
    int nconv = eigs.compute();
#else
    int nconv = eigs.compute(Spectra::SortRule::LargestAlge);
#endif

    // Retrieve results
    Eigen::VectorXd evalues;
#ifdef SPECTRA_LESS_1_0_0
    if (eigs.info() == Spectra::SUCCESSFUL) {
#else
    if (eigs.info() == Spectra::CompInfo::Successful) {
#endif
        evalues = eigs.eigenvalues();
    }

    std::cout << "Eigenvalues found:\n" << evalues << std::endl;

    return 0;
}


#include <Eigen/SVD>
#include <rct_optimizations/validation/noise_qualification.h>
#include <rct_optimizations/pnp.h>

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

namespace rct_optimizations
{

Eigen::Quaterniond computeQuaternionMean(const std::vector<Eigen::Quaterniond>& quaterns)
{
 /* Mean quaternion is found using method described by Markley et al: Quaternion Averaging
  * https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
  *
  * M = sum(w_i * q_i * q_i^T)    Eq. 12
  * q_bar = argmax(q^T * M * q)   Eq. 13
  *
  * "The solution of this maximization problem is well known. The average quaternion is
  * the eigenvector of M corresponding to the maximum eigenvalue."
  *
  * In the above equations, w_i is the weight of the ith quaternion.
  * In this case, all quaternions are equally weighted (i.e. w_i = 1)
  */

  Eigen::Matrix4d M = Eigen::Matrix4d::Zero();

  for(const Eigen::Quaterniond& q : quaterns)
  {
    M += q.coeffs() * q.coeffs().transpose();
  }

  // Calculate the SVD of the M matrix
  Eigen::JacobiSVD<Eigen::Matrix4d> svd(M, Eigen::ComputeFullU);

  // The eigenvectors are represented by the columns of the U matrix; the eigenvector corresponding to the largest eigenvalue is in row 0
  Eigen::Quaterniond q;
  q.coeffs() << svd.matrixU().col(0);

  assert(std::isnan(q.w()) == false &&
         std::isnan(q.x()) == false &&
         std::isnan(q.y()) == false &&
         std::isnan(q.z()) == false);

  return q;
};

QuaternionStats computeQuaternionStats(const std::vector<Eigen::Quaterniond> &quaternions)
{
  QuaternionStats q_stats;
  q_stats.mean = computeQuaternionMean(quaternions);

  double q_var = 0.0;
  for (const Eigen::Quaterniond &q : quaternions)
  {
    q_var += std::pow(q_stats.mean.angularDistance(q), 2.0);
  }
  q_var /= static_cast<double>(quaternions.size() - 1);
  q_stats.stdev = std::sqrt(q_var);

  return q_stats;
}

PnPNoiseStat qualifyNoise2D(const std::vector<PnPProblem>& params)
{
  PnPNoiseStat output;
  std::size_t count = params.size();

  std::vector<Eigen::Isometry3d> solution_transforms;
  solution_transforms.reserve(count);

  std::vector<Eigen::Vector3d> translations;
  translations.reserve(count);

  std::vector<Eigen::Quaterniond> orientations;
  orientations.reserve(count);

  namespace ba = boost::accumulators;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> x_acc;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> y_acc;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> z_acc;

  for (auto& prob : params)
  {
    rct_optimizations::PnPResult result;

    result = rct_optimizations::optimize(prob);

    if (result.converged)
    {
      //we will save the full result here for debugging purposes
      solution_transforms.push_back(result.camera_to_target);
      translations.push_back(solution_transforms.back().translation());

      x_acc(result.camera_to_target.translation()(0));
      y_acc(result.camera_to_target.translation()(1));
      z_acc(result.camera_to_target.translation()(2));

      orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation()));
    }
  }

  output.p_stat.mean << ba::mean(x_acc), ba::mean(y_acc), ba::mean(z_acc);
  output.p_stat.stdev << std::sqrt(ba::variance(x_acc)), std::sqrt(ba::variance(y_acc)), std::sqrt(ba::variance(z_acc));
  output.q_stat = computeQuaternionStats(orientations);

  return output;
}

PnPNoiseStat qualifyNoise3D(const std::vector<PnPProblem3D>& params)
{
  PnPNoiseStat output;
  std::size_t count = params.size();

  std::vector<Eigen::Isometry3d> solution_transforms;
  solution_transforms.reserve(count);

  std::vector<Eigen::Vector3d> translations;
  translations.reserve(count);

  std::vector<Eigen::Quaterniond> orientations;
  orientations.reserve(count);

  namespace ba = boost::accumulators;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> x_acc;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> y_acc;
  ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> z_acc;

  for (auto& prob : params)
  {
    rct_optimizations::PnPResult result;

    result = rct_optimizations::optimize(prob);

    if (result.converged)
    {
      //we will save the full result here for debugging purposes
      solution_transforms.push_back(result.camera_to_target);
      translations.push_back(solution_transforms.back().translation());

      x_acc(result.camera_to_target.translation()(0));
      y_acc(result.camera_to_target.translation()(1));
      z_acc(result.camera_to_target.translation()(2));

      orientations.push_back(Eigen::Quaterniond(result.camera_to_target.rotation()));
    }
  }

  output.p_stat.mean << ba::mean(x_acc), ba::mean(y_acc), ba::mean(z_acc);
  output.p_stat.stdev << std::sqrt(ba::variance(x_acc)), std::sqrt(ba::variance(y_acc)), std::sqrt(ba::variance(z_acc));
  output.q_stat = computeQuaternionStats(orientations);

  return output;
}

}//rct_optimizations


#pragma once

#include <string>
#include <cmath>
#include <exception>
#include <armadillo>

/*!
  * \defgroup typedefs Typedefs
  */

namespace utils
{
    inline double pow2(const double a);
    inline arma::vec pow2(const arma::vec& a);
    inline double pow3(const double a);
    inline double pow4(const double a);
    inline double pow5(const double a);
    inline double pow6(const double a);

    inline arma::vec cubeRoot(const arma::vec& input);

    inline arma::vec centerDifference(const arma::vec& input);

    inline arma::vec centerDifference(const arma::subview_col<arma::mat::elem_type>& input);

    inline arma::vec centerSum(const arma::vec& input);

    inline arma::vec centerSum(const arma::subview_col<arma::mat::elem_type>& input);

    inline arma::vec centerAverage(const arma::vec& input);

    inline arma::vec centerAverage(const arma::subview_col<arma::mat::elem_type>& input);

    inline arma::vec weightedAverage(const arma::vec& weightA, const arma::vec& weightB, const arma::vec& A, const arma::vec& B);

    inline double weightedAverage(const double weightA, const double weightB, const double A, const double B);

    inline bool isRelativeDifferenceWithinTolerance(
            const double oldValue,
            const double newValue,
            const double tolerance);

    inline bool areRelativeDifferencesWithinTolerance(
            const arma::vec& oldValues,
            const arma::vec& newValues,
            const double tolerance);

    inline double convertBurialToStandardDefinition(
            const double burial,
            const double innerDiameter,
            const arma::rowvec& width);

    inline double convertBurialToStandardDefinition(
            const double burial,
            const double innerDiameter,
            const arma::subview_row<arma::mat::elem_type>& width);

    inline arma::vec convertBurialToStandardDefinition(
            const arma::vec& burial,
            const arma::vec& innerDiameter,
            const arma::colvec& width);

    inline arma::vec convertBurialToStandardDefinition(
            const arma::vec& burial,
            const arma::vec& innerDiameter,
            const arma::mat& width);

    inline arma::vec smooth(const arma::vec& x, const double smoothness);

    inline arma::vec smoothTransition(const arma::vec& firstPart, const arma::vec& lastPart, double smoothness = 1.0, const double timeStep = 60);

    inline arma::vec createSmoothTransient(
            const double initialValue,
            const double finalValue,
            const arma::uword n,
            const double smoothness,
            const arma::uword dt);

    inline arma::vec findLogSpacedConcentricShellWidths(
            const double innerRadius,
            const double outerRadius,
            const arma::uword nShells);

    inline arma::cube loadCubeFromFile(const std::string& filename, const arma::file_type& fileType = arma::csv_ascii);
    inline arma::mat loadMatFromFile(const std::string& filename, const arma::file_type& fileType = arma::csv_ascii);
    inline arma::vec loadVecFromFile(const std::string& filename, const arma::file_type& fileType = arma::csv_ascii);
}

inline double utils::pow2(const double a)
{
    return a*a;
}

inline arma::vec utils::pow2(const arma::vec& a)
{
    return a%a;
}

inline double utils::pow3(const double a)
{
    return a*a*a;
}

inline double utils::pow4(const double a)
{
    return a*a*a*a;
}

inline double utils::pow5(const double a)
{
    return a*a*a*a*a;
}

inline double utils::pow6(const double a)
{
    return a*a*a*a*a*a;
}

inline arma::vec utils::cubeRoot(const arma::vec& input)
{
    arma::vec cubed = arma::zeros<arma::vec>(input.n_elem);
    for (arma::uword i = 0; i < input.n_elem; i++)
    {
        cubed(i) = std::cbrt(input(i));
    }
    return cubed;
}

arma::vec utils::centerDifference(const arma::vec& input)
{
    return input(arma::span(1, input.n_elem - 1)) - input(arma::span(0, input.n_elem - 2));
}

arma::vec utils::centerDifference(const arma::subview_col<arma::mat::elem_type>& input)
{
    return input.rows(1, input.n_elem - 1) - input.rows(0, input.n_elem - 2);
}

arma::vec utils::centerSum(const arma::vec& input)
{
    return input(arma::span(1, input.n_elem - 1)) + input(arma::span(0, input.n_elem - 2));
}

arma::vec utils::centerSum(const arma::subview_col<arma::mat::elem_type>& input)
{
    return input.rows(1, input.n_elem - 1) + input.rows(0, input.n_elem - 2);
}

arma::vec utils::centerAverage(const arma::vec& input)
{
    return centerSum(input)/2.0;
}

arma::vec utils::centerAverage(const arma::subview_col<arma::mat::elem_type>& input)
{
    return centerSum(input)/2.0;
}

double utils::weightedAverage(const double weightA, const double weightB, const double A, const double B)
{
    if (std::round(weightA + weightB) == 0)
    {
        throw std::invalid_argument("utils::weightedAverage(): both weights are zero");
    }

    return (A*weightA + B*weightB)/(weightA + weightB);
}

arma::vec utils::weightedAverage(const arma::vec& weightA, const arma::vec& weightB, const arma::vec& A, const arma::vec& B)
{
    if (!arma::all((weightA + weightB) != 0))
    {
        throw std::invalid_argument("utils::weightedAverage(): both weights are zero");
    }

    return (A % weightA + B % weightB)/(weightA + weightB);
}

bool utils::isRelativeDifferenceWithinTolerance(
        const double oldValue,
        const double newValue,
        const double tolerance)
{
    double relativeDifference;
    if (oldValue != 0)
    {
        relativeDifference = std::abs(newValue - oldValue)/std::abs(oldValue);
    }
    else if (newValue != 0)
    {
        relativeDifference = std::abs(newValue - oldValue)/std::abs(newValue);
    }
    else
    {
        // both values zero, difference also zero
        return true;
    }
    return relativeDifference < tolerance;
}

bool utils::areRelativeDifferencesWithinTolerance(
        const arma::vec& oldValues,
        const arma::vec& newValues,
        const double tolerance)
{
    for (arma::uword i = 0; i < oldValues.n_elem; i++)
    {
        double relativeDifference;
        if (oldValues(i) != 0)
        {
            relativeDifference = std::abs(newValues(i) - oldValues(i))/std::abs(oldValues(i));
        }
        else if (newValues(i) != 0)
        {
            relativeDifference = std::abs(newValues(i) - oldValues(i))/std::abs(newValues(i));
        }
        else
        {
            // both values zero, difference also zero
            continue;
        }

        if (relativeDifference > tolerance)
        {
            return false;
        }
    }

    return true;
}

double utils::convertBurialToStandardDefinition(
        const double burial,
        const double innerDiameter,
        const arma::rowvec& width)
{
    return burial + innerDiameter/2.0 + arma::sum(width);
}

double utils::convertBurialToStandardDefinition(
        const double burial,
        const double innerDiameter,
        const arma::subview_row<arma::mat::elem_type>& width)
{
    return burial + innerDiameter/2.0 + arma::sum(width);
}

arma::vec utils::convertBurialToStandardDefinition(
        const arma::vec& burial,
        const arma::vec& innerDiameter,
        const arma::colvec& width)
{
    return burial + innerDiameter/2.0 + arma::sum(width);
}

arma::vec utils::convertBurialToStandardDefinition(
        const arma::vec& burial,
        const arma::vec& innerDiameter,
        const arma::mat& width)
{
    // burial depth is defined as "from top of soil to center of pipe" in our code,
    // but given as distance from top of pipe to top of soil in thesis and in SIM
    // this means that we have to add half a diameter + the width of all layers to convert
    arma::vec convertedBurial = arma::zeros<arma::vec>(burial.n_elem);
    for (arma::uword i = 0; i < burial.n_elem; i++)
    {
        convertedBurial(i) = convertBurialToStandardDefinition(burial(i), innerDiameter(i), width.row(i));
    }

    return convertedBurial;
}

arma::vec utils::smooth(const arma::vec& x, const double smoothness)
{
    return 0.5 + 0.5*tanh((x - 0.5)/smoothness); // use x - 0.5 to get transition point to be between x = 0 and x = 1.0
}

arma::vec utils::smoothTransition(
        const arma::vec& firstPart,
        const arma::vec& lastPart,
        double smoothness,
        const double timeStep)
{
    // smooths transition from firstPart to lastPart by using a tanh function
    // idea from here: http://www.j-raedler.de/2010/10/smooth-transition-between-functions-with-tanh/

    // the smoothing is time-independent (or time-depending, depending on how you look at it),
    // meaning that changing the time step but using the same smoothness should result in the same smoothed curve,
    // if you also scale the x-axis correctly (meaning: use time on the x-axis, not number of time steps)

    // a smoothness of 1.0 will give a transition that uses approx. 10 minutes
    // a smoothness of 5.0 will use around 30 minutes

    arma::uword m = firstPart.n_elem;
    arma::uword n = lastPart.n_elem;

    // non-dimensionalizing smoothness
    // default time step of 60 gives no scaling
    smoothness *= 60.0/timeStep; // 60 is standard timestep

    arma::vec f = arma::join_cols(
                firstPart,
                arma::zeros<arma::vec>(n) + firstPart(m-1) // repeat last element of firstPart
                );
    arma::vec g = arma::join_cols(
                arma::zeros<arma::vec>(m) + lastPart(0), // repeat first element of lastPart
                lastPart
                );

    arma::vec x = arma::linspace(-int(m)+1, n, m+n);  // !!! cast to int before subtracting, since m is unsigned int!!!
    arma::vec s = smooth(x, smoothness);

    arma::vec h = (1-s) % f + s % g;

    return h;
}

arma::vec utils::createSmoothTransient(
        const double initialValue,
        const double finalValue,
        const arma::uword n,
        const double smoothness,
        const arma::uword dt)
{
    // transient will be centered in interval [0, n]
    if (n % 2 != 0)
    {
        std::cout << "n (" << n << ") should be divisible by 2!" << std::endl;
    }
    arma::vec a = arma::zeros<arma::vec>(n/2) + initialValue;
    arma::vec b = arma::zeros<arma::vec>(n/2) + finalValue;
    return smoothTransition(a, b, smoothness, dt);
}

arma::vec utils::findLogSpacedConcentricShellWidths(
        const double innerRadius,
        const double outerRadius,
        const arma::uword nShells)
{
    arma::vec logSpacedLayerRadii = arma::logspace(log10(innerRadius), log10(outerRadius), nShells+1);
    arma::vec logSpacedLayerWidths = arma::zeros<arma::vec>(nShells);
    for (arma::uword i = 0; i < nShells; i++)
    {
        logSpacedLayerWidths(i) = logSpacedLayerRadii(i+1) - logSpacedLayerRadii(i);
    }

    return logSpacedLayerWidths;
}

arma::cube utils::loadCubeFromFile(const std::string& filename, const arma::file_type& fileType)
{
    arma::cube c;
    if (!c.load(filename, fileType))
    {
        throw std::runtime_error("Couldn't load file \"" + filename + "\"");
    }

    return c;
}

arma::mat utils::loadMatFromFile(const std::string& filename, const arma::file_type& fileType)
{
    arma::mat m;
    if (!m.load(filename, fileType))
    {
        throw std::runtime_error("Couldn't load file \"" + filename + "\"");
    }

    return m;
}

arma::vec utils::loadVecFromFile(const std::string& filename, const arma::file_type& fileType)
{
    arma::vec v;
    if (!v.load(filename, fileType))
    {
        throw std::runtime_error("Couldn't load file \"" + filename + "\"");
    }

    return v;
}


#include <iostream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <boost/math/special_functions/beta.hpp>

using namespace std;
using namespace boost::math;

inline double calcindex(double beta, const double a, const double nn)
{
  double p = double(a)/double(nn);
  const double b = nn - a;
  const double r = ((double)a/(double)nn);
  const double rup = ((double)a+1.0)/((double)nn+1.0);
  const double rdn = ((double)a)/((double)nn+1.0);

  for(int k = 0; k < 10; ++k)
  {
    if(p > 1 || p < r || p != p)
    {
      cerr << "Warning! returning approximation for a = " << a << " and b = " << b << endl;
      return r;
    }
    const double safe = p/(1-beta);

    const double lhs = r*(1-ibeta(a+1,b,p));
    const double expmax = lhs + p*ibeta((double)a,b,p);
    //cout << expmax << ". lhs = " << lhs << ". rhs = " << p*ibeta((double)a,b,p) << endl;
    const double risky = r+ beta/(1-beta)*expmax;

    const double orig = risky - safe;
    const double deriv = beta/(1-beta)*ibeta(a,b,p) - 1/(1-beta);
    p -= orig/deriv;
  }
  return p;
}

void computeIndices(const int n, const double beta, vector<vector<double> >& indices)
{
  for(int nn = n-1; nn >= 2; --nn)
  {
    for(int a = 1; a <= nn-1; ++a)
    {
      double p = calcindex(beta, a, nn);
      indices[a-1][nn-a-1] = p;
    }
  }
}

int main(int argc, const char** args)
{
  if(argc != 3)
  {
    cerr << "need 2 arguments [n] [beta]" << endl;
    exit(1);
  }
  int n;
  string strnum = args[1];
  stringstream ss(strnum);
  ss >> n;

  double beta;
  string betastr = args[2];
  stringstream ssbeta(betastr);
  ssbeta >> beta;

  //cout << calcindex(beta,292,292+1170) << endl;
  //cout << "finding indices for size limit [" << n << "] and step size [" << step << "]" << endl;
  vector<vector<double> > indices(n-1, vector<double>(n-1, 0));
  computeIndices(n, beta, indices);

  for(int i = 0; i < n-1; ++i)
  {
    for(int j=0; j < n-2; ++j)
    {
      cout << indices[i][j] << ", ";
    }
    cout << indices[i][n-2] << endl;
  }
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FCPPT_MATH_VECTOR_LENGTH_HPP_INCLUDED
#define FCPPT_MATH_VECTOR_LENGTH_HPP_INCLUDED

#include <fcppt/cast/int_to_float.hpp>
#include <fcppt/math/size_type.hpp>
#include <fcppt/math/vector/length_square.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/utility/enable_if.hpp>
#include <cmath>
#include <type_traits>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace math
{
namespace vector
{

/**
\brief Calculates the length of a vector
\ingroup fcpptmathvector

This is the Euclidean distance (or 2 norm) of the vector. There are two
variants of this function. One works only for floating point vectors (so you
don't have to specify the return type). The other one works for arbitrary
vectors and uses a cast internally.

\note
If you want the length squared, use fcppt::math::vector::length_square

\see
fcppt::math::vector::length_square

Example:

\code
typedef
fcppt::math::vector::static_<float,3>
vector3f;

typedef
fcppt::math::vector::static_<int,3>
vector3i;

vector3f vf(1.0f,2.0f,3.0f);
vector3f vi(1,2,3);

// No need to specify the return type here, it'll be float.
float vf_length =
	fcppt::math::vector::length(
		vf);

// It's not so clear here. So we specify "float" explicitly
float vi_length =
	fcppt::math::vector::length<float>(
		vi);

// Hopefully, this prints "true"
std::cout << (std::abs(vf_length - vi_length) < 0.001f);
\endcode
*/
template<
	typename T,
	fcppt::math::size_type N,
	typename S
>
inline
typename
std::enable_if<
	std::is_floating_point<
		T
	>::value,
	T
>::type
length(
	fcppt::math::vector::object<
		T,
		N,
		S
	> const &_vec
)
{
	return
		std::sqrt(
			fcppt::math::vector::length_square(
				_vec
			)
		);
}

/**
\brief Calculates the length of a vector
\ingroup fcpptmathvector

This is the Euclidean distance (or 2 norm) of the vector. There are two
variants of this function. One works only for floating point vectors (so you
don't have to specify the return type). The other one works for arbitrary
vectors and uses a cast internally.

\note
If you want the length squared, use fcppt::math::vector::length_square

\see
fcppt::math::vector::length_square

Example:

\code
typedef
fcppt::math::vector::static_<float,3>
vector3f;

typedef
fcppt::math::vector::static_<int,3>
vector3i;

vector3f vf(1.0f,2.0f,3.0f);
vector3f vi(1,2,3);

// No need to specify the return type here, it'll be float.
float vf_length =
	fcppt::math::vector::length(
		vf);

// It's not so clear here. So we specify "float" explicitly
float vi_length =
	fcppt::math::vector::length<float>(
		vi);

// Hopefully, this prints "true"
std::cout << (std::abs(vf_length - vi_length) < 0.001f);
\endcode
*/
template<
	typename Dest,
	typename T,
	fcppt::math::size_type N,
	typename S
>
inline
typename
std::enable_if<
	!std::is_floating_point<
		T
	>::value,
	Dest
>::type
length(
	fcppt::math::vector::object<
		T,
		N,
		S
	> const &_vec
)
{
	return
		std::sqrt(
			fcppt::cast::int_to_float<
				Dest
			>(
				fcppt::math::vector::length_square(
					_vec
				)
			)
		);
}

}
}
}

#endif


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * \file
 * \brief Test of calc_kernel_mat function.
 */
#include "num_collect/interp/kernel/calc_kernel_mat.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"
#include "num_collect/interp/kernel/rbf_kernel.h"

TEST_CASE("num_collect::interp::kernel::calc_kernel_mat") {
    using num_collect::interp::kernel::calc_kernel_mat;
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::rbf_kernel;

    SECTION("calculate") {
        const auto kernel = rbf_kernel<euclidean_distance<Eigen::Vector3d>,
            gaussian_rbf<double>>();
        const auto list = std::vector<Eigen::Vector3d,
            Eigen::aligned_allocator<Eigen::Vector3d>>{
            Eigen::Vector3d(1.234, 2.345, 3.456),
            Eigen::Vector3d(1.357, 2.468, 3.579)};

        const Eigen::MatrixXd mat = calc_kernel_mat(kernel, list);

        REQUIRE(mat.rows() == 2);
        REQUIRE(mat.cols() == 2);

        const double non_diagonal_elem =
            std::exp(-(list[0] - list[1]).squaredNorm());
        REQUIRE_THAT(mat(0, 0), Catch::Matchers::WithinRel(1.0));
        REQUIRE_THAT(mat(0, 1), Catch::Matchers::WithinRel(non_diagonal_elem));
        REQUIRE_THAT(mat(1, 0), Catch::Matchers::WithinRel(non_diagonal_elem));
        REQUIRE_THAT(mat(1, 1), Catch::Matchers::WithinRel(1.0));
    }
}


#include "patMixtureNormal.h"
#include "patErrMiscError.h"
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include "patDisplay.h"
#include "patError.h"
#include <sstream>
#include <vector>
using boost::math::normal;

patMixtureNormal::patMixtureNormal(int& components,
	vector<double>& w,
	vector<double>& mu,
	vector<double>& sigma,
	patError *& err):
	m_components(components),m_w(w),m_mu(mu),m_sigma(sigma){

	bool flag = false;
	double total_w=0.0;

	if(m_mu.size()==m_components  && m_sigma.size()==m_components && m_w.size()==m_components){
		for(vector<double>::const_iterator iter = m_w.begin();iter!=m_w.end();++iter){
			total_w+=*iter;
		}

		if (total_w< 1.0000001 && total_w > 0.999999){
			flag= true;
		}
	}

	if(flag !=true){
		stringstream str;
		str << "Wrong distribution" <<total_w;
		err = new patErrMiscError(str.str());
		WARNING(err->describe());
	}
}

double patMixtureNormal::pdf(const double& value) const{

	double proba = 0.0;
	for (int i = 0; i < m_components;++i) {

	boost::math::normal s(m_mu[i],m_sigma[i]);
		proba += m_w[i]
				* boost::math::pdf(s,value ) ;
	}
	return proba;
}


/**
 *  @file    FEMesh.hpp
 *  @brief   The finite element mesh for a line.
 *  @author  Francois Roy
 *  @date    01/06/2020
 */
#ifndef FEMESH_H
#define FEMESH_H

#include <stdexcept>
#include <vector>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "utils/Utils.hpp"

namespace numerical {

namespace fem {

/**
* Holds data structures for a uniform mesh on a line in space, plus a
* uniform mesh in time.
*
* The finite element mesh has n elements and an associated polynomial of
* degree d.
*
* @param lengths A 2-lists of min and max coordinates in the spatial direction.
* @param t0 The initial time in time mesh.
* @param tend The final time in time mesh.
* @param nt Number of cells in time mesh.
* @param n Number of cells in the spatial direction.
* @param d The polynomial degree, default = 1.
*/
template <typename T>
class FEMesh {  // TODO inherit form common/Mesh
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
private:
	const std::vector<T> m_lengths;
	const T m_t0, m_tend;
    const int m_nt, m_nx, m_d;
	T m_dt, m_dx;
	Vec m_x;
	Vec m_t;
public:
	FEMesh(const std::vector<T>& lengths, T t0, T tend, int nx, int nt,
        int d=1)
	  : m_lengths(lengths),
	  m_t0(t0),
      m_tend(tend),
      m_nx(nx),
      m_nt(nt),
      m_d(d)
	{
      	if(m_lengths.size() != 2){
      		throw std::invalid_argument("length is not a 2-list.");
       	}
       	m_dx = (m_lengths[1] - m_lengths[0])/ T(m_nx);
        if(m_dx <= 0){
            throw std::invalid_argument("not a valid interval.");
        }
        m_dt = m_tend / T(m_nt);
        m_x = Vec::LinSpaced(m_nx + 1, m_lengths[0], m_lengths[1]);
        m_t = Vec::LinSpaced(m_nt + 1, m_t0, m_tend);
	}
    /**
    *  Local vertex to global vertex mapping (cells).
    */
    std::vector<std::vector<int>> cells(){
        std::vector<std::vector<int>> out;
        for(int i=0; i<m_nx; i++){
            out.push_back({i, i+1});
        }
        return out;
    }
    /**
    *  Local to global degree of freedom mapping (dof_map).
    */
    std::vector<std::vector<int>> dof_map(){
        std::vector<std::vector<int>> out;
        std::vector<int> m;
        if(m_d == 0){
            for(int i; i<m_nx; i++){
                out.push_back({i});
            }
        } else {
            for(int i; i<m_nx; i++){
                for(int j; j<m_d+1; j++){
                    m.push_back(i*m_d + j);
                }
                out.push_back(m);
            }
        }
        return out;
    }
    /**
    * @return the constant time increment.
    */
    T dt() {
        return m_dt;
    }
    /**
    * @return the space increment.
    */
    T dx() {
        return m_dx;
    }
    /**
    *  The indices of the nodes on the left boundary.
    *
    * @return The indices of the nodes on the left boundary.
    */
    std::vector<int> left(){
        return {0};
    }
    /**
    *  The indices of the nodes on the right boundary.
    *
    * @return The indices of the nodes on the right boundary,
    */
    std::vector<int> right(){
        std::vector<int> out = left();
        for(int& i : out){
            i += m_nx;
        }
    	return out;
    }
    /**
    * @return the time list.
    */
    Vec t() {
        return m_t;
    }
	/**
	* @return the list of coordinates.
	*/
	Vec vertices() {
		return m_x;
	}
};

}  // namespace fem

} // namespace numerical

#endif  // MESH_H


#include "../misc_math.h"
#include "../string_utils.h"
#include <boost/math/special_functions/bessel.hpp>
#include <algorithm>
#include <stdexcept>

using namespace std;
namespace coela {
namespace misc_math {

double linearly_interpolate(double x1, double y1, double x2, double y2, double x_between)
{
//    cerr<<"Inputs: "<<endl;
//    cerr<<"x1, y1: "<< x1 <<","<<y1<<endl;
//    cerr<<"x2, y2: "<< x2 <<","<<y2<<endl;
//    cerr<<"xinterp: "<< x_between<<endl;

    assert(std::min(x1,x2) - err_margin <= x_between
           && std::max(x1,x2) + err_margin >= x_between);

    //Flat line case,  pick arbitrary mid point.
    if (x1==x2) { return (y1+y2)/2.0; }

    double grad = (y2 - y1) / (x2 - x1);
    return y1 + grad*(x_between-x1);
}

double dimensionless_airy_function(double x, double ratio)
{
    //ratio = ratio of DIAMETERS ( or equiv, ratio of radii.) (not area)
    double j = 2.0 * (boost::math::cyl_bessel_j(1, x) -  ratio*boost::math::cyl_bessel_j(1,
                      ratio*x)) / (x*(1.0 - ratio*ratio)) ;     // 2[ J(x)/x - f*f*J(fx)/fx ] / (1-f*f)
    return j*j;
}

double airy_function(double angle, double wavelength, double aperture_diameter,
                     double obscuration_diameter)
{
    double f = obscuration_diameter / aperture_diameter;  //fractional obscuration
    double x = M_PI * aperture_diameter * (angle) /
               wavelength; //rescaled distance from centre
//            double x = M_PI * aperture_diameter * sin(angle) / wavelength; //better for large angles? But irrelevant here
    //avoid divide by 0 - return the value "in the limit" as x-->0:
    if (x==0) { x=1e-9; }
    //simple case: I = ( 2 J1[x] / x )^2
//             double j = 2.0 * ( boost::math::cyl_bessel_j(1, x) -  f*boost::math::cyl_bessel_j(1, f*x) ) / (x*(1.0 - f*f )  );  // 2( J(x) - fJ(fx) ) / x(1-f*f)
//            return j*j;
    return dimensionless_airy_function(x, f);
}

double airy_fwhm_in_rads(double wavelength, double aperture_diameter,
                         double obscuration_diameter)
{
    //Monotonic between peak and first minima- use bisection and linear interpolation NB first minima not at 1.22 if obscuration !=0, but will do for a first approximation
    double lower_limit(0.0), upper_limit(1.22*wavelength/aperture_diameter), eps(1e-5),
           delta(1.0);
    double hwhm_angle(upper_limit/2.0);
    while (fabs(delta)>eps) {
        delta = airy_function(hwhm_angle, wavelength, aperture_diameter,
                              obscuration_diameter) -0.5;
        if (delta >0) {
            //too close to origin
            lower_limit = hwhm_angle;
        } else { upper_limit = hwhm_angle; }
        hwhm_angle = (upper_limit + lower_limit) /2.0;
    }
    return hwhm_angle*2.0;
}

double gaussian_1d_function(double radius, double peak_value, double sigma)
{
    if (sigma!=0) { return peak_value*exp(-1.0*radius*radius/(2.0*sigma*sigma)); }
    else if (radius<1e-7) { return peak_value; }
    return 0;
}

double moffat_function(double radius, double sigma, double beta)
{
    assert(sigma!=0);
    double scale_length = radius/sigma;
    return pow((1+scale_length*scale_length), -1.0*beta);
}

double moffat_fwhm(double sigma, double beta)
{
    return 2.0*sigma* sqrt(pow(2.0, 1.0/beta) - 1);
}

double moffat_sigma(double fwhm, double beta)
{
    double radius_of_half_maximum=fwhm/2.0;
    return radius_of_half_maximum / sqrt((pow(2.0, 1.0/beta) - 1.0));
}

unsigned long integer_factorial(int n)
{
    assert(n<20);
    if (n==0) { return 1ul; }
    return n*integer_factorial(n-1);
}

///Sidesteps the integer limits, but as such is not exact. Used for calculating PDFs for an EMCCD (which are already approximations anyway).
double approx_factorial_double(double number)
{
    if (number <= 1.0) { return 1.0; }
    return number * approx_factorial_double(number - 1.0);
}

double integer_power(double x, int n)
{
    if (n==0) { return 1.0; }
    if (n<0) { return 1.0/integer_power(x,-1*n); }

    double result=x;
    while (n!=1) {
        result*=x;
        n--;
    }
    return result;
}

pair<double,double> fit_1d_parabola_to_find_local_maxima(double x0,
        double y_xm1, double y_x0, double y_xp1)
{
    //nb should have been fed a local maximum, so:
//            using string_utils::ftoa;
//            if (!(y_x0 > y_xm1 && y_x0 > y_xp1))
//                throw std::runtime_error("Fit parabola: Not a local max: "+ftoa(y_x0) +" less than "+ftoa(y_xm1)+"or"+ftoa(y_xp1));
//                ;
    assert(y_x0 >= y_xm1 && y_x0 >= y_xp1);
    pair<double,double> xmax_ymax;
//See Lisa Poyneer 2003, "scene-based SHWFS" for a nicely formatted version... but it's just very simple math.
    double b = -0.5*(y_xm1 - y_xp1);
    double two_a =(y_xm1 + y_xp1 -2*y_x0);
    //c = y_x0

//            xmax_ymax.first= x0 + ( 0.5*(y_xm1 - y_xp1)  )/(y_xm1 + y_xp1 -2*y_x0);
    xmax_ymax.first= x0 - b/two_a;
    xmax_ymax.second=y_x0 - b*b/(2*two_a);

    return xmax_ymax;
}

double bilinear_interpolate(double x, double y,
                            double x0, double y0,
                            double x1, double y1,
                            double f_x0_y0, double f_x1_y0, double f_x0_y1, double f_x1_y1)
{
    double x_frac(x-x0 / (x1-x0)), y_frac(y-y0 / (y1-y0));

    return f_x0_y0 *(1 - x_frac)*(1 - y_frac)
           + f_x1_y0 *(x_frac)*(1 - y_frac)
           + f_x0_y1 *(1-x_frac)*(y_frac)
           + f_x1_y1 *(x_frac)*(y_frac)    ;
}

}
}


//
// Created by jennings on 2018/10/26.
//

#ifndef CSCI5570_LOGITSTIC_REGRESSION_HPP
#define CSCI5570_LOGITSTIC_REGRESSION_HPP

#include "base/magic.hpp"
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include "lib/svm_sample.hpp"

using namespace Eigen;
using namespace csci5570;
using DataStore = std::vector<lib::SVMSample>;

template <typename T>
class LogisticRegression {
public:
  LogisticRegression(DataStore* data_store, float learning_rate=0.001)
    : data_store_(data_store), learning_rate_(learning_rate) {
    // initialize theta
    for(auto& row : (*data_store_)) {
      for(auto& col : row.x_) {
        theta_[col.first] = 0.0;
        grad_[col.first] = 0.0;
      }
    }
  }

  double predict(lib::SVMSample sample) {
    double z = 0;
    for(auto& col : sample.x_) {
      Key key = col.first;
      auto x = col.second;
      z += x * theta_[key];
    }
    return 1.0 / (1 + std::exp(-z));
  }

  double get_loss() {
    double loss = 0;
    for (auto& row : (*data_store_)) {
      int y = row.y_ <= 0 ? 0 : 1;
      double pred = predict(row);
      loss += (-1 * y * std::log(pred) - (1 - y) * std::log(1 - pred));
    }
    return loss/data_store_->size();
  }

  void compute_gradient(std::vector<T>& grad) {
    for(auto& g : grad_) {
      g.second = 0.0;
    }
    for (auto& row : (*data_store_)) {
      // NOTICE that row.y_ maybe +1/-1
      auto y = row.y_ <= 0 ? 0 : 1;
      auto z = 0;
      for(auto& col : row.x_) {
        Key key = col.first;
        auto x = col.second;
        z += x * theta_[key];
      }
      auto g = 1 / (1 + std::exp(-z));
      for(auto& col : row.x_) {
        Key key = col.first;
        auto x = col.second;
        grad_[key] += -learning_rate_ * x * (g - y);
      }
    }
    for(auto& g : grad_) {
      grad.push_back(g.second / data_store_->size());
    }
//    Matrix<T, Dynamic, 1> Z = (*X_) * theta_;
//    Matrix<T, Dynamic, 1> G = Z.unaryExpr([](T z){
//      return 1 / (1 + std::exp(-z));
//    });
//    grad = -learning_rate_ * X_->transpose() * (G - (*Y_));
  }

  void update_theta(const std::vector<Key>& keys, const std::vector<T>& vals) {
    uint32_t size = keys.size();
    for(uint32_t i = 0; i < size; i++) {
      if (theta_.find(keys[i]) == theta_.end()) {continue;}
      theta_[keys[i]] = vals[i];
    }
  }

  float test_acc() {
    float correct = 0;
    uint32_t total = 0;
    for (auto& row : (*data_store_)) {
      // NOTICE that row.y_ maybe +1/-1
      auto y = row.y_ <= 0 ? 0 : 1;
      auto z = 0;
      for(auto& col : row.x_) {
        Key key = col.first;
        auto x = col.second;
        z += x * theta_[key];
      }
      auto g = 1 / (1 + std::exp(-z));
      auto y_pred = g <= 0.5 ? 0 : 1;
      if (y_pred == y) {correct++;}
      total++;
    }
    if (total == 0) {
      return -1;
    }
    return correct / total;
  }

  void get_keys(std::vector<Key>& keys) {
    for(auto& kv : theta_) {
      keys.push_back(kv.first);
    }
  }

private:
  DataStore* data_store_;
  std::map<Key, T> theta_;
  std::map<Key, T> grad_;
  float learning_rate_;
};

#endif //CSCI5570_LOGITSTIC_REGRESSION_HPP


#include "EpipolarConsistency.h"

#include <Eigen/Dense>

namespace EpipolarConsistency
{
	using Geometry::Pi;

	/// 2DO Should use SVD from Geometry module to reduce compile time.
	Geometry::RP3Point estimateIsoCenter(const std::vector<ProjectionMatrix> &Ps)
	{
		int n=(int)Ps.size();
		Eigen::Matrix3d A=Eigen::Vector3d(n,n,n).asDiagonal();
		Eigen::Vector3d b(0,0,0);
		// Distance as norm of a point projected to plane orthogonal to view direction through the origin.
		for (int i=0;i<n;i++)
		{
			const ProjectionMatrix &P(Ps[i]);
			// Center of projection
			Eigen::Vector3d C=Geometry::getCameraCenter(P).block<3,1>(0,0);
			// View direction
			Eigen::Vector3d V=P.block<1,3>(2,0).normalized();
			// O=id-v*v^T is projection in direction v.
			A-=V*V.transpose();
			// For points x on the principal ray it holds that O*x=O*C
			b+=C-V*(V.transpose()*C);
		}
		// Solve by SVD
		Eigen::JacobiSVD<Eigen::Matrix3d> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Geometry::RP3Point O(0,0,0,1);
		O.block<3,1>(0,0)=svd.solve(b);
		return O;
	}

	double estimateObjectRadius(const ProjectionMatrix& P, int n_u, int n_v)
	{
		using namespace Geometry;
		using namespace std;
		// Get the focal lengths in pixels.
		auto   alphas=getCameraFocalLengthPx(P);
		// Compute field of view (use maximum of u and v).
		double fov   =max(abs(atan(0.5*n_u/alphas.first)), abs(atan(0.5*n_v/alphas.second)));
		// The source to iso-center distance.
		double sid   =getCameraCenter(P).head(3).norm();
		// Return approximate radius of an circumscribed  sphere.
		return sin(fov)*sid;
	}

	std::pair<double,double> estimateAngularRange(const RP3Line& B, double object_radius_mm)
	{
		// Distance of baseline to origin
		double baseline_dist=Geometry::pluecker_distance_to_origin(B);
		// If the baseline intersects the object, ECC is not well-defined. We return a half circle anyway.
		if (baseline_dist<=object_radius_mm)
			return std::make_pair(-0.5*Pi,0.5*Pi);
		// Else, find angle of plane which touches the sphere
		double kappa_max=std::abs(std::asin(object_radius_mm/baseline_dist));
		return std::make_pair(-kappa_max,kappa_max);
	}

	double estimateAngularStep(const ProjectionMatrix& P0, const ProjectionMatrix& P1, int n_u, int n_v)
	{
		using namespace Geometry;
		double radius_mm=std::max(estimateObjectRadius(P0,n_u,n_v),estimateObjectRadius(P1,n_u,n_v));
		auto baseline=join_pluecker(getCameraCenter(P0),getCameraCenter(P1));
		auto range_kappa=estimateAngularRange(baseline,radius_mm);
		return 2.0*(range_kappa.second-range_kappa.first)/std::sqrt(n_u*n_u+n_v*n_v);
	}

	Metric& Metric::setObjectRadius(double radius_mm)
	{
		object_radius_mm=radius_mm;
		return *this;
	}

	double Metric::getObjectRadius() const
	{
		if (object_radius_mm>0)
			return object_radius_mm;
		if (getProjectionMatrices().empty())
			return 0;
		auto P=getProjectionMatrices().front();
		return estimateObjectRadius(P,n_u,n_v);
	}

	Metric& Metric::setEpipolarPlaneStep(double dkappa_rad) {
		dkappa=dkappa_rad;
		return *this;
	}

	Metric& Metric::setProjectionMatrices(const std::vector<ProjectionMatrix>& _Ps)
	{
		Ps=_Ps;
		return *this;
	}

	const std::vector<ProjectionMatrix>&Metric:: getProjectionMatrices() const
	{
		return Ps;
	}

	Metric::Metric() : object_radius_mm(0), dkappa(0), n_u(0), n_v(0) {}

	Metric::~Metric() {}

} // namespace EpipolarConsistency


/*--
  Open3DMotion
  Copyright (c) 2004-2012.
  All rights reserved.
  See LICENSE.txt for more information.
--*/

#include <math.h>
#include <stdlib.h>
#include "Open3DMotion/Maths/LinearSolve3.h"

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
extern "C"
{
#include <f2clibs/f2c.h>
#include <clapack.h>
}
#else
#include <Eigen/Dense>
#endif

namespace Open3DMotion
{

	// Linear alegebra helper function: calls LAPACK routine
	// Solves linear least squares in 3D
	bool LinearSolve3(float* x, const float* A, const float* b, int numrows, float* rms /*=NULL*/, float rcond /*=0.001*/)
	{
#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN

		int i, j;

		// transpose as required for lapack
		float* AT = new float[3*numrows];
		for (i = 0; i < 3; i++)
			for (j = 0; j < numrows; j++)
				AT[i*numrows+j] = A[3*j + i];

		// copy input vector
		float* y = new float[numrows];
		for (i = 0; i < numrows; i++)
			y[i] = b[i];

		// params required for lapack
		long mrows = numrows;
		long ncols = 3;
		long nrhs = 1;
		long lda = numrows;
		long ldb = numrows;
		long jpvt[3] = { 0, 0, 0 };
		long rank = 0;
		long info = 0;

		// call lapack function to query work size
		long lwork_query = -1;
		float opt_work_size = 1.0f;
		sgelsy_(&mrows, &ncols, &nrhs, AT, &lda, y, &ldb, jpvt, &rcond, &rank, &opt_work_size, &lwork_query, &info);

		// return if any probs with params
		if (info != 0)
			return false;

		// get opt work size and call for real
		long lwork_opt = (long)opt_work_size;
		float* work = new float[lwork_opt];
		sgelsy_(&mrows, &ncols, &nrhs, AT, &lda, y, &ldb, jpvt, &rcond, &rank, work, &lwork_opt, &info);

		// copy result
		for (i = 0; i < 3; i++)
			x[i] = y[i];

		// done with temp data
		delete [] work;
		delete [] AT;
		delete [] y;

		// must have params ok (info = 0) and full rank (3)
		if (info != 0 || rank != 3)
			return false;

		// if requested, compute RMS
		if (rms)
		{
			float sumsq(0.0f);
			for (i = 0; i < numrows; i++)
			{
				float ax(0.0f);
				for (j = 0; j < 3; j++)
					ax += A[3*i+j] * x[j];
				float diff = ax - b[i];
				sumsq += diff*diff;
			}
			*rms = sqrt(sumsq / numrows);
		}

		return true;

#else

		// Map to Eigen objects
		Eigen::Map< const Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> > C(A, numrows, 3);
    Eigen::Map< const Eigen::Matrix<float, Eigen::Dynamic, 1> > d(b, numrows, 1);
    Eigen::Map< Eigen::Matrix<float, 3, 1> > y(x, 3, 1);

		// compute CTC ( = ATA )
		Eigen::MatrixXf CT = C.transpose();
		Eigen::Matrix3f CTC = CT * C;

		// perform check on reciprocal condition number	to mimic LAPACK behaviour
		Eigen::Vector3f e = Eigen::EigenSolver<Eigen::Matrix3f> (CTC).eigenvalues().real();
		float CTC_rcond = e.minCoeff() / e.maxCoeff();
		if (CTC_rcond < rcond)
			return false;

		// compute CTd = ATb
		Eigen::Vector3f CTd = CT * d;

		// solve
		y = CTC.ldlt().solve(CTd);

		// compute rms error if requested
		if (rms)
			*rms = sqrt((C*y - d).squaredNorm() / numrows);

		return true;

#endif
	}

}


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

#ifndef IGNITION_MATH_EIGEN3_UTIL_HH_
#define IGNITION_MATH_EIGEN3_UTIL_HH_

#include <vector>

#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

#include <ignition/math/AxisAlignedBox.hh>
#include <ignition/math/Matrix3.hh>
#include <ignition/math/OrientedBox.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/eigen3/Conversions.hh>

namespace ignition
{
  namespace math
  {
    namespace eigen3
    {
      /// \brief Get covariance matrix from a set of 3d vertices
      /// https://github.com/isl-org/Open3D/blob/76c2baf9debd460900f056a9b51e9a80de9c0e64/cpp/open3d/utility/Eigen.cpp#L305
      /// \param[in] _vertices a vector of 3d vertices
      /// \return Covariance matrix
      inline Eigen::Matrix3d covarianceMatrix(
        const std::vector<math::Vector3d> &_vertices)
      {
        if (_vertices.empty())
          return Eigen::Matrix3d::Identity();

        Eigen::Matrix<double, 9, 1> cumulants;
        cumulants.setZero();
        for (const auto &vertex : _vertices)
        {
          const Eigen::Vector3d &point = math::eigen3::convert(vertex);
          cumulants(0) += point(0);
          cumulants(1) += point(1);
          cumulants(2) += point(2);
          cumulants(3) += point(0) * point(0);
          cumulants(4) += point(0) * point(1);
          cumulants(5) += point(0) * point(2);
          cumulants(6) += point(1) * point(1);
          cumulants(7) += point(1) * point(2);
          cumulants(8) += point(2) * point(2);
        }

        Eigen::Matrix3d covariance;

        cumulants /= static_cast<double>(_vertices.size());

        covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
        covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
        covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
        covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
        covariance(1, 0) = covariance(0, 1);
        covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
        covariance(2, 0) = covariance(0, 2);
        covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
        covariance(2, 1) = covariance(1, 2);
        return covariance;
      }

      /// \brief Get the oriented 3d bounding box of a set of 3d
      /// vertices using PCA
      /// http://codextechnicanum.blogspot.com/2015/04/find-minimum-oriented-bounding-box-of.html
      /// \param[in] _vertices a vector of 3d vertices
      /// \return Oriented 3D box
      inline ignition::math::OrientedBoxd verticesToOrientedBox(
        const std::vector<math::Vector3d> &_vertices)
      {
        math::OrientedBoxd box;

        // Return an empty box if there are no vertices
        if (_vertices.empty())
          return box;

        math::Vector3d mean;
        for (const auto &point : _vertices)
          mean += point;
        mean /= static_cast<double>(_vertices.size());

        Eigen::Vector3d centroid = math::eigen3::convert(mean);
        Eigen::Matrix3d covariance = covarianceMatrix(_vertices);

        // Eigen Vectors
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
          eigenSolver(covariance, Eigen::ComputeEigenvectors);
        Eigen::Matrix3d eigenVectorsPCA = eigenSolver.eigenvectors();

        // This line is necessary for proper orientation in some cases.
        // The numbers come out the same without it, but the signs are
        // different and the box doesn't get correctly oriented in some cases.
        eigenVectorsPCA.col(2) =
          eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));

        // Transform the original cloud to the origin where the principal
        // components correspond to the axes.
        Eigen::Matrix4d projectionTransform(Eigen::Matrix4d::Identity());
        projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
        projectionTransform.block<3, 1>(0, 3) =
          -1.0f * (projectionTransform.block<3, 3>(0, 0) * centroid);

        Eigen::Vector3d minPoint(INF_I32, INF_I32, INF_I32);
        Eigen::Vector3d maxPoint(-INF_I32, -INF_I32, -INF_I32);

        // Get the minimum and maximum points of the transformed cloud.
        for (const auto &point : _vertices)
        {
          Eigen::Vector4d pt(0, 0, 0, 1);
          pt.head<3>() = math::eigen3::convert(point);
          Eigen::Vector4d tfPoint = projectionTransform * pt;
          minPoint = minPoint.cwiseMin(tfPoint.head<3>());
          maxPoint = maxPoint.cwiseMax(tfPoint.head<3>());
        }

        const Eigen::Vector3d meanDiagonal = 0.5f * (maxPoint + minPoint);

        // quaternion is calculated using the eigenvectors (which determines
        // how the final box gets rotated), and the transform to put the box
        // in correct location is calculated
        const Eigen::Quaterniond bboxQuaternion(eigenVectorsPCA);
        const Eigen::Vector3d bboxTransform =
          eigenVectorsPCA * meanDiagonal + centroid;

        math::Vector3d size(
            maxPoint.x() - minPoint.x(),
            maxPoint.y() - minPoint.y(),
            maxPoint.z() - minPoint.z()
        );
        math::Pose3d pose;
        pose.Rot() = math::eigen3::convert(bboxQuaternion);
        pose.Pos() = math::eigen3::convert(bboxTransform);

        box.Size(size);
        box.Pose(pose);
        return box;
      }
    }
  }
}

#endif


#pragma once

#include <math.h>
#include <vector>
#include <iostream>
#include <Eigen/Dense>
#include <Mahi/Util/Timing/Clock.hpp>

struct ardsqexpKernel{
    ardsqexpKernel():
    length_scales(1,1),
    length_scales_2(1,1),
    train(1,std::vector<double>(1,0)),
    sigma_f(1.0),
    sigma_f_2(1.0),
    K(train.size(),0)
    {
    }

    ardsqexpKernel(std::vector<std::vector<double>> train, std::vector<double> theta):
    train(train),
    sigma_f(theta[theta.size()-1]),
    sigma_f_2(sigma_f*sigma_f),
    K(train.size(),0)
    {
        length_scales = std::vector<double>(theta.begin(),theta.end()-1);
        length_scales_2 = std::vector<double>(length_scales.size(),0);
        for (size_t i = 0; i < length_scales.size(); i++){
            length_scales_2[i] = length_scales[i]*length_scales[i];
        }
    }

    std::vector<double> operator()(const std::vector<double>& New){
        for (auto i = 0; i < train.size(); i++){
            double ard_sum = 0;
            for(auto j = 0; j < train[0].size(); j++) {
                double diff = train[i][j]-New[j];
                ard_sum += diff*diff/(length_scales_2[j]);
            }
            K[i] = sigma_f_2*exp(-0.5*ard_sum);
        }
        return K;
    }

    std::vector<std::vector<double>> train;
    std::vector<double> length_scales;
    std::vector<double> length_scales_2;
    double sigma_f;
    double sigma_f_2;
    std::vector<double> K;
};


//
//  kc_rsa.cpp
//  kcalg
//
//  Created by knightc on 2019/7/22.
//  Copyright © 2019 knightc. All rights reserved.
//

#include "opentsb/kc_sec.h"
#include "opentsb/kc_common.h"
#include "rsaUtil.hpp"

#include "opentsb/test.h"

#include <cstdio>
#include <vector>

#include <NTL/vec_ZZ.h>

int rsa_getKey(ZZ pubKey[],ZZ privKey[]) {

    ZZ p,q,n ,f_n;
    ZZ pubkey_e,privKey_d;

   pubkey_e= 65537;

       RandomPrime(p, 1024);
       RandomPrime(q, 1024);
    //    NextPrime(q, p );
//    p= 37;
//    q= 23;
//    if (GCD(p , q )==1) {
//        cout << "p and q is prime " << endl;
//    }

    n= p * q;
    oula(p, q, f_n);

    if (GCD(pubkey_e, f_n )== 1) {
        cout << " pubkey_e is true \n" << endl;
    }

    privKey_d = InvMod(pubkey_e, f_n);

    pubKey[0]=pubkey_e;
    pubKey[1]=n;

    privKey[0] = privKey_d;
    privKey[1] =p;
    privKey[2] = q ;

    cout <<"公钥 e = "<< pubkey_e <<"\n"
    << "共模 n = " << n << "\n"
    << "欧拉 n = " << f_n << "\n"
    <<"私钥 d = " <<privKey_d<< endl;

    return 1;
}

ZZ rsa_enc(const ZZ pubKey[],unsigned char *plaintext ,long plain_len) {

//    unsigned char *cyperTxt={0};
    ZZ e,n,plaintx_z,cypertx_z;

    e= pubKey[0];
    n= pubKey[1];

    plaintx_z= ZZFromBytes(plaintext, plain_len);
   cout  <<"明文 M = " <<plaintx_z<< endl;
//    cout <<"公钥 e = "<< e <<"\n"
//        << "共模 n = " << n << "\n"
//        <<"明文 M = " <<plaintx_z<< endl;

    cypertx_z = PowerMod(plaintx_z, e, n);
    cout << "密文 C = " << cypertx_z << endl;

//    BytesFromZZ(cyperTxt, cypertx_z, 256);
//
//    return cyperTxt;
    return cypertx_z;

}

int rsa_decy(ZZ cyperTxt_z,ZZ privKey[] , unsigned char *plaintext) {

    ZZ d,n,plaintx_z;
//    long ctx_len;

 //   ctx_len= getArrayLen(cyperTxt);

    d= privKey[0];
    n= privKey[1] * privKey[2];
//    cout <<"私钥 d = "<< d <<"\n"
//    << "共模 n = " << n << "\n"
//      <<"密文 C = " <<cyperTxt_z<< endl;
  //  ZZFromBytes(cyperTxt_z, cyperTxt, ctx_len);

   plaintx_z= PowerMod(cyperTxt_z, d, n);
    cout << "解密 M = " << plaintx_z << endl;

//    BytesFromZZ(plaintext, plaintx_z,1);
//    cout << "明文 M = " << plaintext << endl;

    return 0;
}

int test_rsa() {

//    unsigned   char std_Message[19]={
//        0x65,0x6E,0x63,0x72,0x79,0x70,0x74,0x69,0x6F,0x6E,0x20,0x73,0x74,0x61,0x6E,
//        0x64,0x61,0x72,0x64};
    unsigned char std_Message[1]={0x65};

    unsigned char *plaintx={0};
   // unsigned char *cypertx= {0};
    ZZ cypertx;

    long plain_len;
    ZZ pubKey[2],privKey[3];
    ZZ e,d,n;

    plain_len= getArrayLen(std_Message);
    rsa_getKey(pubKey, privKey);

    cypertx= rsa_enc(pubKey,std_Message, plain_len);

    rsa_decy(cypertx, privKey, plaintx);

    return 0;
}