/*
 File provvisorio per le definizioni di funzioni matematiche utili
 */

#ifndef scontornamento_hpp
#define scontornamento_hpp

#include <vector>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <map>
#include <exception>
// #define ARMA_DONT_USE_WRAPPER
#include <armadillo>
#include "myMathFunc.hpp"

namespace fm {
    typedef std::map<double,double> setOfPointsType_;
    typedef setOfPointsType_::value_type pointType_;
    /*
     imageNormalize(src,dst,[Argomenti opzionali])

     Funzione che normalizza l'immagine passata in input.

     Argomenti
     - src immagine di partenza (eventualmente convertita in CV_8U)
     - dst immagine finale
     - brightness parametro al cui crescere aumenta la luminosità finale dell'immagine; serve a calcolare il valore che verrà normalizzato a 0.5
     - leftCut porzione della coda sinistra da tenere
     - rightCut porzione della coda destra da tenere
     - histSmooth ampiezza smussamento gaussiano dell'istogramma
     - reparSmooth ampiezza smussamento gaussiano dell'istogramma dopo riparametrizzazione
     - minMaxFilter grandezza del filtro per ottenere l'immagine dei minimi e dei massimi
     - eqSmooth ampiezza smussamento gaussiano dopo equalizzazione
     */
    void imageNormalize(const Image__& src,
                        Image__& dst,
                        const double& brightness,
                        const double& leftCut,
                        const double& rightCut,
                        const int& histSmooth,
                        const int& reparSmooth,
                        const int& minMaxFilter,
                        const double& mincp1,
                        const double& mincp2,
                        const double& maxcp1,
                        const double& maxcp2);

    /*
     imageBimodalize(src,dst,[Argomenti opzionali])

     Funzione che regolarizza il contrasto dell'immagine.
     src e dst possono essere la stessa funzione.

     Argomenti
     - src immagine di partenza (eventualmente convertita in CV_8U)
     - dst immagine finale
     - brightness parametro al cui crescere aumenta la luminosità finale dell'immagine; serve a calcolare il valore che verrà normalizzato a 0.5
     - leftCut porzione della coda sinistra da tenere
     - rightCut porzione della coda destra da tenere
     - histSmooth ampiezza smussamento gaussiano dell'istogramma
     - reparSmooth ampiezza smussamento gaussiano dell'istogramma dopo riparametrizzazione
     */
    void imageBimodalize(const Image__& src,
                         Image__& dst,
                        const double& brightness,
                        const double& leftCut,
                        const double& rightCut,
                        const int& histSmooth,
                        const int& reparSmooth);

    /*
     ImageEqualize(src, dst, minMaxFilter, mincp1, mincp2, maxcp1, maxcp2)

     Funzione che equalizza l'immagine. src e dst possono essere la stessa matrice.

     Argomenti
     - src immagine di input
     - dst immagine di output
     - minMaxFilter ampiezza dei filtri circolari di minimo e di massimo
     - mincp1, mincp2, maxcp1, maxcp2 ascissa dei punti di controllo
            per il riscalamento dei minimi e dei massimi
     */
    void ImageEqualize(const Image__& src,
                       Image__& dst,
                       const int& minMaxFilter,
                       const double& mincp1,
                       const double& mincp2,
                       const double& maxcp1,
                       const double& maxcp2,
                       cv::InputArray& mask = cv::noArray());
}

#endif /* scontornamento_hpp */


#include "ros/ros.h"
#include <cv_bridge/cv_bridge.h>
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/core/core.hpp>
#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>     //make sure to include the relevant headerfiles
#include <opencv2/highgui/highgui.hpp>
#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <iostream>
#include <boost/algorithm/string.hpp>
#include <string>
#include <sstream>
#include <stdlib.h>
#include <fstream>
#include <vector>
#include <cmath>

using namespace cv;
using namespace cv_bridge;
using namespace std;

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */

string directory;
int frameCount;

void calibrate(int numImages, int numCornersHorizontal, int numCornersVertical, const char* intrinsic_name, const char* distortion_name);
void getImages(const sensor_msgs::Image::ConstPtr& msg);

void calibrate(int numImages, int numCornersHorizontal, int numCornersVertical, const char* intrinsic_name, const char* distortion_name){
	ofstream intrinsicFile;
	intrinsicFile.open(intrinsic_name);
	ofstream distortionParams;
	distortionParams.open(distortion_name);
	namedWindow("Process", WINDOW_NORMAL);

// Task 2
	int numSquares = numCornersHorizontal * numCornersVertical;
	Size boardSize = Size(numCornersHorizontal, numCornersVertical);

	vector<vector<Point3f> > object_points;
	vector<vector<Point2f> > image_points;
	Mat image;

	vector<Point3f> obj;
	for(int j=0;j<numSquares;j++){
		obj.push_back(Point3f( j/numCornersHorizontal , j%numCornersHorizontal , 0.0f));
	}

	for (int i = 0; i < numImages; i++){
		vector<Point2f> pointBuf;
		ostringstream name;
		name << directory << "/img" << i << ".jpg";
		string filename = name.str();
		image = imread(filename, CV_LOAD_IMAGE_GRAYSCALE);

	// Find chessboard inner corners, 10 corners per row/7 per column
		bool found = findChessboardCorners(image, boardSize, pointBuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);

		if(found){
		// Find subpixels with the criteria
			TermCriteria criteria = TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.01 );
			cornerSubPix(image, pointBuf, Size(10,10), Size(-1, -1), criteria);
			drawChessboardCorners(image, cvSize(numCornersHorizontal,numCornersVertical), pointBuf, found);

			cvtColor(image, image, CV_GRAY2BGR);
			imshow("Process", image);
			waitKey(200);
			image_points.push_back(pointBuf);
			object_points.push_back(obj);
		// Draw the corners on a colored image
	/*	// Write the image
			//imwrite("Task_1.jpg", image[i]);

			//imshow("Screen", image[i]);
			//waitKey(100);*/
			std::cout << i + 1 << " processed " << pointBuf[0] << "\n";
		}
	}

	Mat intrinsic = Mat(3,3, CV_32FC1);
	Mat distCoeffs;
	Size imageSize = image.size();
	vector<Mat> rvecs, tvecs;
	intrinsic.ptr<double>(0)[0] = 1;
	intrinsic.ptr<double>(1)[1] = 1;

	calibrateCamera(object_points, image_points, imageSize, intrinsic, distCoeffs, rvecs, tvecs);
	std::cout << "Matrix is " << intrinsic << endl << distCoeffs << endl;

	for(int i = 0; i < 3; i++){
		for(int j =0; j < 3; j++){
			intrinsicFile << intrinsic.at<double>(i, j) << " ";
		}
		intrinsicFile << endl;
	}

	for(int k = 0; k < 5; k++)
		distortionParams << distCoeffs.at<double>(k) << " ";

//	intrinsicFile << intrinsic;
//	distortionParams << distCoeffs;

	intrinsicFile.close();
	distortionParams.close();
	return;
}

void getImages(const sensor_msgs::Image::ConstPtr& msg)
{
	// Get the image and translate it to the image, then display it.
	char setting = 'a';
	ROS_INFO("IMG received");
	CvImagePtr cv_ptr;
	cv_ptr = toCvCopy(msg, "bgr8");
	Mat image(cv_ptr->image);
	namedWindow("Video", WINDOW_NORMAL);
	imshow("Video", image);
	char key = waitKey(30);
	//if (key == ' '){
		cout << "space bar\n";
		ostringstream name;
		name << directory << "/img" << frameCount++ << ".jpg";
		string filename = name.str();
		imwrite(filename, image);
		waitKey(10);
	if(key == 'x' || frameCount > 100){
		cout << frameCount;
		ostringstream iname, dname;
		iname << directory << "/intrinsic.txt";
		dname << directory << "/distortion.txt";
		calibrate(frameCount, 9, 7, iname.str().c_str(), dname.str().c_str() );
		return; // Actually, call the calibrate function.
	}

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "VideoShow");

  ros::NodeHandle n;
  frameCount = 0;

  	std::string topic;
	if (n.getParam("/videoFeed", topic)){
		ROS_INFO("Got param: %s", topic.c_str());
	}else{
		ROS_ERROR("Failed to get param 'videoFeed'");
	}
  	std::string topic2;
	if (n.getParam("videoFeed", topic2)){
		ROS_ERROR("Got param: %s", topic2.c_str());
	}else{
		ROS_ERROR("Failed to get param 'videoFeed'");
	}

	if (n.getParam("/saveDir", directory)){
		ROS_INFO("Got param: %s", directory.c_str() );
	}else{
		ROS_ERROR("Failed to get param 'saveDir'");
	}

	char call[256];
	sprintf(call, "exec cp %s/intrinsic.txt %s/..", directory.c_str(), directory.c_str() );
	int a = system(call);
	sprintf(call, "exec rm %s/*", directory.c_str());
	a = system(call );

 	ros::Subscriber imSub = n.subscribe(topic, 1, getImages);
	ros::Rate r(0.5);

	while (ros::ok() ){
		ros::Duration(0.5).sleep();
		ros::spinOnce();
		r.sleep();
	}
 // ros::spin(); return 0;
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012        MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/sdk/unit/details/main.hpp>
#include <nt2/sdk/unit/details/ulp.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <cmath>
#include <vector>
#include <limits>
#include <typeinfo>
#include <iostream>

template<class T> bool check_ulp_fundamental()
{
  T value(1), different_value;
  std::vector< nt2::details::failed_value<T,T> >  fails;

  different_value = value + 16*std::numeric_limits<T>::epsilon();

  std::cout << "Check for: " << typeid(T).name() << "\t";
  double u0 = 0;
  bool ok = nt2::unit::max_ulp(value,value,0.5,fails,u0);
  std::cout << "max_ulp(a,a) = " << u0 << "\t";

  fails.clear();
  double u1 = 0;
  ok = nt2::unit::max_ulp(value,different_value,8,fails, u1);
  std::cout << "max_ulp(a,b) = " << u1 << "\t";
  std::cout << ((!u0 && u1==8) ? "OK" : "NOT OK") << "\n";

  return !u0 && u1==8;
}

template<class T> bool check_ulp_sequence()
{
  std::vector<T> values(17), different_values(17);
  std::vector< nt2::details::failed_value<T,T> >  fails;

  for(std::size_t i=0;i<values.size();++i)
    values[i] = different_values[i] = T(1);

  different_values[9] += 16*std::numeric_limits<T>::epsilon();
  different_values[7] += 5*std::numeric_limits<T>::epsilon();

  std::cout << "Check for: " << typeid(T).name() << "\t";
  double u0 = 0;
  bool ok = nt2::unit::max_ulp(values,values,0.5,fails,u0);
  std::cout << "max_ulp(a,a) = " << u0 << "\t";

  fails.clear();
  double u1 = 0;
  ok = nt2::unit::max_ulp(values,different_values,8,fails, u1);
  std::cout << "max_ulp(a,b) = " << u1 << "\t";
  std::cout << ((!u0 && u1==8) ? "OK" : "NOT OK")  << "\n";

  return (!u0 && u1==8);
}

NT2_UNIT_MAIN_SPEC int NT2_UNIT_MAIN(int argc, char* argv[])
{
  std::cout << "Check for basic types\n";
  bool  check =   check_ulp_fundamental<double>()
              &&  check_ulp_fundamental<float>();

  std::cout << "\nCheck for Sequence\n";
        check =   check_ulp_sequence<double>()
              &&  check_ulp_sequence<float>();

  return check ? 0 : 1;
}


#include "stdafx.h"
#include "GeneticSequence.h"
#include <string>
#include <boost\dynamic_bitset.hpp>
#include <stdexcept>
#include <tuple>

using namespace ModularGenetics;

