#include "PolyMesh/IOManager.h"
#include <Eigen/Sparse>
#define pi 3.1415926

using namespace acamcad;
using namespace polymesh;
using namespace Eigen;

void BarycentricCoordinates(PolyMesh* mesh)
{
	int v_n = mesh->numVertices();
	std::vector<int> inner_id(v_n);
	int iter;
	int boundary_num, inner_num;

	std::vector<double> u(mesh->numVertices());
	std::vector<double> v(mesh->numVertices());

	iter = 0;
	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			inner_id[(*v_it)->index()] = iter;
			iter++;
		}
	}
	inner_num = iter;
	boundary_num = v_n - iter;

	MHalfedge* first_heh;
	for (HalfEdgeIter he_it = mesh->halfedge_begin(); he_it != mesh->halfedge_end(); ++he_it)
	{
		if (mesh->isBoundary(*he_it))
		{
			first_heh = *he_it;
			break;
		}
	}
	MHalfedge* iter_heh = first_heh->next();
	iter = 0;
	while (iter_heh != first_heh)
	{
		MVert* from_v = iter_heh->fromVertex();
		u[from_v->index()] = cos(double(2 * pi *iter / boundary_num));
		v[from_v->index()] = sin(double(2 * pi *iter / boundary_num));
		iter_heh = iter_heh->next();
		iter++;
	}
	u[first_heh->fromVertex()->index()] = cos(double(2 * pi *iter / boundary_num));
	v[first_heh->fromVertex()->index()] = sin(double(2 * pi *iter / boundary_num));

	SparseMatrix<double> weight(v_n, v_n);
	std::vector<Triplet<double>> triplet;

	for (FaceIter f_it = mesh->polyfaces_begin(); f_it != mesh->polyfaces_end(); ++f_it)
	{
		MHalfedge* heh = (*f_it)->halfEdge();
		MVert* v0 = heh->fromVertex();
		MVert* v1 = heh->toVertex();
		MHalfedge* next_heh = heh->next();
		MVert* v2 = next_heh->toVertex();

		double l2 = (v0->position() - v1->position()).norm();
		double l1 = (v0->position() - v2->position()).norm();
		double l0 = (v1->position() - v2->position()).norm();

		double angle = acos(dot(v2->position() - v0->position(), v1->position() - v0->position())/(l1*l2));
		triplet.push_back(Triplet<double>(v0->index(), v1->index(), tan(angle*0.5) / l2));
		triplet.push_back(Triplet<double>(v0->index(), v2->index(), tan(angle*0.5) / l1));

		angle = acos(dot(v0->position() - v1->position(), v2->position() - v1->position())/(l2*l0));
		triplet.push_back(Triplet<double>(v1->index(), v0->index(), tan(angle*0.5) / l2));
		triplet.push_back(Triplet<double>(v1->index(), v2->index(), tan(angle*0.5) / l0));

		angle = acos(dot(v0->position() - v2->position(), v1->position() - v2->position())/(l0*l1));
		triplet.push_back(Triplet<double>(v2->index(), v0->index(), tan(angle*0.5) / l1));
		triplet.push_back(Triplet<double>(v2->index(), v1->index(), tan(angle*0.5) / l0));
	}
	weight.setFromTriplets(triplet.begin(), triplet.end());

	SparseMatrix<double> inner(inner_num, inner_num);
	VectorXd ub(inner_num), vb(inner_num);

	triplet.clear();
	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			double tempu = 0;
			double tempv = 0;
			for (VertexVertexIter vv_it = mesh->vv_iter(*v_it); vv_it.isValid(); ++vv_it)
			{
				if (mesh->isBoundary(*vv_it))
				{
					tempu += u[(*vv_it)->index()] * weight.coeff((*v_it)->index(), (*vv_it)->index());
					tempv += v[(*vv_it)->index()] * weight.coeff((*v_it)->index(), (*vv_it)->index());
				}
				else
				{
					triplet.push_back(Triplet<double>(inner_id[(*v_it)->index()], inner_id[(*vv_it)->index()], -weight.coeff((*v_it)->index(), (*vv_it)->index())));
				}
				triplet.push_back(Triplet<double>(inner_id[(*v_it)->index()], inner_id[(*v_it)->index()], weight.coeff((*v_it)->index(), (*vv_it)->index())));
			}
			ub[inner_id[(*v_it)->index()]] = tempu;
			vb[inner_id[(*v_it)->index()]] = tempv;
		}
	}
	inner.setFromTriplets(triplet.begin(), triplet.end());
	SparseLU<SparseMatrix<double>> solver;
	solver.analyzePattern(inner);
	solver.factorize(inner);
	VectorXd result_u = solver.solve(ub);
	VectorXd result_v = solver.solve(vb);

	for (VertexIter v_it = mesh->vertices_begin(); v_it != mesh->vertices_end(); ++v_it)
	{
		if (!mesh->isBoundary(*v_it))
		{
			(*v_it)->setPosition(result_u[inner_id[(*v_it)->index()]], result_v[inner_id[(*v_it)->index()]], 0);
		}
		else
		{
			(*v_it)->setPosition(u[(*v_it)->index()], v[(*v_it)->index()], 0);
		}
	}
}