std::tuple<GeneticSequence, GeneticSequence> GeneticSequence::Crossover(int num_crossovers, const GeneticSequence & sequence_1, const GeneticSequence & sequence_2)
{
	//Validate values
	if (num_crossovers < 1)
		throw std::invalid_argument("Must have one or more crossovers.");
	if (sequence_1.sequence_length != sequence_2.sequence_length)
		throw std::invalid_argument("Genetic sequences must be of equal length to crossover.");

	std::string child_1, child_2;
	boost::to_string(sequence_1.bit_set, child_1);
	boost::to_string(sequence_2.bit_set, child_2);

	for (int i = 0; i < num_crossovers; i++)
	{
		int crossover_index = 1 + (rand() % (sequence_1.sequence_length - 1));

		//Crossover w/ substrings
		std::string new_child_1 = child_1.substr(0, crossover_index) + child_2.substr(crossover_index, sequence_1.sequence_length - crossover_index);
		std::string new_child_2 = child_2.substr(0, crossover_index) + child_1.substr(crossover_index, sequence_1.sequence_length - crossover_index);

		//Apply crossover
		child_1 = new_child_1;
		child_2 = new_child_2;
	}

	return std::tuple<GeneticSequence, GeneticSequence>(GeneticSequence(child_1), GeneticSequence(child_2));
}

GeneticSequence::GeneticSequence(int sequence_length, bool init_random) : sequence_length(sequence_length)
{
	if (init_random)
	{
		//Generate random string
		std::string bit_string = "";
		for (int i = 0; i < sequence_length; i++)
		{
			bit_string += (rand() + 1) % 2 == 0 ? '0' : '1';
		}
		this->bit_set = boost::dynamic_bitset<>(bit_string);
	}
	else
	{
		//Generate sequence of 0's of defined length
		this->bit_set = boost::dynamic_bitset<>(sequence_length);
	}
}

GeneticSequence::GeneticSequence(boost::dynamic_bitset<> bit_set) : sequence_length(bit_set.size())
{
	this->bit_set = bit_set;
}

GeneticSequence::GeneticSequence(std::string bit_string) : sequence_length(bit_string.length())
{
	this->bit_set = boost::dynamic_bitset<>(bit_string);
}

const boost::dynamic_bitset<>& ModularGenetics::GeneticSequence::get_bit_set() const
{
	return this->bit_set;
}


/*
 * Loss.h
 *
 *  Created on: Dec 11, 2016
 *      Author: ken
 */

#ifndef LOSS_H_
#define LOSS_H_

#include <armadillo>
#include "../layer/Layer.hpp"

/**
 * While the accumulating methods, LOSS and LOSS_PRIME for fields should stay fixed, over-ride
 * the single-sample methods of the same name in the children class.
 * */
class Loss {
public:
	Loss();
	virtual ~Loss();

	virtual double loss(arma::Cube<double> output, arma::Cube<double> y);
	double loss(arma::field<arma::Cube<double>> outputs, arma::field<arma::Cube<double>> ys);

	virtual arma::Cube<double> loss_prime(const arma::Cube<double>& output, const arma::Cube<double>& y);
	arma::field<arma::Cube<double>> loss_prime(const arma::field<arma::Cube<double>>& outputs, const arma::field<arma::Cube<double>>& ys);
};

#endif /* LOSS_H_ */


//
//  LinSysSolver.hpp
//  OptCuts
//
//  Created by Minchen Li on 6/30/18.
//

#ifndef LinSysSolver_hpp
#define LinSysSolver_hpp

#include "Types.hpp"

#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <set>
#include <map>
#include <iostream>

namespace OptCuts {

    template <typename vectorTypeI, typename vectorTypeS>
    class LinSysSolver
    {
    protected:
        int numRows;
        Eigen::VectorXi ia, ja;
        std::vector<std::map<int, int>> IJ2aI;
        Eigen::VectorXd a;

    public:
        virtual ~LinSysSolver(void) {};

    public:
        virtual void set_type(int threadAmt, int _mtype, bool is_upper_half = false) = 0;

        virtual void set_pattern(const std::vector<std::set<int>>& vNeighbor,
                                 const std::set<int>& fixedVert)
        {
            numRows = static_cast<int>(vNeighbor.size()) * DIM;
            ia.resize(vNeighbor.size() * DIM + 1);
            ia[0] = 1; // 1 + nnz above row i
            ja.resize(0); // colI of each element
            IJ2aI.resize(0); // map from matrix index to ja index
            IJ2aI.resize(vNeighbor.size() * DIM);
            for(int rowI = 0; rowI < vNeighbor.size(); rowI++) {
                if(fixedVert.find(rowI) == fixedVert.end()) {
                    int oldSize_ja = static_cast<int>(ja.size());
                    IJ2aI[rowI * DIM][rowI * DIM] = oldSize_ja;
                    IJ2aI[rowI * DIM][rowI * DIM + 1] = oldSize_ja + 1;
                    if(DIM == 3) {
                        IJ2aI[rowI * DIM][rowI * DIM + 2] = oldSize_ja + 2;
                    }
                    ja.conservativeResize(oldSize_ja + DIM);
                    ja[oldSize_ja] = rowI * DIM + 1;
                    ja[oldSize_ja + 1] = rowI * DIM + 2;
                    if(DIM == 3) {
                        ja[oldSize_ja + 2] = rowI * DIM + 3;
                    }

                    int nnz_rowI = 1;
                    for(const auto& colI : vNeighbor[rowI]) {
                        if(fixedVert.find(colI) == fixedVert.end()) {
                            if(colI > rowI) {
                                // only the lower-left part
                                // colI > rowI means upper-right, but we are preparing CSR here
                                // in a row-major manner and CHOLMOD is actually column-major
                                int oldSize_ja_temp = static_cast<int>(ja.size());
                                IJ2aI[rowI * DIM][colI * DIM] = oldSize_ja_temp;
                                IJ2aI[rowI * DIM][colI * DIM + 1] = oldSize_ja_temp + 1;
                                if(DIM == 3) {
                                    IJ2aI[rowI * DIM][colI * DIM + 2] = oldSize_ja_temp + 2;
                                }
                                ja.conservativeResize(oldSize_ja_temp + DIM);
                                ja[oldSize_ja_temp] = colI * DIM + 1;
                                ja[oldSize_ja_temp + 1] = colI * DIM + 2;
                                if(DIM == 3) {
                                    ja[oldSize_ja_temp + 2] = colI * DIM + 3;
                                }
                                nnz_rowI++;
                            }
                        }
                    }

                    // another row for y,
                    // excluding the left-bottom entry on the diagonal band
                    IJ2aI[rowI * DIM + 1] = IJ2aI[rowI * DIM];
                    for(auto& IJ2aI_newRow : IJ2aI[rowI * DIM + 1]) {
                        IJ2aI_newRow.second += nnz_rowI * DIM - 1;
                    }
                    ja.conservativeResize(ja.size() + nnz_rowI * DIM - 1);
                    ja.bottomRows(nnz_rowI * DIM - 1) = ja.block(oldSize_ja + 1, 0, nnz_rowI * DIM - 1, 1);

                    if(DIM == 3) {
                        // third row for z
                        IJ2aI[rowI * DIM + 2] = IJ2aI[rowI * DIM + 1];
                        for(auto& IJ2aI_newRow : IJ2aI[rowI * DIM + 2]) {
                            IJ2aI_newRow.second += nnz_rowI * DIM - 2;
                        }
                        ja.conservativeResize(ja.size() + nnz_rowI * DIM - 2);
                        ja.bottomRows(nnz_rowI * DIM - 2) = ja.block(oldSize_ja + 2, 0, nnz_rowI * DIM - 2, 1);

                        IJ2aI[rowI * DIM + 2].erase(rowI * DIM);
                        IJ2aI[rowI * DIM + 2].erase(rowI * DIM + 1);
                    }
                    IJ2aI[rowI * DIM + 1].erase(rowI * DIM);

                    ia[rowI * DIM + 1] = ia[rowI * DIM] + nnz_rowI * DIM;
                    ia[rowI * DIM + 2] = ia[rowI * DIM + 1] + nnz_rowI * DIM - 1;
                    if(DIM == 3) {
                        ia[rowI * DIM + 3] = ia[rowI * DIM + 2] + nnz_rowI * DIM - 2;
                    }
                }
                else {
                    int oldSize_ja = static_cast<int>(ja.size());
                    IJ2aI[rowI * DIM][rowI * DIM] = oldSize_ja;
                    IJ2aI[rowI * DIM + 1][rowI * DIM + 1] = oldSize_ja + 1;
                    if(DIM == 3) {
                        IJ2aI[rowI * DIM + 2][rowI * DIM + 2] = oldSize_ja + 2;
                    }
                    ja.conservativeResize(oldSize_ja + DIM);
                    ja[oldSize_ja] = rowI * DIM + 1;
                    ja[oldSize_ja + 1] = rowI * DIM + 2;
                    if(DIM == 3) {
                        ja[oldSize_ja + 2] = rowI * DIM + 3;
                    }
                    ia[rowI * DIM + 1] = ia[rowI * DIM] + 1;
                    ia[rowI * DIM + 2] = ia[rowI * DIM + 1] + 1;
                    if(DIM == 3) {
                        ia[rowI * DIM + 3] = ia[rowI * DIM + 2] + 1;
                    }
                }
            }
            a.resize(ja.size());
        }
        virtual void set_pattern(const Eigen::SparseMatrix<double>& mtr) = 0; //NOTE: mtr must be SPD

        virtual void update_a(const vectorTypeI &II,
                              const vectorTypeI &JJ,
                              const vectorTypeS &SS)
        {
            //TODO: faster O(1) indices!!

            assert(II.size() == JJ.size());
            assert(II.size() == SS.size());

            a.setZero(ja.size());
            for(int tripletI = 0; tripletI < II.size(); tripletI++) {
                int i = II[tripletI], j = JJ[tripletI];
                if(i <= j) {
                    //        if((i <= j) && (i != 2) && (j != 2)) {
                    assert(i < IJ2aI.size());
                    const auto finder = IJ2aI[i].find(j);
                    assert(finder != IJ2aI[i].end());
                    a[finder->second] += SS[tripletI];
                }
            }
            //    a[IJ2aI[2].find(2)->second] = 1.0;
        }
        virtual void update_a(const Eigen::SparseMatrix<double>& mtr)
        {
            assert(0 && "please implement in subclass!");
        }

        virtual void analyze_pattern(void) = 0;

        virtual bool factorize(void) = 0;

        virtual void solve(Eigen::VectorXd &rhs,
                           Eigen::VectorXd &result) = 0;

        virtual void multiply(const Eigen::VectorXd& x,
                              Eigen::VectorXd& Ax)
        {
            assert(x.size() == numRows);
            assert(IJ2aI.size() == numRows);

            Ax.setZero(numRows);
            for(int rowI = 0; rowI < numRows; ++rowI) {
                for(const auto& colI : IJ2aI[rowI]) {
                    Ax[rowI] += colI.second * x[colI.first];
                    if(rowI != colI.first) {
                        Ax[colI.first] += colI.second * x[rowI];
                    }
                }
            }
        }

    public:
        virtual double coeffMtr(int rowI, int colI) const {
            if(rowI > colI) {
                // return only upper right part for symmetric matrix
                int temp = rowI;
                rowI = colI;
                colI = temp;
            }
            assert(rowI < IJ2aI.size());
            const auto finder = IJ2aI[rowI].find(colI);
            if(finder != IJ2aI[rowI].end()) {
                return a[finder->second];
            }
            else {
                return 0.0;
            }
        }
        virtual void getCoeffMtr(Eigen::SparseMatrix<double>& mtr) const {
            mtr.resize(numRows, numRows);
            mtr.setZero();
            mtr.reserve(a.size() * 2 - numRows);
            for(int rowI = 0; rowI < numRows; rowI++) {
                for(const auto& colIter : IJ2aI[rowI]) {
                    mtr.insert(rowI, colIter.first) = a[colIter.second];
                    if(rowI != colIter.first) {
                        mtr.insert(colIter.first, rowI) = a[colIter.second];
                    }
                }
            }
        }
        virtual void setCoeff(int rowI, int colI, double val) {
            //TODO: faster O(1) indices!!

            if(rowI <= colI) {
                assert(rowI < IJ2aI.size());
                const auto finder = IJ2aI[rowI].find(colI);
                assert(finder != IJ2aI[rowI].end());
                a[finder->second] = val;
            }
        }
        virtual void setZero(void) {
            a.setZero();
        }
        virtual void addCoeff(int rowI, int colI, double val) {
            //TODO: faster O(1) indices!!

            if(rowI <= colI) {
                assert(rowI < IJ2aI.size());
                const auto finder = IJ2aI[rowI].find(colI);
                assert(finder != IJ2aI[rowI].end());
                a[finder->second] += val;
            }
        }

        virtual int getNumRows(void) const {
            return numRows;
        }
        virtual int getNumNonzeros(void) const {
            return a.size();
        }
        virtual const std::vector<std::map<int, int>>& getIJ2aI(void) const {
            return IJ2aI;
        }
        virtual Eigen::VectorXi& get_ia(void) { return ia; }
        virtual Eigen::VectorXi& get_ja(void) { return ja; }
        virtual Eigen::VectorXd& get_a(void) { return a; }
        virtual const Eigen::VectorXd& get_a(void) const { return a; }
    };

}

#endif /* LinSysSolver_hpp */


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file TransformEvaluator.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_TRANSFORM_EVALUATOR_HPP
#define STEAM_TRANSFORM_EVALUATOR_HPP

#include <Eigen/Core>

#include <steam/evaluator/blockauto/BlockAutomaticEvaluator.hpp>
#include <steam/state/LieGroupStateVar.hpp>
#include <steam/state/VectorSpaceStateVar.hpp>
#include <steam/common/Time.hpp>

namespace steam {
namespace se3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Evaluator for a transformation matrix
///
/// *Note that we fix MAX_STATE_DIM to 6. Typically the performance benefits of fixed size
///  matrices begin to die if larger than 6x6. Size 6 allows for transformation matrices
///  and 6D velocities. If you have a state larger than this, consider writing an
///  error evaluator that extends from ErrorEvaluatorX.
//////////////////////////////////////////////////////////////////////////////////////////////
typedef BlockAutomaticEvaluator<lgmath::se3::Transformation, 6, 6> TransformEvaluator;

} // se3
} // steam

#endif // STEAM_TRANSFORM_EVALUATOR_HPP


#ifndef SCITBX_R3_UTILS_HPP
#define SCITBX_R3_UTILS_HPP

#include <scitbx/vec3.h>
#include <tbxx/error_utils.hpp>
#include <boost/unordered_set.hpp>
#include <boost/noncopyable.hpp>
#include <vector>

namespace scitbx { namespace r3_utils {

  struct clash_detector_simple : boost::noncopyable
  {
    std::vector<boost::unordered_set<unsigned> > exclusion_sets;
    double threshold_sq;

    clash_detector_simple() {}

    clash_detector_simple(
      unsigned n_vertices,
      double threshold)
    :
      exclusion_sets(n_vertices),
      threshold_sq(threshold*threshold)
    {}

    void
    add_exclusion(
      int i,
      int j)
    {
      TBXX_ASSERT(i < j);
      exclusion_sets[i].insert(j);
    }

    bool
    has_clash(
      af::const_ref<vec3<double> > const& sites_cart)
    {
      TBXX_ASSERT(sites_cart.size() == exclusion_sets.size());
      unsigned n = static_cast<unsigned>(sites_cart.size());
      for(unsigned i=0;i<n-1;i++) {
        vec3<double> const& sci = sites_cart[i];
        boost::unordered_set<unsigned> const& esi = exclusion_sets[i];
        boost::unordered_set<unsigned>::const_iterator
          esi_end = esi.end();
        for(unsigned j=i+1;j<n;j++) {
          if (esi.find(j) != esi_end) continue;
          vec3<double> const& scj = sites_cart[j];
          double d = (scj - sci).length_sq();
          if (d < threshold_sq) {
            return true;
          }
        }
      }
      return false;
    }
  };

}}

#endif // GUARD


#pragma once
#include <Eigen/Geometry>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <trifinger_cameras/camera_parameters.hpp>
#include <trifinger_object_tracking/cube_model.hpp>
#include <trifinger_object_tracking/types.hpp>

// ignore all warnings of optim (this is a third-party library)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"

#define OPTIM_ENABLE_ARMA_WRAPPERS
#define OPTIM_DONT_USE_OPENMP
#include <optim/optim.hpp>

#pragma GCC diagnostic pop

namespace trifinger_object_tracking
{
struct Stats
{
    cv::Vec3f lower_bound;
    cv::Vec3f upper_bound;
    cv::Vec3f mean;
};

cv::Mat getPoseMatrix(cv::Point3f, cv::Point3f);

class PoseDetector
{
public:
    //! Number of cameras
    static constexpr unsigned int N_CAMERAS = 3;

    typedef std::array<std::vector<std::vector<cv::Point>>,
                       PoseDetector::N_CAMERAS>
        MasksPixels;

    PoseDetector(const CubeModel &cube_model,
                 const std::array<trifinger_cameras::CameraParameters,
                                  N_CAMERAS> &camera_parameters);

    Pose find_pose(
        const std::array<std::vector<FaceColor>, N_CAMERAS> &dominant_colors,
        const std::array<std::vector<cv::Mat>, N_CAMERAS> &masks);

    // TODO which points are projected?
    std::vector<std::vector<cv::Point2f>> get_projected_points() const;

    /**
     * @brief Get corner indices of the visible faces.
     *
     * Determines which faces of the object is visible to the camera and returns
     * the corner indices of these faces.
     *
     * @param camera_idx Index of the camera.
     * @param cube_pose_world Pose of the cube in the world frame.
     *
     * @return For each visible face a pair of the face color and the list of
     *     the corner indices of the four corners of that face.
     */
    std::vector<std::pair<FaceColor, std::array<unsigned int, 4>>>
    get_visible_faces(unsigned int camera_idx,
                      const cv::Affine3f &cube_pose_world) const;

    /**
     * @brief Get corner indices of the visible faces.
     *
     * Overloaded version that used the last detected pose for the cube.
     * Call find_pose() first, otherwise the result is undefined!
     *
     * @param camera_idx Index of the camera.
     *
     * @return For each visible face a pair of the face color and the list of
     *     the corner indices of the four corners of that face.
     */
    std::vector<std::pair<FaceColor, std::array<unsigned int, 4>>>
    get_visible_faces(unsigned int camera_idx) const;

    unsigned int get_num_misclassified_pixels() const
    {
        return num_misclassified_pixels_;
    }

    float get_segmented_pixels_ratio() const
    {
        return segmented_pixels_ratio_;
    }

    float get_confidence() const
    {
        return confidence_;
    }

    void set_pose(const Pose &pose);

private:
    CubeModel cube_model_;

    std::array<cv::Mat, N_CAMERAS> camera_matrices_;
    std::array<cv::Mat, N_CAMERAS> distortion_coeffs_;
    std::array<cv::Vec3d, N_CAMERAS> camera_orientations_;
    std::array<cv::Vec3d, N_CAMERAS> camera_translations_;

    cv::Mat corners_in_cube_frame_;
    cv::Mat reference_vector_normals_;

    Stats position_;
    Stats orientation_;

    //! Total number of pixels in one image (i.e. width*height).
    const unsigned int num_total_pixels_in_image_;

    //! @brief Number of misclassified pixels in the last call of find_pose().
    unsigned int num_misclassified_pixels_ = 0;
    float segmented_pixels_ratio_ = 0;
    float confidence_ = 0.0;

    void optimize_using_optim(
        const std::array<std::vector<FaceColor>, N_CAMERAS> &dominant_colors,
        const std::array<std::vector<cv::Mat>, N_CAMERAS> &masks);

    float cost_function(
        const cv::Vec3f &position,
        const cv::Vec3f &orientation,
        const std::array<std::vector<FaceColor>, N_CAMERAS> &dominant_colors,
        const MasksPixels &masks_pixels,
        const float distance_cost_scaling,
        const float invisibility_cost_scaling,
        const float height_cost_scaling);

    float compute_confidence(
        const cv::Vec3f &position,
        const cv::Vec3f &orientation,
        const std::array<std::vector<FaceColor>, N_CAMERAS> &dominant_colors,
        const MasksPixels &masks_pixels);

    void compute_color_visibility(
        const FaceColor &color,
        const cv::Mat &face_normals,
        const cv::Mat &cube_corners,
        bool *is_visible,
        float *face_normal_dot_camera_direction) const;

    void compute_face_normals_and_corners(const unsigned int camera_idx,
                                          const cv::Affine3f &cube_pose_world,
                                          cv::Mat *normals,
                                          cv::Mat *corners) const;

    bool is_face_visible(FaceColor color,
                         unsigned int camera_idx,
                         const cv::Affine3f &cube_pose_world,
                         float *out_dot_product = nullptr) const;
};

}  // namespace trifinger_object_tracking


// Copyright (c) 2017 University of Minnesota
//
// MCLSCENE Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// By Matt Overby (http://www.mattoverby.net)

#ifndef MCL_VEC3_H
#define MCL_VEC3_H 1

// Helper to hush api warnings
#define MCL_UNUSED(x) (void)(x)

#include <Eigen/Geometry>

namespace mcl {

	// Common types that I don't feel like typing out all the time:
	template <typename T> using Vec4 = Eigen::Matrix<T,4,1>;
	template <typename T> using Vec3 = Eigen::Matrix<T,3,1>;
	template <typename T> using Vec2 = Eigen::Matrix<T,2,1>;
	typedef Vec4<float> Vec4f;
	typedef Vec3<float> Vec3f;
	typedef Vec2<float> Vec2f;
	typedef Vec4<double> Vec4d;
	typedef Vec3<double> Vec3d;
	typedef Vec2<double> Vec2d;
	typedef Vec4<int> Vec4i;
	typedef Vec3<int> Vec3i;
	typedef Vec2<int> Vec2i;

namespace vec {

	template <typename T> // Returns a normalized vector
	static inline Vec3<T> normalized(const Vec3<T> &v){ Vec3<T> t=v; t.normalize(); return t; }

	template <typename T, size_t D> // Vec output as transpose with spaces instead of tabs
	static inline std::string to_str(const Eigen::Matrix<T,D,1> &v){
		std::stringstream ss; ss << v[0];
		for( size_t i=1; i<D; ++i ){ ss << ' ' << v[i]; }
		return ss.str();
	}

	template <typename T> // Compute barycentric coords for a point on a triangle
	static inline Vec3<T> barycoords(const Vec3<T> &p, const Vec3<T> &p0, const Vec3<T> &p1, const Vec3<T> &p2){
		Vec3<T> v0 = p1 - p0, v1 = p2 - p0, v2 = p - p0;
		T d00 = v0.dot(v0);
		T d01 = v0.dot(v1);
		T d11 = v1.dot(v1);
		T d20 = v2.dot(v0);
		T d21 = v2.dot(v1);
		T invDenom = 1.0 / (d00 * d11 - d01 * d01);
		Vec3<T> r;
		r[1] = (d11 * d20 - d01 * d21) * invDenom;
		r[2] = (d00 * d21 - d01 * d20) * invDenom;
		r[0] = 1.0 - r[1] - r[2];
		return r;
	}

	template <typename T> // scalar triple product
	static inline T scalar_triple_product( const Vec3<T> &u, const Vec3<T> &v, const Vec3<T> &w ){ return u.dot(v.cross(w)); }