int main(int argc, const char **argv)
{
	if (argc != 2)
	{
		std::cout << "========== Hw7 Usage  ==========\n";
		std::cout << std::endl;
		std::cout << "blablabla.exe	test.obj\n";
		std::cout << std::endl;
		std::cout << "=================================================\n";
        return 0;
	}
	std::string mesh_path = argv[1];
	PolyMesh* mesh = new PolyMesh();
	loadMesh(mesh_path, mesh);
	BarycentricCoordinates(mesh);
	writeMesh("result_" + mesh_path, mesh);
    return 0;
}


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


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


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


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


#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

// #include "g2o/core/auto_differentiation.h"
// #include "g2o/core/base_unary_edge.h"
// #include "g2o/core/base_vertex.h"
// #include "g2o/core/optimization_algorithm_factory.h"
// #include "g2o/core/sparse_optimizer.h"
// #include "g2o/stuff/command_args.h"
// #include "g2o/stuff/sampler.h"
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/auto_differentiation.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include "matplotlibcpp.h"

G2O_USE_OPTIMIZATION_LIBRARY(dense);

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

template <class T, class TT>
T g(TT x, T a, T b, T c)
{
  return ceres::exp(a * x * x + b * x + c);
}

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> {
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  virtual void setToOriginImpl() override {
    _estimate << 0.0, 0.0, 0.0;
  }
  virtual void oplusImpl(const double* update) override {
    _estimate += Eigen::Vector3d(update);
  }

  virtual bool read(std::istream&) {}
  virtual bool write(std::ostream&) const {}
};

class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    CurveFittingEdge(double x): _x(x) {}

    // Without auto-diff
    virtual void computeError() override {
      const CurveFittingVertex* v = static_cast<CurveFittingVertex*>(_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      _error(0, 0) = _measurement - g(_x, abc[0], abc[1], abc[2]);
    }

    // Analytical jacobian
    virtual void linearizeOplus() override {
      const CurveFittingVertex *v = static_cast<CurveFittingVertex*>(_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      double y = g(_x, abc[0], abc[1], abc[2]);
      _jacobianOplusXi[0] = -_x * _x * y;
      _jacobianOplusXi[1] = -_x * y;
      _jacobianOplusXi[2] = -y;
    }

    // with auto-diff
    // template <class T>
    // bool operator() (const T* abc, T *error) const {
    //   error[0] = measurement() - g(_x, abc[0], abc[1], abc[2]);
    //   return true;
    // }

    virtual bool read(istream&) {}
    virtual bool write(ostream&) const {}

  private:
    double _x;

  // G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff
};

int main(int argc, char **argv) {
  std::cout << "Gauss-Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = 5.5;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

  // auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  //   g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
  // );

  g2o::SparseOptimizer optimizer;
  // optimizer.setAlgorithm(solver);
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(g2o::OptimizationAlgorithmFactory::instance()->construct("lm_dense", solverProperty));
  optimizer.setVerbose(true);

  // Create vertex
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(a, b, c));
  v->setId(0);
  optimizer.addVertex(v);

  // Add edges
  for (int i = 0; i < N; ++i)
  {
    CurveFittingEdge * edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);
    edge->setMeasurement(y_data[i]);
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() / (obs_sigma * obs_sigma));
    optimizer.addEdge(edge);
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  optimizer.initializeOptimization();
  optimizer.optimize(10);

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  a = v->estimate()[0];
  b = v->estimate()[1];
  c = v->estimate()[2];

  std::cout << "final = " << a << " " << b << " " << c << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], a, b, c);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}