	template <typename T> // Compute barycentric coords for a point in a tet
	static inline Vec4<T> barycoords(const Vec3<T> &p, const Vec3<T> &a, const Vec3<T> &b, const Vec3<T> &c, const Vec3<T> &d){
		Vec3<T> vap = p - a;
		Vec3<T> vbp = p - b;
		Vec3<T> vab = b - a;
		Vec3<T> vac = c - a;
		Vec3<T> vad = d - a;
		Vec3<T> vbc = c - b;
		Vec3<T> vbd = d - b;
		T va6 = scalar_triple_product(vbp, vbd, vbc);
		T vb6 = scalar_triple_product(vap, vac, vad);
		T vc6 = scalar_triple_product(vap, vad, vab);
		T vd6 = scalar_triple_product(vap, vab, vac);
		T v6 = 1.0 / scalar_triple_product(vab, vac, vad);
		return Vec4<T>(va6*v6, vb6*v6, vc6*v6, vd6*v6);
	}

	template <typename T> // Spherical coords to cartesian
	static inline Vec3<T> spherical_to_cartesian(T theta, T phi){
		T sin_t = std::sin(theta); T cos_t = std::cos(theta);
		T sin_p = std::sin(phi); T cos_p = std::cos(phi);
		return Vec3<T>( sin_t * sin_p, sin_t * cos_p, cos_t );
	}

	template <typename T>  // Cartesian coords to spherical
	static inline Vec2<T> cartesian_to_spherical(const Vec3<T> &v){
		Vec2<T> r( std::acos(v[2]), std::atan2(v[1], v[0]) );
		if(r[1] < 0){ r[1] += 2*M_PI; }
		return r;
	}

} // end namespace vec

// Randoms (u1, u2, etc...): 0 to 1
// Putting it here until I find a better spot
namespace sample {

	template<typename T> // Uniformly samples a cone (e.g. spotlight)
	static inline Vec3<T> uniform_cone( T u1, T u2, T max_theta ){
		T cos_theta = (1 - u1) + u1 * std::cos(max_theta);
		T sin_theta = std::sqrt(1 - cos_theta*cos_theta);
		T phi = u2 * 2 * M_PI;
		return Vec3<T>( std::cos(phi)*sin_theta, std::sin(phi)*sin_theta, cos_theta );
	}

	template<typename T> // Cosine weighted hemisphere sampling (e.g. diffuse reflection)
	static inline Vec3<T> cosine_hemisphere( T u1, T u2 ){
		T r = std::sqrt( u1 );
		T theta = 2 * M_PI * u2;
		return Vec3<T>( r * std::cos(theta), r * std::sin(theta), std::sqrt( std::max(T(0), T(1.f-u1)) ) );
	}

}; // end namespace sample

} // end namespace mcl

/*
//
//	trimesh and mcl::Vec xforms:
//
namespace trimesh {

	template <typename T, typename U>
	static inline mcl::Vec3<T> operator*(const trimesh::XForm<U> &m, const mcl::Vec3<T> &v){
		mcl::Vec3<T> r;
		r[0] = m[0]*v[0]+m[4]*v[1]+m[8]*v[2]+m[12];
		r[1] = m[1]*v[0]+m[5]*v[1]+m[9]*v[2]+m[13];
		r[2] = m[2]*v[0]+m[6]*v[1]+m[10]*v[2]+m[14];
		return r;
	}

	template <typename T, typename U>
	static inline mcl::Vec4<T> operator*(const trimesh::XForm<U> &m, const mcl::Vec4<T> &v){
		mcl::Vec4<T> r;
		r[0] = m[0]*v[0]+m[4]*v[1]+m[8]*v[2]+m[12]*v[3];
		r[1] = m[1]*v[0]+m[5]*v[1]+m[9]*v[2]+m[13]*v[3];
		r[2] = m[2]*v[0]+m[6]*v[1]+m[10]*v[2]+m[14]*v[3];
		r[3] = m[3]*v[0]+m[7]*v[1]+m[11]*v[2]+m[15]*v[3];
		return r;
	}

} // end namespace trimesh
*/

#endif


#include "mex.h"

#include <igl/readOBJ.h>
#include <igl/matlab/prepare_lhs.h>
#include <Eigen/Core>

void mexFunction(
     int          nlhs,
     mxArray      *plhs[],
     int          nrhs,
     const mxArray *prhs[]
     )
{
  using namespace Eigen;
  /* Check for proper number of arguments */

  if (nrhs != 1)
  {
    mexErrMsgIdAndTxt("MATLAB:mexcpp:nargin",
        "readOBJ requires 1 input arguments, the path of the file to open");
  }

  // Read the file path
  char* file_path = mxArrayToString(prhs[0]);

  MatrixXd V;
  MatrixXi F;

  // Read the mesh
  if(!igl::readOBJ(file_path,V,F))
  {
    mexErrMsgIdAndTxt("MATLAB:mexcpp:fileio", "igl::readOBJ failed.");
  }
  // Return the matrices to matlab
  switch(nlhs)
  {
    case 2:
      igl::matlab::prepare_lhs_index(F,plhs+1);
    case 1:
      igl::matlab::prepare_lhs_double(V,plhs);
    default: break;
  }

  return;
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

//
// This ensures all our code gets tested, even though it may
// not be the fastest configuration in normal use:
//
#define BOOST_MP_USE_LIMB_SHIFT

#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/timer.hpp>
#include "test.hpp"

#ifdef _MSC_VER
#pragma warning(disable : 4127) //  Conditional expression is constant
#endif

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3)
#define TEST1
#define TEST2
#define TEST3
#endif

template <class T>
T generate_random(unsigned bits_wanted)
{
   static boost::random::mt19937               gen;
   typedef boost::random::mt19937::result_type random_type;

   T        max_val;
   unsigned digits;
   if (std::numeric_limits<T>::is_bounded && (bits_wanted == (unsigned)std::numeric_limits<T>::digits))
   {
      max_val = (std::numeric_limits<T>::max)();
      digits  = std::numeric_limits<T>::digits;
   }
   else
   {
      max_val = T(1) << bits_wanted;
      digits  = bits_wanted;
   }

   unsigned bits_per_r_val = std::numeric_limits<random_type>::digits - 1;
   while ((random_type(1) << bits_per_r_val) > (gen.max)())
      --bits_per_r_val;

   unsigned terms_needed = digits / bits_per_r_val + 1;

   T val = 0;
   for (unsigned i = 0; i < terms_needed; ++i)
   {
      val *= (gen.max)();
      val += gen();
   }
   val %= max_val;
   return val;
}

template <class T>
struct is_checked_cpp_int : public boost::mpl::false_
{};
template <unsigned MinBits, unsigned MaxBits, boost::multiprecision::cpp_integer_type SignType, class Allocator, boost::multiprecision::expression_template_option ET>
struct is_checked_cpp_int<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<MinBits, MaxBits, SignType, boost::multiprecision::checked, Allocator>, ET> > : public boost::mpl::true_
{};

template <class Number>
struct tester
{
   typedef Number                                         test_type;
   typedef typename test_type::backend_type::checked_type checked;

   unsigned     last_error_count;
   boost::timer tim;

   boost::multiprecision::mpz_int a, b, c, d;
   int                            si;
   unsigned                       ui;
   test_type                      a1, b1, c1, d1;

   void t1()
   {
      using namespace boost::multiprecision;
      BOOST_CHECK_EQUAL(a.str(), a1.str());
      BOOST_CHECK_EQUAL(b.str(), b1.str());
      BOOST_CHECK_EQUAL(c.str(), c1.str());
      BOOST_CHECK_EQUAL(d.str(), d1.str());
      BOOST_CHECK_EQUAL(mpz_int(a + b).str(), test_type(a1 + b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) += b).str(), (test_type(a1) += b1).str());
      BOOST_CHECK_EQUAL((mpz_int(b) += a).str(), (test_type(b1) += a1).str());
      BOOST_CHECK_EQUAL(mpz_int(a - b).str(), test_type(a1 - b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) -= b).str(), (test_type(a1) -= b1).str());
      BOOST_CHECK_EQUAL(mpz_int(mpz_int(-a) + b).str(), test_type(test_type(-a1) + b1).str());
      BOOST_CHECK_EQUAL(mpz_int(mpz_int(-a) - b).str(), test_type(test_type(-a1) - b1).str());
      BOOST_CHECK_EQUAL(mpz_int(c * d).str(), test_type(c1 * d1).str());
      BOOST_CHECK_EQUAL((mpz_int(c) *= d).str(), (test_type(c1) *= d1).str());
      BOOST_CHECK_EQUAL((mpz_int(d) *= c).str(), (test_type(d1) *= c1).str());
      BOOST_CHECK_EQUAL(mpz_int(c * -d).str(), test_type(c1 * -d1).str());
      BOOST_CHECK_EQUAL(mpz_int(-c * d).str(), test_type(-c1 * d1).str());
      BOOST_CHECK_EQUAL((mpz_int(c) *= -d).str(), (test_type(c1) *= -d1).str());
      BOOST_CHECK_EQUAL((mpz_int(-d) *= c).str(), (test_type(-d1) *= c1).str());
      BOOST_CHECK_EQUAL(mpz_int(b * c).str(), test_type(b1 * c1).str());
      BOOST_CHECK_EQUAL(mpz_int(a / b).str(), test_type(a1 / b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) /= b).str(), (test_type(a1) /= b1).str());
      BOOST_CHECK_EQUAL(mpz_int(a / -b).str(), test_type(a1 / -b1).str());
      BOOST_CHECK_EQUAL(mpz_int(-a / b).str(), test_type(-a1 / b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) /= -b).str(), (test_type(a1) /= -b1).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) /= b).str(), (test_type(-a1) /= b1).str());
      BOOST_CHECK_EQUAL(mpz_int(a / d).str(), test_type(a1 / d1).str());
      BOOST_CHECK_EQUAL(mpz_int(a % b).str(), test_type(a1 % b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= b).str(), (test_type(a1) %= b1).str());
      BOOST_CHECK_EQUAL(mpz_int(a % -b).str(), test_type(a1 % -b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= -b).str(), (test_type(a1) %= -b1).str());
      BOOST_CHECK_EQUAL(mpz_int(-a % b).str(), test_type(-a1 % b1).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) %= b).str(), (test_type(-a1) %= b1).str());
      BOOST_CHECK_EQUAL(mpz_int(a % d).str(), test_type(a1 % d1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= d).str(), (test_type(a1) %= d1).str());

      if (!std::numeric_limits<test_type>::is_bounded)
      {
         test_type p = a1 * b1;
         test_type r;
         divide_qr(p, b1, p, r);
         BOOST_CHECK_EQUAL(p, a1);
         BOOST_CHECK_EQUAL(r, test_type(0));

         p = a1 * d1;
         divide_qr(p, d1, p, r);
         BOOST_CHECK_EQUAL(p, a1);
         BOOST_CHECK_EQUAL(r, test_type(0));

         divide_qr(p, test_type(1), p, r);
         BOOST_CHECK_EQUAL(p, a1);
         BOOST_CHECK_EQUAL(r, test_type(0));
      }
   }

   void t2()
   {
      using namespace boost::multiprecision;
      // bitwise ops:
      BOOST_CHECK_EQUAL(mpz_int(a | b).str(), test_type(a1 | b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) |= b).str(), (test_type(a1) |= b1).str());
      if (!is_checked_cpp_int<test_type>::value)
      {
         BOOST_CHECK_EQUAL(mpz_int(-a | b).str(), test_type(-a1 | b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) |= b).str(), (test_type(-a1) |= b1).str());
         BOOST_CHECK_EQUAL(mpz_int(a | -b).str(), test_type(a1 | -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(a) |= -b).str(), (test_type(a1) |= -b1).str());
         BOOST_CHECK_EQUAL(mpz_int(-a | -b).str(), test_type(-a1 | -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) |= -b).str(), (test_type(-a1) |= -b1).str());
      }
      BOOST_CHECK_EQUAL(mpz_int(a & b).str(), test_type(a1 & b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) &= b).str(), (test_type(a1) &= b1).str());
      if (!is_checked_cpp_int<test_type>::value)
      {
         BOOST_CHECK_EQUAL(mpz_int(-a & b).str(), test_type(-a1 & b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) &= b).str(), (test_type(-a1) &= b1).str());
         BOOST_CHECK_EQUAL(mpz_int(a & -b).str(), test_type(a1 & -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(a) &= -b).str(), (test_type(a1) &= -b1).str());
         BOOST_CHECK_EQUAL(mpz_int(-a & -b).str(), test_type(-a1 & -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) &= -b).str(), (test_type(-a1) &= -b1).str());
      }
      BOOST_CHECK_EQUAL(mpz_int(a ^ b).str(), test_type(a1 ^ b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) ^= b).str(), (test_type(a1) ^= b1).str());
      if (!is_checked_cpp_int<test_type>::value)
      {
         BOOST_CHECK_EQUAL(mpz_int(-a ^ b).str(), test_type(-a1 ^ b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) ^= b).str(), (test_type(-a1) ^= b1).str());
         BOOST_CHECK_EQUAL(mpz_int(a ^ -b).str(), test_type(a1 ^ -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(a) ^= -b).str(), (test_type(a1) ^= -b1).str());
         BOOST_CHECK_EQUAL(mpz_int(-a ^ -b).str(), test_type(-a1 ^ -b1).str());
         BOOST_CHECK_EQUAL((mpz_int(-a) ^= -b).str(), (test_type(-a1) ^= -b1).str());
      }
      // Shift ops:
      for (unsigned i = 0; i < 128; ++i)
      {
         if (!std::numeric_limits<test_type>::is_bounded)
         {
            BOOST_CHECK_EQUAL(mpz_int(a << i).str(), test_type(a1 << i).str());
            BOOST_CHECK_EQUAL(mpz_int(-a << i).str(), test_type(-a1 << i).str());
         }
         else if (!is_checked_cpp_int<test_type>::value)
         {
            test_type t1(mpz_int(a << i).str());
            test_type t2 = a1 << i;
            BOOST_CHECK_EQUAL(t1, t2);
            t1 = test_type(mpz_int(-a << i).str());
            t2 = -a1 << i;
            BOOST_CHECK_EQUAL(t1, t2);
         }
         BOOST_CHECK_EQUAL(mpz_int(a >> i).str(), test_type(a1 >> i).str());
         if (!is_checked_cpp_int<test_type>::value)
         {
            BOOST_CHECK_EQUAL(mpz_int(-a >> i).str(), test_type(-a1 >> i).str());
         }
      }
      // gcd/lcm
      BOOST_CHECK_EQUAL(mpz_int(gcd(a, b)).str(), test_type(gcd(a1, b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(c, d)).str(), test_type(lcm(c1, d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-a, b)).str(), test_type(gcd(-a1, b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-c, d)).str(), test_type(lcm(-c1, d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-a, -b)).str(), test_type(gcd(-a1, -b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-c, -d)).str(), test_type(lcm(-c1, -d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(a, -b)).str(), test_type(gcd(a1, -b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(c, -d)).str(), test_type(lcm(c1, -d1)).str());
      // Integer sqrt:
      mpz_int   r;
      test_type r1;
      BOOST_CHECK_EQUAL(sqrt(a, r).str(), sqrt(a1, r1).str());
      BOOST_CHECK_EQUAL(r.str(), r1.str());
   }

   void t3()
   {
      using namespace boost::multiprecision;
      // Now check operations involving signed integers:
      BOOST_CHECK_EQUAL(mpz_int(a + si).str(), test_type(a1 + si).str());
      BOOST_CHECK_EQUAL(mpz_int(a + -si).str(), test_type(a1 + -si).str());
      BOOST_CHECK_EQUAL(mpz_int(-a + si).str(), test_type(-a1 + si).str());
      BOOST_CHECK_EQUAL(mpz_int(si + a).str(), test_type(si + a1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) += si).str(), (test_type(a1) += si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) += -si).str(), (test_type(a1) += -si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) += si).str(), (test_type(-a1) += si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) += -si).str(), (test_type(-a1) += -si).str());
      BOOST_CHECK_EQUAL(mpz_int(a - si).str(), test_type(a1 - si).str());
      BOOST_CHECK_EQUAL(mpz_int(a - -si).str(), test_type(a1 - -si).str());
      BOOST_CHECK_EQUAL(mpz_int(-a - si).str(), test_type(-a1 - si).str());
      BOOST_CHECK_EQUAL(mpz_int(si - a).str(), test_type(si - a1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) -= si).str(), (test_type(a1) -= si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) -= -si).str(), (test_type(a1) -= -si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) -= si).str(), (test_type(-a1) -= si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) -= -si).str(), (test_type(-a1) -= -si).str());
      BOOST_CHECK_EQUAL(mpz_int(b * si).str(), test_type(b1 * si).str());
      BOOST_CHECK_EQUAL(mpz_int(b * -si).str(), test_type(b1 * -si).str());
      BOOST_CHECK_EQUAL(mpz_int(-b * si).str(), test_type(-b1 * si).str());
      BOOST_CHECK_EQUAL(mpz_int(si * b).str(), test_type(si * b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) *= si).str(), (test_type(a1) *= si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) *= -si).str(), (test_type(a1) *= -si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) *= si).str(), (test_type(-a1) *= si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) *= -si).str(), (test_type(-a1) *= -si).str());
      BOOST_CHECK_EQUAL(mpz_int(a / si).str(), test_type(a1 / si).str());
      BOOST_CHECK_EQUAL(mpz_int(a / -si).str(), test_type(a1 / -si).str());
      BOOST_CHECK_EQUAL(mpz_int(-a / si).str(), test_type(-a1 / si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) /= si).str(), (test_type(a1) /= si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) /= -si).str(), (test_type(a1) /= -si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) /= si).str(), (test_type(-a1) /= si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) /= -si).str(), (test_type(-a1) /= -si).str());
      BOOST_CHECK_EQUAL(mpz_int(a % si).str(), test_type(a1 % si).str());
      BOOST_CHECK_EQUAL(mpz_int(a % -si).str(), test_type(a1 % -si).str());
      BOOST_CHECK_EQUAL(mpz_int(-a % si).str(), test_type(-a1 % si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= si).str(), (test_type(a1) %= si).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= -si).str(), (test_type(a1) %= -si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) %= si).str(), (test_type(-a1) %= si).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) %= -si).str(), (test_type(-a1) %= -si).str());
      if ((si > 0) || !is_checked_cpp_int<test_type>::value)
      {
         BOOST_CHECK_EQUAL(mpz_int(a | si).str(), test_type(a1 | si).str());
         BOOST_CHECK_EQUAL((mpz_int(a) |= si).str(), (test_type(a1) |= si).str());
         BOOST_CHECK_EQUAL(mpz_int(a & si).str(), test_type(a1 & si).str());
         BOOST_CHECK_EQUAL((mpz_int(a) &= si).str(), (test_type(a1) &= si).str());
         BOOST_CHECK_EQUAL(mpz_int(a ^ si).str(), test_type(a1 ^ si).str());
         BOOST_CHECK_EQUAL((mpz_int(a) ^= si).str(), (test_type(a1) ^= si).str());
         BOOST_CHECK_EQUAL(mpz_int(si | a).str(), test_type(si | a1).str());
         BOOST_CHECK_EQUAL(mpz_int(si & a).str(), test_type(si & a1).str());
         BOOST_CHECK_EQUAL(mpz_int(si ^ a).str(), test_type(si ^ a1).str());
      }
      BOOST_CHECK_EQUAL(mpz_int(gcd(a, si)).str(), test_type(gcd(a1, si)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(si, b)).str(), test_type(gcd(si, b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(c, si)).str(), test_type(lcm(c1, si)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(si, d)).str(), test_type(lcm(si, d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-a, si)).str(), test_type(gcd(-a1, si)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-si, b)).str(), test_type(gcd(-si, b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-c, si)).str(), test_type(lcm(-c1, si)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-si, d)).str(), test_type(lcm(-si, d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-a, -si)).str(), test_type(gcd(-a1, -si)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-si, -b)).str(), test_type(gcd(-si, -b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-c, -si)).str(), test_type(lcm(-c1, -si)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-si, -d)).str(), test_type(lcm(-si, -d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(a, -si)).str(), test_type(gcd(a1, -si)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(si, -b)).str(), test_type(gcd(si, -b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(c, -si)).str(), test_type(lcm(c1, -si)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(si, -d)).str(), test_type(lcm(si, -d1)).str());
   }

   void t4()
   {
      using namespace boost::multiprecision;
      // Now check operations involving unsigned integers:
      BOOST_CHECK_EQUAL(mpz_int(a + ui).str(), test_type(a1 + ui).str());
      BOOST_CHECK_EQUAL(mpz_int(-a + ui).str(), test_type(-a1 + ui).str());
      BOOST_CHECK_EQUAL(mpz_int(ui + a).str(), test_type(ui + a1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) += ui).str(), (test_type(a1) += ui).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) += ui).str(), (test_type(-a1) += ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a - ui).str(), test_type(a1 - ui).str());
      BOOST_CHECK_EQUAL(mpz_int(-a - ui).str(), test_type(-a1 - ui).str());
      BOOST_CHECK_EQUAL(mpz_int(ui - a).str(), test_type(ui - a1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) -= ui).str(), (test_type(a1) -= ui).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) -= ui).str(), (test_type(-a1) -= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(b * ui).str(), test_type(b1 * ui).str());
      BOOST_CHECK_EQUAL(mpz_int(-b * ui).str(), test_type(-b1 * ui).str());
      BOOST_CHECK_EQUAL(mpz_int(ui * b).str(), test_type(ui * b1).str());
      BOOST_CHECK_EQUAL((mpz_int(a) *= ui).str(), (test_type(a1) *= ui).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) *= ui).str(), (test_type(-a1) *= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a / ui).str(), test_type(a1 / ui).str());
      BOOST_CHECK_EQUAL(mpz_int(-a / ui).str(), test_type(-a1 / ui).str());
      BOOST_CHECK_EQUAL((mpz_int(a) /= ui).str(), (test_type(a1) /= ui).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) /= ui).str(), (test_type(-a1) /= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a % ui).str(), test_type(a1 % ui).str());
      BOOST_CHECK_EQUAL(mpz_int(-a % ui).str(), test_type(-a1 % ui).str());
      BOOST_CHECK_EQUAL((mpz_int(a) %= ui).str(), (test_type(a1) %= ui).str());
      BOOST_CHECK_EQUAL((mpz_int(-a) %= ui).str(), (test_type(-a1) %= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a | ui).str(), test_type(a1 | ui).str());
      BOOST_CHECK_EQUAL((mpz_int(a) |= ui).str(), (test_type(a1) |= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a & ui).str(), test_type(a1 & ui).str());
      BOOST_CHECK_EQUAL((mpz_int(a) &= ui).str(), (test_type(a1) &= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(a ^ ui).str(), test_type(a1 ^ ui).str());
      BOOST_CHECK_EQUAL((mpz_int(a) ^= ui).str(), (test_type(a1) ^= ui).str());
      BOOST_CHECK_EQUAL(mpz_int(ui | a).str(), test_type(ui | a1).str());
      BOOST_CHECK_EQUAL(mpz_int(ui & a).str(), test_type(ui & a1).str());
      BOOST_CHECK_EQUAL(mpz_int(ui ^ a).str(), test_type(ui ^ a1).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(a, ui)).str(), test_type(gcd(a1, ui)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(ui, b)).str(), test_type(gcd(ui, b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(c, ui)).str(), test_type(lcm(c1, ui)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(ui, d)).str(), test_type(lcm(ui, d1)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(-a, ui)).str(), test_type(gcd(-a1, ui)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(-c, ui)).str(), test_type(lcm(-c1, ui)).str());
      BOOST_CHECK_EQUAL(mpz_int(gcd(ui, -b)).str(), test_type(gcd(ui, -b1)).str());
      BOOST_CHECK_EQUAL(mpz_int(lcm(ui, -d)).str(), test_type(lcm(ui, -d1)).str());

      if (std::numeric_limits<test_type>::is_modulo && checked::value)
      {
         static mpz_int m = mpz_int(1) << std::numeric_limits<test_type>::digits;
         mpz_int        t(a);
         test_type      t1(a1);
         for (unsigned i = 0; i < 10; ++i)
         {
            t *= a;
            t %= m;
            t += a;
            t %= m;
            t1 *= a1;
            t1 += a1;
         }
         BOOST_CHECK_EQUAL(t.str(), t1.str());
      }
   }

   void t5()
   {
      using namespace boost::multiprecision;
      //
      // Now integer functions:
      //
      mpz_int   z1, z2;
      test_type t1, t2;
      divide_qr(a, b, z1, z2);
      divide_qr(a1, b1, t1, t2);
      BOOST_CHECK_EQUAL(z1.str(), t1.str());
      BOOST_CHECK_EQUAL(z2.str(), t2.str());
      BOOST_CHECK_EQUAL(integer_modulus(a, si), integer_modulus(a1, si));
      BOOST_CHECK_EQUAL(lsb(a), lsb(a1));
      BOOST_CHECK_EQUAL(msb(a), msb(a1));

      for (unsigned i = 0; i < 1000; i += 13)
      {
         BOOST_CHECK_EQUAL(bit_test(a, i), bit_test(a1, i));
      }
      if (!std::numeric_limits<test_type>::is_modulo)
      {
         // We have to take care that our powers don't grow too large, otherwise this takes "forever",
         // also don't test for modulo types, as these may give a different result from arbitrary
         // precision types:
         BOOST_CHECK_EQUAL(mpz_int(pow(d, ui % 19)).str(), test_type(pow(d1, ui % 19)).str());
         BOOST_CHECK_EQUAL(mpz_int(powm(a, b, c)).str(), test_type(powm(a1, b1, c1)).str());
         BOOST_CHECK_EQUAL(mpz_int(powm(a, b, ui)).str(), test_type(powm(a1, b1, ui)).str());
         BOOST_CHECK_EQUAL(mpz_int(powm(a, ui, c)).str(), test_type(powm(a1, ui, c1)).str());
      }
      BOOST_CHECK_EQUAL(lsb(a), lsb(a1));
      BOOST_CHECK_EQUAL(msb(a), msb(a1));
   }

   static void test_bug_cases()
   {
      if (!std::numeric_limits<test_type>::is_bounded)
      {
         // https://svn.boost.org/trac/boost/ticket/7878
         test_type a("0x1000000000000000000000000000000000000000000000000000000000000000");
         test_type b = 0xFFFFFFFF;
         test_type c = a * b + b; // quotient has 1 in the final place
         test_type q, r;
         divide_qr(c, b, q, r);
         BOOST_CHECK_EQUAL(a + 1, q);
         BOOST_CHECK_EQUAL(r, 0);

         b = static_cast<test_type>("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
         c = a * b + b; // quotient has 1 in the final place
         divide_qr(c, b, q, r);
         BOOST_CHECK_EQUAL(a + 1, q);
         BOOST_CHECK_EQUAL(r, 0);
         //
         // Not a bug, but test some other special cases that don't otherwise occur through
         // random testing:
         //
         c = a * b; // quotient has zero in the final place
         divide_qr(c, b, q, r);
         BOOST_CHECK_EQUAL(q, a);
         BOOST_CHECK_EQUAL(r, 0);
         divide_qr(c, a, q, r);
         BOOST_CHECK_EQUAL(q, b);
         BOOST_CHECK_EQUAL(r, 0);
         ++c;
         divide_qr(c, b, q, r);
         BOOST_CHECK_EQUAL(q, a);
         BOOST_CHECK_EQUAL(r, 1);
      }
      // Bug https://svn.boost.org/trac/boost/ticket/8126:
      test_type a("-4294967296");
      test_type b("4294967296");
      test_type c("-1");
      a = (a / b);
      BOOST_CHECK_EQUAL(a, -1);
      a = -4294967296;
      a = (a / b) * c;
      BOOST_CHECK_EQUAL(a, 1);
      a = -23;
      b = 23;
      a = (a / b) * c;
      BOOST_CHECK_EQUAL(a, 1);
      a = -23;
      a = (a / b) / c;
      BOOST_CHECK_EQUAL(a, 1);
      a = test_type("-26607734784073568386365259775");
      b = test_type("8589934592");
      a = a / b;
      BOOST_CHECK_EQUAL(a, test_type("-3097548007973652377"));
      // Bug https://svn.boost.org/trac/boost/ticket/8133:
      a           = test_type("0x12345600012434ffffffffffffffffffffffff");
      unsigned ui = 0xffffffff;
      a           = a - ui;
      BOOST_CHECK_EQUAL(a, test_type("0x12345600012434ffffffffffffffff00000000"));
      a = test_type("0x12345600012434ffffffffffffffffffffffff");
#ifndef BOOST_NO_LONG_LONG
      unsigned long long ull = 0xffffffffffffffffuLL;
      a                      = a - ull;
      BOOST_CHECK_EQUAL(a, test_type("0x12345600012434ffffffff0000000000000000"));
#endif
      //
      // Now check that things which should be zero really are
      // https://svn.boost.org/trac/boost/ticket/8145:
      //
      a = -1;
      a += 1;
      BOOST_CHECK_EQUAL(a, 0);
      a = 1;
      a += -1;
      BOOST_CHECK_EQUAL(a, 0);
      a = -1;
      a += test_type(1);
      BOOST_CHECK_EQUAL(a, 0);
      a = 1;
      a += test_type(-1);
      BOOST_CHECK_EQUAL(a, 0);
      a = test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
      a -= test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
      BOOST_CHECK_EQUAL(a, 0);
      a = -test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
      a += test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
      BOOST_CHECK_EQUAL(a, 0);
      a = 2;
      a *= 0;
      BOOST_CHECK_EQUAL(a, 0);
      a = -2;
      a *= 0;
      BOOST_CHECK_EQUAL(a, 0);
      a = 2;
      a *= test_type(0);
      BOOST_CHECK_EQUAL(a, 0);
      a = -2;
      a *= test_type(0);
      BOOST_CHECK_EQUAL(a, 0);
      a = -2;
      a /= 50;
      BOOST_CHECK_EQUAL(a, 0);
      a = -test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF");
      a /= (1 + test_type("0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF"));
      BOOST_CHECK_EQUAL(a, 0);
      // https://svn.boost.org/trac/boost/ticket/8160
      a = 1;
      a = 0 / test_type(1);
      BOOST_CHECK_EQUAL(a, 0);
      a = 1;
      a = 0 % test_type(25);
      BOOST_CHECK_EQUAL(a, 0);
#ifndef TEST2
      // https://svn.boost.org/trac/boost/ticket/11364
      a           = 0xfffffffeu;
      b           = -2;
      c           = a ^ b;
      test_type d = ~(a ^ ~b);
      BOOST_CHECK_EQUAL(c, d);
#endif
#if defined(TEST2) || defined(TEST3)
      // https://svn.boost.org/trac/boost/ticket/11648
      a = (std::numeric_limits<test_type>::max)() - 69;
      b = a / 139;
      ++b;
      c           = a / b;
      test_type r = a % b;
      BOOST_CHECK(r < b);
      BOOST_CHECK_EQUAL(a - c * b, r);
#endif
      for (ui = 0; ui < 1000; ++ui)
      {
         boost::multiprecision::mpz_int t;
         boost::multiprecision::mpz_int s1 = sqrt(boost::multiprecision::mpz_int(ui), t);
         a                                 = sqrt(test_type(ui), b);
         BOOST_CHECK_EQUAL(a.str(), s1.str());
         BOOST_CHECK_EQUAL(b.str(), t.str());
      }
      a = -1;
      ++a;
      BOOST_CHECK_EQUAL(a, 0);
      ++--a;
      BOOST_CHECK_EQUAL(a, 0);
      --++a;
      BOOST_CHECK_EQUAL(a, 0);

      {
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<> >                                                                                bigint;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<64, 64, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> >   u64;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<128, 128, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> > u128;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<256, 256, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> > u256;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<256, 256, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> >   s256;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<160, 160, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> > u160;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<160, 160, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> >   s160;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<512, 512, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void> > u512;
         typedef boost::multiprecision::number<boost::multiprecision::cpp_int_backend<512, 512, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> >   s512;

         {
            u256   a = 14;
            bigint b = bigint("115792089237316195423570985008687907853269984665640564039457584007913129639948");
            // to fix cast `a` to dev::bigint
            BOOST_CHECK(a < b);
         }
         {
            u256            a      = 1;
            boost::uint64_t amount = 1;
            u256            b      = a << amount;
            BOOST_CHECK_EQUAL(b, 2);

            u256 high_bit = u256(0);
            bit_set(high_bit, 255);
            BOOST_CHECK_EQUAL(a << 255, high_bit);
            BOOST_CHECK_EQUAL(a << boost::uint64_t(256), 0);
            BOOST_CHECK_EQUAL(a << 0, a);

            u256 c = 3;
            BOOST_CHECK_EQUAL(c, 3);
            BOOST_CHECK_EQUAL(c << boost::uint64_t(256), 0);
            BOOST_CHECK_EQUAL(c << 0, c);

            // Bug workaround:
            BOOST_CHECK_EQUAL(static_cast<u256>(bigint(u256(3)) << 255), u256(1) << 255);
         }
         {
            BOOST_CHECK_EQUAL(u256(3) << 255, u256(1) << 255);

            u256            a      = 1;
            boost::uint64_t amount = 1;
            u256            b      = a >> amount;
            BOOST_CHECK_EQUAL(b, 0);
            BOOST_CHECK_EQUAL(a >> 255, 0);
            BOOST_CHECK_EQUAL(a >> boost::uint64_t(256), 0);
            BOOST_CHECK_EQUAL(a >> boost::uint64_t(-1), 0);

            u256 h;
            bit_set(h, 255);
            BOOST_CHECK_EQUAL(h >> 0, u256(1) << 255);
            BOOST_CHECK_EQUAL(h >> 1, u256(1) << 254);
            BOOST_CHECK_EQUAL(h >> 2, u256(1) << 253);
            BOOST_CHECK_EQUAL(h >> 254, u256(1) << 1);
            BOOST_CHECK_EQUAL(h >> 255, u256(1) << 0);
            BOOST_CHECK_EQUAL(h >> 256, 0);
            BOOST_CHECK_EQUAL(h >> boost::uint64_t(-1), 0);

            u256 g;
            bit_set(g, 255);
            bit_set(g, 254);
            BOOST_CHECK_EQUAL(g >> 255, 1);
            BOOST_CHECK_EQUAL(g >> 254, 3);
            BOOST_CHECK_EQUAL(g >> 253, 3 << 1);
            BOOST_CHECK_EQUAL(g >> 252, 3 << 2);
            BOOST_CHECK_EQUAL(g >> 251, 3 << 3);
            BOOST_CHECK_EQUAL(g >> 0, u256(3) << 254);
            BOOST_CHECK_EQUAL(g >> 1, u256(3) << 253);
            BOOST_CHECK_EQUAL(g >> 2, u256(3) << 252);
            BOOST_CHECK_EQUAL(g >> 3, u256(3) << 251);
            BOOST_CHECK_EQUAL(g >> 100, u256(3) << 154);
            BOOST_CHECK_EQUAL(g >> 256, 0);
            BOOST_CHECK_EQUAL(g >> 257, 0);
            BOOST_CHECK_EQUAL(g >> boost::uint32_t(-1), 0);
            BOOST_CHECK_EQUAL(g >> boost::uint64_t(-1), 0);
            BOOST_CHECK_EQUAL(g >> boost::uint16_t(-1), 0);
            BOOST_CHECK_EQUAL(g >> (boost::uint16_t(-1) - 1), 0);
         }
         {
            s256     a      = 1;
            uint64_t amount = 1;
            s256     b      = a >> amount;
            BOOST_CHECK_EQUAL(b, 0);
            BOOST_CHECK_EQUAL(a >> 255, 0);
            BOOST_CHECK_EQUAL(a >> boost::uint64_t(256), 0);
            BOOST_CHECK_EQUAL(a >> boost::uint64_t(-1), 0);

            s256 n = -1;
            BOOST_CHECK_EQUAL(n >> 0, n);
            BOOST_CHECK_EQUAL(n >> 1, n);
            BOOST_CHECK_EQUAL(n >> 2, n);
            BOOST_CHECK_EQUAL(n >> 254, n);
            BOOST_CHECK_EQUAL(n >> 255, n);
            BOOST_CHECK_EQUAL(n >> 256, n);
            BOOST_CHECK_EQUAL(n >> 257, n);
            BOOST_CHECK_EQUAL(n >> ~boost::uint64_t(0), n);

            // Test min value. This actually -(2^256-1), not -(2^255) as in C.
            s256 h = (std::numeric_limits<s256>::min)();
            BOOST_CHECK_LT(h, 0);
            BOOST_CHECK_EQUAL(h >> 0, h);
            BOOST_CHECK_EQUAL(h >> 256, -1);

            // Test EVM min value.
            s256 g = s256(-1) << 255;
            BOOST_CHECK_LT(g, 0);
            BOOST_CHECK_EQUAL(static_cast<u256>(g), u256(1) << 255);
            BOOST_CHECK_EQUAL(g >> 0, g);
            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 1), u256(0b11) << 254);
            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 2), u256(0b111) << 253);
            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 3), u256(0b1111) << 252);

            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 255), ~u256(0));
            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 254), ~u256(0b1));
            BOOST_CHECK_EQUAL(static_cast<u256>(g >> 253), ~u256(0b11));

            // Test shifting more that one bit.
            s256 k = s256(0b111) << 252;
            BOOST_CHECK_EQUAL(k, u256(0b111) << 252);
            BOOST_CHECK_EQUAL(k >> 1, u256(0b111) << 251);
            BOOST_CHECK_EQUAL(k >> 2, u256(0b111) << 250);
            BOOST_CHECK_EQUAL(k >> 252, 0b111);
            BOOST_CHECK_EQUAL(k >> 253, 0b11);
            BOOST_CHECK_EQUAL(k >> 254, 0b1);
            BOOST_CHECK_EQUAL(k >> 255, 0);
            BOOST_CHECK_EQUAL(k >> 256, 0);
            BOOST_CHECK_EQUAL(k >> ~boost::uint32_t(0), 0);

            // Division equivalence.

            // Built-in type:
            if (std::numeric_limits<boost::int64_t>::is_specialized)
            {
               boost::int64_t d = (std::numeric_limits<boost::int64_t>::min)();
               BOOST_CHECK_EQUAL(d >> 1, d / 2);
               int64_t e = d + 1;
               BOOST_CHECK_EQUAL(e >> 1, e / 2 - 1);

               // Boost type:
               BOOST_CHECK_EQUAL(h >> 1, h / 2 - 1);
            }
         }
      }
   }

   void test()
   {
      using namespace boost::multiprecision;

      test_bug_cases();

      last_error_count = 0;

      BOOST_CHECK_EQUAL(Number(), 0);

      for (int i = 0; i < 10000; ++i)
      {
         a = generate_random<mpz_int>(1000);
         b = generate_random<mpz_int>(512);
         c = generate_random<mpz_int>(256);
         d = generate_random<mpz_int>(32);

         si = d.convert_to<int>();
         ui = si;

         a1 = static_cast<test_type>(a.str());
         b1 = static_cast<test_type>(b.str());
         c1 = static_cast<test_type>(c.str());
         d1 = static_cast<test_type>(d.str());

         t1();
         t2();
#ifndef SLOW_COMPILER
         t3();
         t4();
         t5();
#endif

         if (last_error_count != (unsigned)boost::detail::test_errors())
         {
            last_error_count = boost::detail::test_errors();
            std::cout << std::hex << std::showbase;

            std::cout << "a    = " << a << std::endl;
            std::cout << "a1   = " << a1 << std::endl;
            std::cout << "b    = " << b << std::endl;
            std::cout << "b1   = " << b1 << std::endl;
            std::cout << "c    = " << c << std::endl;
            std::cout << "c1   = " << c1 << std::endl;
            std::cout << "d    = " << d << std::endl;
            std::cout << "d1   = " << d1 << std::endl;
            std::cout << "a + b   = " << a + b << std::endl;
            std::cout << "a1 + b1 = " << a1 + b1 << std::endl;
            std::cout << std::dec;
            std::cout << "a - b   = " << a - b << std::endl;
            std::cout << "a1 - b1 = " << a1 - b1 << std::endl;
            std::cout << "-a + b   = " << mpz_int(-a) + b << std::endl;
            std::cout << "-a1 + b1 = " << test_type(-a1) + b1 << std::endl;
            std::cout << "-a - b   = " << mpz_int(-a) - b << std::endl;
            std::cout << "-a1 - b1 = " << test_type(-a1) - b1 << std::endl;
            std::cout << "c*d    = " << c * d << std::endl;
            std::cout << "c1*d1  = " << c1 * d1 << std::endl;
            std::cout << "b*c    = " << b * c << std::endl;
            std::cout << "b1*c1  = " << b1 * c1 << std::endl;
            std::cout << "a/b    = " << a / b << std::endl;
            std::cout << "a1/b1  = " << a1 / b1 << std::endl;
            std::cout << "a/d    = " << a / d << std::endl;
            std::cout << "a1/d1  = " << a1 / d1 << std::endl;
            std::cout << "a%b    = " << a % b << std::endl;
            std::cout << "a1%b1  = " << a1 % b1 << std::endl;
            std::cout << "a%d    = " << a % d << std::endl;
            std::cout << "a1%d1  = " << a1 % d1 << std::endl;
         }

         //
         // Check to see if test is taking too long.
         // Tests run on the compiler farm time out after 300 seconds,
         // so don't get too close to that:
         //
#ifndef CI_SUPPRESS_KNOWN_ISSUES
         if (tim.elapsed() > 200)
#else
         if (tim.elapsed() > 25)
#endif
         {
            std::cout << "Timeout reached, aborting tests now....\n";
            break;
         }
      }
   }
};

int main()
{
   using namespace boost::multiprecision;

#ifdef TEST1
   tester<cpp_int> t1;
   t1.test();
#endif
#ifdef TEST2
   tester<number<cpp_int_backend<2048, 2048, signed_magnitude, checked, void> > > t2;
   t2.test();
#endif
#ifdef TEST3
   // Unchecked test verifies modulo arithmetic:
   tester<number<cpp_int_backend<2048, 2048, signed_magnitude, unchecked, void> > > t3;
   t3.test();
#endif
#ifdef TEST4
   tester<number<cpp_int_backend<0, 2048, signed_magnitude, unchecked, std::allocator<char> > > > t4;
   t4.test();
#endif
#ifdef TEST5
   tester<number<cpp_int_backend<0, 2048, signed_magnitude, unchecked> > > t5;
   t5.test();
#endif
   return boost::report_errors();
}


#define BOOST_TEST_MODULE GCDTest

#include "GCD.hpp"
#include <vector>
#include <cstdint>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(GCDSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnGreatestCommonDivisor)
{
    // Arrange
    vector<int> numbers = { 4, 16, 256 };
    int expected_gcd = 4;

    GCD<int> gcd(numbers);

    // Act
    auto actual_gcd  = gcd.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_gcd, actual_gcd);
}

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnGreatestCommonDivisor_2)
{
    // Arrange
    vector<int> numbers = { 18, 84 };
    int expected_gcd = 6;

    GCD<int> gcd(numbers);

    // Act
    auto actual_gcd  = gcd.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_gcd, actual_gcd);
}

BOOST_AUTO_TEST_CASE(ShouldCorrectlyReturnGreatestCommonDivisor_3)
{
    // Arrange
    vector<int> numbers = { 48, 180 };
    int expected_gcd = 12;

    GCD<int> gcd(numbers);

    // Act
    auto actual_gcd  = gcd.get();

    // Assert
    BOOST_CHECK_EQUAL(expected_gcd, actual_gcd);
}

BOOST_AUTO_TEST_SUITE_END()


#include "numerique.h"
#include "chiffres.h"

#include <boost/multiprecision/integer.hpp>

namespace {
    unsigned short _base(std::ostream &os) {
        auto basefield = os.flags() & std::ios::basefield;
        if (basefield == std::ios::hex) {
            return 16;
        } else if (basefield == std::ios::oct) {
            return 8;
        }
        return 10;
    }
}

namespace std {
    ostream &operator<<(ostream &os, const uint128_t i) {
        std::ostream::sentry s(os);
        if (s) {
            const auto base = _base(os);
            if ((os.flags() & std::ios::showbase) == std::ios::showbase) {
                switch (base) {
                    case 16:
                        os << "0x";
                        break;
                    case 8:
                        os << '0';
                        break;
                    default:
                        break;
                }
            }
            if (i == 0) {
                os << '0';
                return os;
            }
            const char *bases = "0123456789abcdef";
            for (auto c: chiffres::extraire_chiffres(i, base)) {
                os << bases[c];
            }
        }
        return os;
    }

    ostream &operator<<(ostream &os, const int128_t i) {
        if ((os.flags() & ios::basefield) != ios::dec) {
            throw std::domain_error("base != 10");
        }

        if (i >= 0) {
            os << static_cast<uint128_t>(i);
        } else {
            os << '-' << static_cast<uint128_t>(-i);
        }

        return os;
    }

    istream &operator>>(istream &is, uint128_t &i) {
        if ((is.flags() & ios::basefield) != ios::dec) {
            throw std::domain_error("base != 10");
        }

        string s;
        is >> s;

        if (s.front() == '-') {
            throw std::domain_error("non negatif !");
        }

        i = 0;
        for (char chiffre : s) {
            i = 10 * i + static_cast<uint128_t>(chiffre - '0');
        }

        return is;
    }

    istream &operator>>(istream &is, int128_t &i) {
        if ((is.flags() & ios::basefield) != ios::dec) {
            throw std::domain_error("base != 10");
        }

        string s;
        is >> s;

        bool negatif = (s.front() == '-');

        i = 0;
        for (auto it = negatif ? s.begin() + 1 : s.begin(), en = s.end(); it != en; ++it) {
            i = 10 * i + static_cast<int128_t>(*it - '0');
        }

        if (negatif) {
            i *= -1;
        }

        return is;
    }

    string to_string(int128_t n) {
        ostringstream oss;
        oss << n;
        return oss.str();
    }

    string to_string(uint128_t n) {
        ostringstream oss;
        oss << n;
        return oss.str();
    }

    int128_t sqrt(int128_t n) {
        return boost::multiprecision::sqrt(n);
    }

    uint128_t sqrt(uint128_t n) {
        return boost::multiprecision::sqrt(n);
    }
}


#include <utility>
#include <random>
#include "Gain.h"
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Gain.h>
#include <valarray>
#include <set>
#include <iostream>
#include "RNG.h"
#include "Utils.h"

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

Gain::Gain(std::set<double> new_times_amp, std::set<double> new_times_phase) :
    logamp_amp(-3.0),
    logamp_phase(-2.0),
    logscale_amp(7.0),
    logscale_phase(5.0),
    phase_mean(0.0)

    {
    // Get unique times of gain amplitudes
    std::vector<double> times_amp_vec;
    times_amp_vec.assign(new_times_amp.begin(), new_times_amp.end());
    // Get unique times of gain phases
    std::vector<double> times_phase_vec;
    times_phase_vec.assign(new_times_phase.begin(), new_times_phase.end());
    // Convert to Eigen VectorXd
    times_amp = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_amp_vec.data(), times_amp_vec.size());
    times_phase = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_phase_vec.data(), times_phase_vec.size());

    amplitudes = std::valarray<double>(1.0, times_amp.size());
    phases = std::valarray<double>(0.0, times_amp.size());
    v_amp = std::valarray<double>(0.0, times_amp.size());
    v_phase = std::valarray<double>(0.0, times_phase.size());
    C_amp = MatrixXd(times_amp.size(), times_amp.size());
    C_phase = MatrixXd(times_phase.size(), times_phase.size());
    L_amp = MatrixXd(times_amp.size(), times_amp.size());
    L_phase = MatrixXd(times_phase.size(), times_phase.size());

    calculate_C_amp();
    calculate_C_phase();
    calculate_L_amp();
    calculate_L_phase();
    }

Gain::Gain(std::set<double> new_times) :
    logamp_amp(-3.0),
    logamp_phase(-2.0),
    logscale_amp(7.0),
    logscale_phase(5.0),
    phase_mean(0.0)

    {
    // Get unique times of gain amplitude & phase
    std::vector<double> times_vec;
    times_vec.assign(new_times.begin(), new_times.end());
    // Convert to Eigen VectorXd
    times_amp = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_vec.data(), times_vec.size());
    times_phase = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_vec.data(), times_vec.size());

    amplitudes = std::valarray<double>(1.0, times_amp.size());
    phases = std::valarray<double>(0.0, times_amp.size());
    v_amp = std::valarray<double>(0.0, times_amp.size());
    v_phase = std::valarray<double>(0.0, times_phase.size());
    C_amp = MatrixXd(times_amp.size(), times_amp.size());
    C_phase = MatrixXd(times_phase.size(), times_phase.size());
    L_amp = MatrixXd(times_amp.size(), times_amp.size());
    L_phase = MatrixXd(times_phase.size(), times_phase.size());

    calculate_C_amp();
    calculate_C_phase();
    calculate_L_amp();
    calculate_L_phase();
    }

//Gain::Gain(const Gain &other) {
//    logamp_amp = other.logamp_amp;
//    logamp_phase = other.logamp_phase;
//    logscale_amp = other.logscale_amp;
//    logscale_phase = other.logscale_phase;
//    times_amp = other.times_amp;
//    times_phase = other.times_phase;
//    amplitudes =other.amplitudes;
//    phases = other.phases;
//    v_amp = other.v_amp;
//    v_phase = other.v_phase;
//    C_amp = other.C_amp;
//    C_phase = other.C_phase;
//    L_amp = other.L_amp;
//    L_phase = other.L_phase;
//}

void Gain::print_times(std::ostream &out) const
{
    out << "times amp = " << std::endl;
    for (int i=0; i<times_amp.size(); i++) {
        out << times_amp[i] << ", ";
    }
    out << std::endl;

    out << "times phase = " << std::endl;
    for (int i=0; i<times_phase.size(); i++) {
        out << times_phase[i] << ", ";
    }
    out << std::endl;

}

void Gain::print_hp(std::ostream &out) const
{
    out << "For amplitude: logamplitude = " << logamp_amp << ", " << "logscale = " << logscale_amp << std::endl;
    out << "For phase: logamplitude = " << logamp_phase << ", " << "logscale = " << logscale_phase << std::endl;

}

void Gain::print_v(std::ostream &out) const
{
    out << "v amp = " << std::endl;
    for (int i=0; i<v_amp.size(); i++) {
        out << v_amp[i] << ", ";
    }
    out << std::endl;

    out << "v_phase = " << std::endl;
    for (int i=0; i<v_phase.size(); i++) {
        out << v_phase[i] << ", ";
    }
    out << std::endl;
}

void Gain::print_C(std::ostream &out) const
{
    out << "C amp = " << std::endl;
    out << C_amp << std::endl;
    out << "C phase = " << std::endl;
    out << C_phase << std::endl;
}

void Gain::print_L(std::ostream &out) const
{
    out << "L amp = " << std::endl;
    out << L_amp << std::endl;
    out << "L phase= " << std::endl;
    out << L_phase << std::endl;
}

void Gain::set_hp_amp(std::valarray<double> params) {
    logamp_amp = params[0];
    logscale_amp = params[1];
}

void Gain::set_hp_phase(std::valarray<double> params) {
    logamp_phase = params[0];
    logscale_phase = params[1];
}

//void Gain::set_v_amp(std::valarray<double> params) {
//    v_amp = std::move(params);
//}
//
//
//void Gain::set_v_phase(std::valarray<double> params) {
//    v_phase = std::move(params);
//}

void Gain::from_prior_v_amp() {
    //v_amp = make_normal_random(size_amp());
    v_amp = std::valarray<double> (0.0, size_amp());
}

void Gain::from_prior_v_phase() {
    //v_phase = make_normal_random(size_phase());
    v_phase = std::valarray<double> (0.0, size_phase());
}

void Gain::from_prior_phase_mean(DNest4::RNG& rng) {
    //phase_mean = -1.0*M_PI + 2.0*M_PI*rng.rand();
    phase_mean = 0.0;
}

void Gain::from_prior_hp_amp(DNest4::RNG& rng) {
    //std::cout << "Generating from prior Gain HP AMP" << std::endl;
    //logamp_amp = -3.0 + 1.0*rng.randn();
    //logscale_amp = 7.0 + 1.0*rng.randn();
    logamp_amp = -3.0;
    logscale_amp = 7.0;
}

void Gain::from_prior_hp_phase(DNest4::RNG& rng) {
    //logamp_phase = -3.0 + 1.0*rng.randn();
    //logscale_phase = 5.0 + 1.0*rng.randn();
    logamp_phase = -2.0;
    logscale_phase = 5.0;
}

void Gain::calculate_C_amp() {
    //std::cout << "Calculating C of amp GP" << std::endl;
    Eigen::MatrixXd sqdist = - 2*times_amp*times_amp.transpose();
    sqdist.rowwise() += times_amp.array().square().transpose().matrix();
    sqdist.colwise() += times_amp.array().square().matrix();
    sqdist *= (-0.5/(exp(2.0*logscale_amp)));
    C_amp = exp(logamp_amp) * sqdist.array().exp();
}

void Gain::calculate_C_phase() {
    Eigen::MatrixXd sqdist = - 2*times_phase*times_phase.transpose();
    sqdist.rowwise() += times_phase.array().square().transpose().matrix();
    sqdist.colwise() += times_phase.array().square().matrix();
    sqdist *= (-0.5/(exp(2.0*logscale_phase)));
    C_phase = exp(logamp_phase) * sqdist.array().exp();
}

void Gain::calculate_L_amp() {
    //std::cout << "Calculating L of amp GP" << std::endl;
    // perform the Cholesky decomposition of covariance matrix
    Eigen::LLT<Eigen::MatrixXd> cholesky = C_amp.llt();
    // get the lower triangular matrix L
    L_amp = cholesky.matrixL();
}

void Gain::calculate_L_phase() {
    // perform the Cholesky decomposition of covariance matrix
    Eigen::LLT<Eigen::MatrixXd> cholesky = C_phase.llt();
    // get the lower triangular matrix L
    L_phase = cholesky.matrixL();
}

int Gain::size_amp() {
    return times_amp.size();
}

int Gain::size_phase() {
    return times_phase.size();
}

std::valarray<double>& Gain::get_amplitudes() {
    return amplitudes;
}

std::valarray<double>& Gain::get_phases() {
    return phases;
}

void Gain::calculate_amplitudes() {
    //std::cout << "Calculating amplitudes" << std::endl;
    // Convert std::valarray ``v_amp`` to Eigen::VectorXd
    VectorXd v_amp_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&v_amp[0], v_amp.size());

    //// Debug print out v
    //std::valarray<double> v_amp = std::valarray<double>(v_amp_vec.data(), v_amp_vec.size());
    ////std::cout << "V amplitudes = " << std::endl;
    //for (int i=0; i < times_amp.size(); i++) {
    //    std::cout << v_amp[i] << ", ";
    //}
    //std::cout << std::endl;

    VectorXd amp = L_amp*v_amp_vec;
    // Convert Eigen::VectorXd to std::valarray
    amplitudes = 1.0 + std::valarray<double>(amp.data(), amp.size());
    //std::cout << "Calculated amplitudes: " << std::endl;
    //print_amplitudes(std::cout);

}

void Gain::calculate_phases() {
    // Convert std::valarray ``v_amp`` to Eigen::VectorXd
    VectorXd v_phase_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&v_phase[0], v_phase.size());

    VectorXd phase = L_phase*v_phase_vec;

    //// Debug print out v
    //std::valarray<double> v_phases = std::valarray<double>(v_phase_vec.data(), v_phase_vec.size());
    //std::cout << "V phases = " << std::endl;
    //for (int i=0; i < times_phase.size(); i++) {
    //    std::cout << v_phases[i] << ", ";
    //}
    //std::cout << std::endl;

    // Convert Eigen::VectorXd to std::valarray
    phases = phase_mean + std::valarray<double>(phase.data(), phase.size());
    //std::cout << "Calculated phases: " << std::endl;
    //print_phases(std::cout);

}

void Gain::print_amplitudes(std::ostream &out) const {
    out << "amplitudes = " << std::endl;
    for (int i=0; i < times_amp.size(); i++) {
        out << amplitudes[i] << ", ";
    }
    out << std::endl;
}

void Gain::print_phases(std::ostream &out) const {
    out << "phases = " << std::endl;
    for (int i=0; i < times_phase.size(); i++) {
        out << phases[i] << ", ";
    }
    out << std::endl;
}

double Gain::perturb(DNest4::RNG &rng) {
    double logH = 0.;

    int which = rng.rand_int(2);
    // Amplitude GP
    if(which == 0) {

        // Choose what value to perturb
        int which = rng.rand_int(amplitudes.size());

        logH -= -0.5*pow(v_amp[which], 2.0);
        v_amp[which] += rng.randn();
        logH += -0.5*pow(v_amp[which], 2.0);

        // Pre-reject
        if(rng.rand() >= exp(logH)) {
            return -1E300;
        }
        else
            logH = 0.0;

        calculate_amplitudes();
    }
    // Phase GP
    else {

        // More often perturb phase latent variables
        if(rng.rand() <= 0.9) {

            // Choose what value to perturb
            int which = rng.rand_int(phases.size());

            logH -= -0.5*pow(v_phase[which], 2.0);
            v_phase[which] += rng.randn();
            logH += -0.5*pow(v_phase[which], 2.0);

            // Pre-reject
            if (rng.rand() >= exp(logH)) {
                return -1E300;
            } else
                logH = 0.0;

        }
        else {
            //logH -= -0.5*pow(phase_mean/1.5, 2.0);
            phase_mean += 2.0*M_PI*rng.randh();
            //logH += -0.5*pow(phase_mean/1.5, 2.0);
            DNest4::wrap(phase_mean, -1.0*M_PI, M_PI);

            //// Pre-reject
            //if (rng.rand() >= exp(logH)) {
            //    return -1E300;
            //} else
            //    logH = 0.0;
        }

        calculate_phases();

    }

    return logH;
}

std::string Gain::description() const {
    std::string descr;
    for (int i = 0; i < times_amp.size(); i++) {
        descr += ("amp" + std::to_string(i) + " ");
    }
    descr += "phase_mean ";
    for (int i = 0; i < times_phase.size(); i++) {
        descr += ("phase" + std::to_string(i) + " ");
    }
    descr.pop_back();
    return descr;
}

void Gain::print(std::ostream &out) const {
    for (int i = 0; i < times_amp.size(); i++) {
        out << amplitudes[i] << '\t';
    }
    out << phase_mean << '\t';
    for (int i = 0; i < times_phase.size(); i++) {
        out << phases[i] << '\t';
    }
}

std::valarray<double> make_normal_random(int number)
{
    std::random_device rd;
    std::normal_distribution<double> normalDistr(0, 1);
    std::minstd_rand generator(rd());
    std::valarray<double> randNums(number);

    for (int i=0; i<number; i++) {
        randNums[i] = normalDistr(generator);
    }
    return randNums;
}