#define PY_ARRAY_UNIQUE_SYMBOL superimg_PyArray_API
#define NO_IMPORT_ARRAY

#include <string>
#include <cmath>

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include <boost/array.hpp>

#include <boost/accumulators/accumulators.hpp>

#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/extended_p_square_quantile.hpp>
#include <boost/accumulators/statistics/tail_quantile.hpp>

#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/cgp2d/cgp2d.hxx"
#include "seglib/cgp2d/cgp2d_python.hxx"
#include "seglib/distances/distance.hxx"

namespace python = boost::python;

namespace cgp2d {

    // tgrid and input image type
    typedef Cgp<CoordinateType,LabelType> CgpType;
    typedef CgpType::TopologicalGridType TopologicalGridType;

    void mergeFeatures(
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<1,float>              weights,
        vigra::NumpyArray<2,float>              features,
        vigra::NumpyArray<2,float>              mergedFeatures,
        vigra::NumpyArray<1,float>              weightBuffer
    ){
        CGP_ASSERT_OP(labeling.shape(0),==,features.shape(0));
        CGP_ASSERT_OP(labeling.shape(0),==,mergedFeatures.shape(0));
        CGP_ASSERT_OP(features.shape(1),==,mergedFeatures.shape(1));
        CGP_ASSERT_OP(weightBuffer.shape(0),==,features.shape(0));

        // initalize with zeros
        std::fill(mergedFeatures.begin(),mergedFeatures.begin()+numberOfLabels,0.0);
        std::fill(weightBuffer.begin(),weightBuffer.begin()+numberOfLabels,0.0);

        const size_t nItems = labeling.shape(0);
        const size_t nFeatures = features.shape(1);

        // accumulate
        for(size_t i=0;i<nItems;++i){

            // get the label
            const size_t label = labeling(i);
            // get the weight
            const float weight = weights(i);
            // accumulate features
            for(size_t f=0;f<nFeatures;++f){
                mergedFeatures(label,f)+=weight*features(i,f);
            }
            // accumulate weights
            weightBuffer(i)+=weight;
        }
        // normalize

        for(size_t label=0;label<numberOfLabels;++label){
            const float weight=weights(label);
            for(size_t f=0;f<nFeatures;++f){
                mergedFeatures(label,f)/=weight;
            }
        }

    }

    float withinClusterDist(
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<1,float>              weights,
        vigra::NumpyArray<2,float>              features,
        vigra::NumpyArray<2,float>              mergedFeatures
    ){
        CGP_ASSERT_OP(labeling.shape(0),==,features.shape(0));
        CGP_ASSERT_OP(labeling.shape(0),==,mergedFeatures.shape(0));
        CGP_ASSERT_OP(features.shape(1),==,mergedFeatures.shape(1));

        const size_t nItems = labeling.shape(0);
        const size_t nFeatures = features.shape(1);

        float totalD = 0.0;

        for(size_t i=0;i<nItems;++i){

            // get the label
            const size_t label = labeling(i);

            // compute the distance between the feature of the item (superpixel)
            // and the feature of the cluster (cluster of superpixel / "HyperRegion")
            vigra::MultiArrayView<1,float> a=features.bindInner(i);
            vigra::MultiArrayView<1,float> b=mergedFeatures.bindInner(label);
            const  float d = distances::Distance<float>::klDivergenz(a.begin(),a.end(),b.begin(),b.end());
            // get the weight of the item (superpixel size)
            const float weight=weights(i);

            totalD += weight*d;

            //weightBuffer(l)+=weight
        }
        return totalD;
    }

    float betweenClusterDist(
        const Cgp<CoordinateType,LabelType> &   cgp,
        vigra::NumpyArray<1,LabelType>          labeling,
        const size_t                            numberOfLabels,
        vigra::NumpyArray<2,float>              mergedFeatures
    ){

        float totalD=0.0f;

        std::set<size_t> used;
        const size_t nBoundaries = cgp.numCells(1);
        for(size_t b=0;b<nBoundaries;++b){
            const size_t r1 = cgp.bound<1>(b,0)-1;
            const size_t r2 = cgp.bound<1>(b,1)-1;

            size_t l1 =labeling(r1);
            size_t l2 =labeling(r2);

            // active boundarie ?
            if(l1!=l2){

                if(l2<l1){
                    std::swap(l1,l2);
                }
                const size_t key = l1+ l2*numberOfLabels;
                if(used.find(key)==used.end()){
                    used.insert(key);
                    vigra::MultiArrayView<1,float> a=mergedFeatures.bindInner(l1);
                    vigra::MultiArrayView<1,float> b=mergedFeatures.bindInner(l2);
                    const float d = distances::Distance<float>::klDivergenz(a.begin(),a.end(),b.begin(),b.end());
                    totalD+=d;
                }
            }
        }
        return totalD;
    }

    void export_merge(){

        python::def("_mergeFeatures",vigra::registerConverters(&mergeFeatures),
            (
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("weights"),
                python::arg("features"),
                python::arg("mergedFeatures"),
                python::arg("weightBuffer")
            )
        );

        python::def("_withinClusterDist",vigra::registerConverters(&withinClusterDist),
            (
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("weights"),
                python::arg("features"),
                python::arg("mergedFeatures")
            )
        );

        python::def("_betweenClusterDist",vigra::registerConverters(&betweenClusterDist),
            (
                python::arg("cgp"),
                python::arg("labeling"),
                python::arg("numberOfLabels"),
                python::arg("mergedFeatures")
            )
        );

    }

}


#include <boost/test/unit_test.hpp>

#include <fc/crypto/elliptic.hpp>
#include <fc/log/logger.hpp>
#include <fc/io/raw.hpp>
#include <fc/variant.hpp>
#include <fc/reflect/variant.hpp>

BOOST_AUTO_TEST_SUITE(fc_crypto)

BOOST_AUTO_TEST_CASE(blind_test)
{
   try {
      auto InB1 = fc::sha256::hash("InB1");
      auto InB2 = fc::sha256::hash("InB2");
      auto OutB1 = fc::sha256::hash("OutB1");

      auto InC1 = fc::ecc::blind(InB1,25);
      auto InC2 = fc::ecc::blind(InB2,75);

      auto OutC1 = fc::ecc::blind(OutB1,40);

      auto OutB2 = fc::ecc::blind_sum( {InB1,InB2,OutB1}, 2 );
      auto OutC2 = fc::ecc::blind( OutB2, 60 );

      /*
      FC_ASSERT( fc::ecc::verify_sum( {},{InC1,InC2},  100 ) );
      FC_ASSERT( fc::ecc::verify_sum( {InC1,InC2}, {}, -100 ) );
      */

      //FC_ASSERT( fc::ecc::verify_sum( {InC1,InC2}, {OutC1}, -60 ) );

      BOOST_CHECK( fc::ecc::verify_sum( {InC1,InC2}, {OutC1,OutC2}, 0 ) );
      auto nonce = fc::sha256::hash("nonce");

      auto proof = fc::ecc::range_proof_sign( 0, OutC1, OutB1, nonce, 0, 0, 40 );
//      wdump( (proof.size()));

      auto result = fc::ecc::range_get_info( proof );
//      wdump((result));
      BOOST_CHECK( result.max_value >= 60 );
      BOOST_CHECK( result.min_value >= 0 );

      auto B1 = fc::sha256::hash("B1");
      auto B2 = fc::sha256::hash("B2");
      auto b3 = fc::sha256::hash("b3");
      auto B4 = fc::sha256::hash("B4");
      auto C1 = fc::ecc::blind( B1, 1  );
      auto C2 = fc::ecc::blind( B2, 2  );
      /*auto c3 = */fc::ecc::blind( b3, 3  );
      /*auto C4 = */fc::ecc::blind( B4, -1 );

      auto B3 = fc::ecc::blind_sum( {B1,B2}, 2 );
      auto C3 = fc::ecc::blind( B3, 3 );

      auto B2m1 = fc::ecc::blind_sum( {B2,B1}, 1 );
      /*auto C2m1 = */fc::ecc::blind( B2m1, 1 );

      BOOST_CHECK( fc::ecc::verify_sum( {C1,C2}, {C3}, 0 ) );
      BOOST_CHECK( fc::ecc::verify_sum( {C1,C2}, {C3}, 0 ) );
      BOOST_CHECK( fc::ecc::verify_sum( {C3}, {C1,C2}, 0 ) );
      BOOST_CHECK( fc::ecc::verify_sum( {C3}, {C1,C2}, 0 ) );

      {
         auto B1 = fc::sha256::hash("B1");
         /*auto B2 = */fc::sha256::hash("B2");
         /*auto B3 = */fc::sha256::hash("B3");
         /*auto B4 = */fc::sha256::hash("B4");

         //secp256k1_scalar_get_b32((unsigned char*)&B1, (const secp256k1_scalar_t*)&B2);
         //B1 = fc::variant("b2e5da56ef9f2a34d3e22fd12634bc99261e95c87b9960bf94ed3d27b30").as<fc::sha256>();

         auto C1 = fc::ecc::blind( B1, INT64_MAX );
         auto C2 = fc::ecc::blind( B1, 0 );
         auto C3 = fc::ecc::blind( B1, 1 );
         /*auto C4 = */fc::ecc::blind( B1, 2 );

         BOOST_CHECK( fc::ecc::verify_sum( {C2}, {C3}, -1 ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C1}, {C1}, 0 ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C2}, {C2}, 0 ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C3}, {C2}, 1 ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C1}, {C2}, INT64_MAX ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C1}, {C2}, INT64_MAX ) );
         BOOST_CHECK( fc::ecc::verify_sum( {C2}, {C1}, -INT64_MAX ) );
      }

      {
         auto Out1 = fc::sha256::hash("B1");
         auto Out2 = fc::sha256::hash("B2");
         auto OutC1 = fc::ecc::blind( Out1, 250 );
         auto OutC2 = fc::ecc::blind( Out2, 750 );
         auto InBlind = fc::ecc::blind_sum( {Out1,Out2}, 2 );
         auto InC  = fc::ecc::blind( InBlind, 1000 );
         auto In0  = fc::ecc::blind( InBlind, 0 );

         BOOST_CHECK( fc::ecc::verify_sum( {InC}, {OutC1,OutC2}, 0 ) );
         BOOST_CHECK( fc::ecc::verify_sum( {InC}, {In0}, 1000 ) );

      }
   }
   catch ( const fc::exception& e )
   {
      edump((e.to_detail_string()));
   }
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <iostream>
#include <stdio.h>
#include <sys/ioctl.h>
#include <limits.h>
#include <assert.h>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/distributions/pareto.hpp>
#include <boost/random.hpp>
#include <algorithm>

using namespace std;
#define B * 1L
#define KB * (1024 B)
#define MB * (1024 KB)
#define GB * (1024 MB)
#define file_size_bytes (4 GB)
#define io_size (4 KB)
#define height 20

#define ALIGN_MASK(x, mask) (((x) + (mask)) & ~(mask))
#define ALIGN(x, a)  ALIGN_MASK((x), ((__typeof__(x))(a) - 1))

void print_dist(const vector<uint64_t> &list) {
   struct winsize w;
   ioctl(STDOUT_FILENO, TIOCGWINSZ, &w);

   vector<int> buckets(w.ws_col, 0);
   int upper = *(max_element(list.begin(), list.end()));
   int divisor = upper / (buckets.size() - 1);

   cout << "Bucket divisor: " << divisor << endl;
   for (const auto &i : list) {
      buckets[i/divisor] += 1;
   }
   cout << "first bucket: " << buckets[0] << endl;

   upper = *(max_element(buckets.begin(), buckets.end()));
   divisor = upper / (height - 1);

   for (auto &i : buckets)
      i = i/divisor;

   for (int i = height; i > 0;  i--) {
      for (const auto &j : buckets) {
         if (j >= i)
            cout << "*";
         else
            cout << " ";
      }
      cout << endl;
   }

   for (int i=0; i < w.ws_col; i++)
      cout << "_";
   cout << endl;
}

int main(int argc, char** argv)
{
   vector<uint64_t> io_list;
   boost::mt19937 rg;
   boost::math::pareto_distribution<> dist;
   boost::random::uniform_real_distribution<> uniformReal(1.0,10.0);
   //rg.seed(time(NULL));
   boost::variate_generator<boost::mt19937&,
   boost::random::uniform_real_distribution<> > generator(rg, uniformReal);

   double value;
   for (uint64_t i = 0; i < file_size_bytes / io_size; i++)  {
      //cout << (int)(file_size_bytes * boost::math::pdf(dist, uniformReal(rg))) << endl;
      value = boost::math::pdf(dist,generator());
      cout << (uint64_t)(file_size_bytes * value) << endl;
      io_list.push_back(ALIGN((uint64_t)(file_size_bytes * value), (4 << 10)));
   }
   //print_dist(io_list);
}


#include "scan_calculator.h"
#include <graycode.h>
#include <multiview.h>
#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <boost/thread.hpp>

void ScanCalculator::ScanWorker(int threadId, int startRow, int endRow)
{
	MultiView stereoView;
	stereoView.ReadRawCal("rawcal.ini");

	std::vector<cv::Point2f> p;
	std::vector<cv::Point3f> lines;
	for(int r = startRow; r < endRow; r++)//for(int r = 1; r < size.height-5; r++)
	{
		for(int c = 1; c < 1290; c++)//for(int c = 1; c < size.width-5; c++)
		{
			p.push_back(cv::Point2f(c, r));
		}
	}
	cv::computeCorrespondEpilines(p, 1, F, lines);
	//std::cout << "size of lines: "<< lines.size() << std::endl;

	cv::Mat cam0pnts(1,1,CV_64FC2);
	cv::Mat cam1pnts(1,1,CV_64FC2);

	//find correspondence and triangulate
	T2DLINED l;
	int counter = 0;
	for(int r = startRow; r < endRow; r++)
	{
		//std::cout << "thread " << threadId << " : " << r << std::endl;
		for(int c = 1; c < 1290; c++)//for(int c = 1; c < size.width-5; c++)
		{
			cv::Mat pnts3D = cv::Mat(1,1,CV_64FC4);

			l.p0.x = 0;
			l.p0.y = -(lines[counter].z/lines[counter].y);
			double m = -(lines[counter].x / lines[counter].y);
			double angle = atan(m);
			l.vr.x = 1250*cos(angle);
			l.vr.y = 1250*sin(angle);

			double _U=0, _V=0;
			double srcW = BLInterpolate(p[counter].x, p[counter].y, absL);
			counter++;

			if(!stereoView.DetectPtOnEplipolarLine(l, srcW, absR, p[counter-1].x, p[counter-1].y, _U, _V, 2, PP2))
			{
				continue;
			}

			cam0pnts.at<cv::Point2d>(0) = p[counter-1];
			cam1pnts.at<cv::Point2d>(0) = cv::Point2d(_U, _V);

			cv::triangulatePoints(PP1,PP2,cam0pnts,cam1pnts,pnts3D);
			pnts3D = pnts3D / pnts3D.at<double>(3);

			clouds[threadId].push_back(pnts3D);
		}
	}
}

void LoadAbsolutePhase(const char* filename, cv::Mat& absPhase)
{
    cv::Mat dummy = cv::imread(filename, CV_16U);
    short result = 0;

    for(int r = 0; r < absPhase.rows; r++)
    {
        for(int c = 0; c < absPhase.cols; c++)
        {
            unsigned short val = dummy.at<unsigned short>(cv::Point2i(c,r));

            result = val;
            if(val > 32767)
                result = val - 65536;

            absPhase.at<short>(cv::Point2i(c,r)) = result;
        }
    }
}

ScanCalculator::ScanCalculator()
{
}

ScanCalculator::~ScanCalculator()
{

}

bool ScanCalculator::StartCalculation(std::vector<std::vector<cv::Mat> >& sequence, CalibrationResult& c_result)
{
	int height = sequence[0][0].rows;
	int width = sequence[0][0].cols;
    cv::Size size = cv::Size(sequence[0][0].cols, sequence[0][0].rows);
    CalculateAbsPhase(sequence);

	absL = cv::Mat(height, width, CV_16S);
    absR = cv::Mat(height, width, CV_16S);
    LoadAbsolutePhase("absL.tiff", absL);
    LoadAbsolutePhase("absR.tiff", absR);

    cv::stereoRectify(c_result.K[0], c_result.D[0], c_result.K[1], c_result.D[1], size, c_result.R, c_result.T, R1, R2, P1, P2, Q);
	F = c_result.F;

    //P = [K*R -(K*R)*C];
    cv::Mat exterior = cv::Mat::zeros(4,4,CV_64F);
    cv::Mat middle = cv::Mat::eye(3,4,CV_64F);
    //std::cout << middle << std::endl;

    PP1 = cv::Mat::zeros(3,4,CV_64F);
    cv::Mat rotmat;
    cv::Rodrigues(c_result.rvecs[0][0], rotmat);
    //rotmat = rotmat.inv();
    exterior.at<double>(0,0) = rotmat.at<double>(0,0);
    exterior.at<double>(0,1) = rotmat.at<double>(0,1);
    exterior.at<double>(0,2) = rotmat.at<double>(0,2);
    exterior.at<double>(1,0) = rotmat.at<double>(1,0);
    exterior.at<double>(1,1) = rotmat.at<double>(1,1);
    exterior.at<double>(1,2) = rotmat.at<double>(1,2);
    exterior.at<double>(2,0) = rotmat.at<double>(2,0);
    exterior.at<double>(2,1) = rotmat.at<double>(2,1);
    exterior.at<double>(2,2) = rotmat.at<double>(2,2);
    exterior.at<double>(0,3) = c_result.tvecs[0][0].at<double>(0);
    exterior.at<double>(1,3) = c_result.tvecs[0][0].at<double>(1);
    exterior.at<double>(2,3) = c_result.tvecs[0][0].at<double>(2);
    exterior.at<double>(3,0) = 0;
    exterior.at<double>(3,1) = 0;
    exterior.at<double>(3,2) = 0;
    exterior.at<double>(3,3) = 1;
    PP1 = c_result.K[0] * middle * exterior;
    PP1 = PP1 / PP1.at<double>(2,3);
    std::cout << "P hesaplanan 1" << PP1 << std::endl;

    PP2 = cv::Mat::zeros(3,4,CV_64F);
    cv::Mat rotmat2;
    cv::Rodrigues(c_result.rvecs[1][0], rotmat2);
    exterior.at<double>(0,0) = rotmat2.at<double>(0,0);
    exterior.at<double>(0,1) = rotmat2.at<double>(0,1);
    exterior.at<double>(0,2) = rotmat2.at<double>(0,2);
    exterior.at<double>(1,0) = rotmat2.at<double>(1,0);
    exterior.at<double>(1,1) = rotmat2.at<double>(1,1);
    exterior.at<double>(1,2) = rotmat2.at<double>(1,2);
    exterior.at<double>(2,0) = rotmat2.at<double>(2,0);
    exterior.at<double>(2,1) = rotmat2.at<double>(2,1);
    exterior.at<double>(2,2) = rotmat2.at<double>(2,2);
    exterior.at<double>(0,3) = c_result.tvecs[1][0].at<double>(0);
    exterior.at<double>(1,3) = c_result.tvecs[1][0].at<double>(1);
    exterior.at<double>(2,3) = c_result.tvecs[1][0].at<double>(2);
    exterior.at<double>(3,0) = 0;
    exterior.at<double>(3,1) = 0;
    exterior.at<double>(3,2) = 0;
    exterior.at<double>(3,3) = 1;
    PP2 = c_result.K[1] * middle * exterior;
    PP2 = PP2 / PP2.at<double>(2,3);
    std::cout << "P hesaplanan 2" << PP2 << std::endl;

	boost::thread_group tg;
	int thread_amount = 64;
	clouds.resize(thread_amount);
	int step = 960 / thread_amount;
	for(int i = 0; i<thread_amount; i++)
	{
		tg.create_thread(boost::bind(&ScanCalculator::ScanWorker, this, i, (i*step)+1, (i+1)*step ));
	}

	int t0 = time(NULL);
	tg.join_all();
	int t1 = time(NULL);
	printf ("time = %d secs\n", t1 - t0);

	std::ofstream file("out.txt");
	for(int i = 0; i < clouds.size(); i++)
	{
		m_ptCloud.insert(m_ptCloud.end(), clouds[i].begin(), clouds[i].end());
	}

    return true;
}

std::vector<cv::Mat>* ScanCalculator::GetCloud()
{
    return& m_ptCloud;
}

void ScanCalculator::CalculateAbsPhase(std::vector<std::vector<cv::Mat> >& sequence)
{
    int height = sequence[0][0].rows;
    int width = sequence[0][0].cols;
    cv::Mat absPhaseL = cv::Mat::zeros( height, width, CV_16U );
    cv::Mat absPhaseR = cv::Mat::zeros( height, width, CV_16U );
    CalculateGP(absPhaseL, sequence[0], 1, 16, 17);
    cv::imwrite("absL.tiff", absPhaseL);
    CalculateGP(absPhaseR, sequence[1], 1, 16, 17);
    cv::imwrite("absR.tiff", absPhaseR);
}


// CPP program to find factorial of large
// number using boost library.
#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using boost::multiprecision::cpp_int;
using namespace std;

cpp_int Factorial(int number)
{
    cpp_int num = 1;
    for (int i = 1; i <= number; i++)
        num = num * i;
    return num;
}

int main()
{
    int number = 100;
    cpp_int fact = Factorial(number);
    cout << fact << endl;
    return 0;
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
//#include "../../point_to_plane_tait_bryan_wc_jacobian.h"
//#include "../../point_to_plane_rodrigues_wc_jacobian.h"
//#include "../../point_to_plane_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"
#include "../include/cauchy.h"
#include "../../distance_point_to_plane_tait_bryan_wc_jacobian.h"
#include "../../distance_point_to_plane_rodrigues_wc_jacobian.h"
#include "../../distance_point_to_plane_quaternion_wc_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<Eigen::Affine3d> trajectory;
std::vector<Eigen::Affine3d> planes_global;
std::vector<std::vector<Eigen::Affine3d>> planes_local;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	TaitBryanPose pose;
	pose.px = -40;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = -20;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = -0;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = 20;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	pose.px = 40;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));

	for(size_t i = 0 ; i < 100; i++){
		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 10;
		planes_global.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}

	planes_local.resize(trajectory.size());
	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d m_inv = trajectory[i].inverse();

		for(size_t j = 0 ; j < planes_global.size(); j++){
			Eigen::Affine3d m_line_local = m_inv * planes_global[j];
			planes_local[i].push_back(m_line_local);
		}
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("distance_point_to_plane");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();

	for(size_t i = 0 ; i < planes_local.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		for(size_t j = 0 ; j < planes_local[i].size(); j++){
			Eigen::Affine3d m_line_global = m * planes_local[i][j];

			glBegin(GL_LINES);
				glColor3f(1.0f, 0.0f, 0.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,0), m_line_global(1,3) + m_line_global(1,0), m_line_global(2,3) + m_line_global(2,0));

				glColor3f(0.0f, 1.0f, 0.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,1), m_line_global(1,3) + m_line_global(1,1), m_line_global(2,3) + m_line_global(2,1));

				glColor3f(0.0f, 0.0f, 1.0f);
				glVertex3f(m_line_global(0,3), m_line_global(1,3), m_line_global(2,3));
				glVertex3f(m_line_global(0,3) + m_line_global(0,2), m_line_global(1,3) + m_line_global(1,2), m_line_global(2,3) + m_line_global(2,2));
			glEnd();
		}
	}

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
			for(size_t i = 0 ; i  < trajectory.size(); i++){
				TaitBryanPose pose;
				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				trajectory[i] = trajectory[i] * affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_tait_bryan_wc(delta,
									pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 6> delta_jacobian;
							delta_distance_point_to_plane_tait_bryan_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 6; jj++){
									int ic = i * 6;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 6, trajectory.size() * 6);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];

					trajectory[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'r':{
			for(size_t i = 0; i < trajectory.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(trajectory[i]);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.0000001;
				trajectory[i] = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						RodriguesPose pose = pose_rodrigues_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_rodrigues_wc(delta,
									pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 6> delta_jacobian;
							delta_distance_point_to_plane_rodrigues_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.sx, pose.sy, pose.sz,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 6; jj++){
									int ic = i * 6;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 6, trajectory.size() * 6);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 6, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 6){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];

					trajectory[i] = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < planes_local.size(); i++){
				for(size_t j = 0 ; j < planes_local.size(); j++){
					if(i != j){
						QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);

						for(size_t k = 0 ; k < planes_local[i].size(); k++){
							Eigen::Affine3d plane_target_global = trajectory[j] * planes_local[j][k];

							double a,b,c,d;
							a = plane_target_global(0,2);
							b = plane_target_global(1,2);
							c = plane_target_global(2,2);
							d = -a * plane_target_global(0,3) - b * plane_target_global(1,3) - c * plane_target_global(2,3);

							Eigen::Vector3d point_source_local(planes_local[i][k](0,3), planes_local[i][k](1,3), planes_local[i][k](2,3));
							Eigen::Matrix<double, 1, 1> delta;
							delta_distance_point_to_plane_quaternion_wc(delta,
									pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							Eigen::Matrix<double, 1, 7> delta_jacobian;
							delta_distance_point_to_plane_quaternion_wc_jacobian(delta_jacobian,
									pose.px, pose.py, pose.pz, pose.q0, pose.q1, pose.q2, pose.q3,
									point_source_local.x(), point_source_local.y(), point_source_local.z(),
									a, b, c, d);

							int ir = tripletListB.size();

							for(int ii = 0; ii < 1; ii++){
								for(int jj = 0; jj < 7; jj++){
									int ic = i * 7;
									if(delta_jacobian(ii,jj) != 0.0){
										tripletListA.emplace_back(ir + ii, ic + jj , -delta_jacobian(ii,jj));
									}
								}
							}

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta(0,0), 1));

							tripletListB.emplace_back(ir    , 0,  delta(0,0));
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);
			tripletListA.emplace_back(ir + 6 , 6, 1);

			tripletListP.emplace_back(ir     , ir,     10000000000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 10000000000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 10000000000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 10000000000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 10000000000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 10000000000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 10000000000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);
			tripletListB.emplace_back(ir + 6 , 0, 0);

			for(size_t i = 0 ; i < trajectory.size(); i++){
				int ic = i * 7;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 7, trajectory.size() * 7);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];
					trajectory[i] = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to trajectory" << std::endl;
	std::cout << "t: optimize Tait-Bryan" << std::endl;
	std::cout << "r: optimize Rodrigues" << std::endl;
	std::cout << "q: optimize Quaternion" << std::endl;
}


#include <CppADCodeGenEigenPy/ADModel.h>
#include <Eigen/Eigen>

namespace ad = CppADCodeGenEigenPy;

// Our custom model extends the ad::ADModel class
template <typename Scalar>
struct ExampleModel : public ad::ADModel<Scalar> {
    using typename ad::ADModel<Scalar>::ADVector;

    // Generate the input used when differentiating the function
    ADVector input() const override { return ADVector::Ones(3); }

    // Generate parameters used when differentiating the function
    // This can be skipped if the function takes no parameters
    ADVector parameters() const override { return ADVector::Ones(3); }

    // Implement a weighted sum-of-squares function.
    ADVector function(const ADVector& input,
                      const ADVector& parameters) const override {
        ADVector output(1);
        for (int i = 0; i < 3; ++i) {
            output(0) += 0.5 * parameters(i) * input(i) * input(i);
        }
        return output;
    }
};

int main() {
    // Compile model named ExampleModel and save in the current directory; the
    // model is saved as a shared object file named libExampleModel.so
    ExampleModel<double>().compile("ExampleModel", ".",
                                   ad::DerivativeOrder::First);
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/drop_while.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/negate.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;
using namespace hana::literals;

auto negative = [](auto x) {
    return x < hana::int_c<0>;
};

BOOST_HANA_CONSTANT_CHECK(
    hana::drop_while(hana::make_range(hana::int_c<-3>, hana::int_c<6>), negative)
        ==
    hana::make_range(hana::int_c<0>, hana::int_c<6>)
);

BOOST_HANA_CONSTANT_CHECK(
    hana::drop_while(hana::make_tuple(1_c, -2_c, 4_c, 5_c), negative)
        ==
    hana::make_tuple(1_c, -2_c, 4_c, 5_c)
);

int main() { }


//
// Created by George Lewis on 4/21/18.
//

#include "bloomfilter.h"
#include <boost/functional/hash.hpp>

BloomFilter::BloomFilter(size_t numBits, size_t numHashes) : bitMap(numBits, false), hashSeeds(numHashes) {
    for (size_t i = 0; i < numHashes; ++i) {
        hashSeeds.at(i) = i;
    }
}

std::vector<bool> BloomFilter::getHashBits(const std::string& key) {

    std::vector<bool> hashBits(bitMap.size());

    for (unsigned long bitIdx : hashSeeds) {
        boost::hash_combine(bitIdx, key);
        hashBits[bitIdx % bitMap.size()] = true;
    }

    return hashBits;
}

void BloomFilter::add(const std::string& key) {

    std::vector<bool> hashBits = getHashBits(key);

    for (size_t i = 0; i < bitMap.size(); ++i) {
        if (hashBits[i]) {
            bitMap.at(i) = true;
        }
    }
}

bool BloomFilter::containsKey(const std::string& key) {

    std::vector<bool> hashBits = getHashBits(key);

    for (size_t i = 0; i < bitMap.size(); ++i) {
        // If the bit is set for THIS key but not
        // in the bit map, then we KNOW FOR SURE
        // that this key has not been added.
        if (hashBits[i] && !bitMap[i]) {
            return false;
        }
    }

    // If all the bits for this key are set to on,
    // then we return true (either this key is present
    // or it's a false-positive)
    return true;
}

void BloomFilter::clear() {
    for (auto&& i : bitMap) {
        i = false;
    }
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/**
 * @file tableLookup.cpp
 * @brief Code for homomorphic table lookup and fixed-point functions
 */
#include <limits>
#include <cmath>
#include <cstdlib>
#include <stdexcept>
#include <NTL/BasicThreadPool.h>
#include <helib/intraSlot.h>
#include <helib/tableLookup.h>

#ifdef HELIB_DEBUG
#include <helib/debugging.h>
#endif

namespace helib {

static void recursiveProducts(const CtPtrs& products,
                              const CtPtrs_slice& array);
static double pow2_double(long n); // compute 2^n as double

// For an n-size array, compute the 2^n products
//     products[j] = \prod_{i s.t. j_i=1} array[i]
//                   \times \prod_{i s.t. j_i=0}(a-array[i])
void computeAllProducts(/*Output*/ CtPtrs& products,
                        /*Index*/ const CtPtrs& array,
                        std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  long nBits = array.size();
  if (lsize(products) > 0) {
    long nBits2 = NTL::NumBits(lsize(products) - 1); // ceil(log_2(size))
    if (nBits > nBits2)
      nBits = nBits2; // ignore extra bits in 'array'
  }
  if (nBits < 1)
    return; // do nothing
  // Output cannot be bigger than 2^16
  assertTrue(nBits <= 16, "Output cannot be bigger than 2^16");

  if (lsize(products) == 0) // try to set the output size
    products.resize(1L << nBits, &array);
  for (long i = 0; i < lsize(products); i++)
    products[i]->clear();

  // Check that we have enough levels, try to bootstrap otherwise
  assertNotNull(array.ptr2nonNull(),
                "Invalid array (could not find non-null Ctxt)");
  long bpl = array.ptr2nonNull()->getContext().BPL();
  if (findMinBitCapacity(array) < (NTL::NumBits(nBits) + 1) * bpl) {
    const Ctxt* ct = array.ptr2nonNull(); // find some non-null Ctxt
    assertNotNull(unpackSlotEncoding,
                  "unpackSlotEncoding must not be null when bootstrapping");
    assertTrue(ct->getPubKey().isBootstrappable(),
               "Cannot bootstrap with non-bootstrappable public key");
    packedRecrypt(array,
                  *unpackSlotEncoding,
                  *(ct->getContext().ea),
                  /*belowLevel=*/nBits + 3);
  }
  if (findMinBitCapacity(array) < (NTL::NumBits(nBits) + 1) * bpl)
    throw LogicError("not enough levels for table lookup");

  // Call the recursive function that computes the products
  recursiveProducts(products, CtPtrs_slice(array, 0, nBits));
}

// The input is a plaintext table T[] and an array of encrypted bits
// I[], holding the binary representation of an index i into T.
// The output is the encrypted value T[i].
void tableLookup(Ctxt& out,
                 const std::vector<zzX>& table,
                 const CtPtrs& idx,
                 std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  out.clear();
  std::vector<Ctxt> products(lsize(table),
                             out); // to hold subset products of idx
  CtPtrs_vectorCt pWrap(products); // A wrapper

  // Compute all products of encrypted bits =: b_i
  computeAllProducts(pWrap, idx, unpackSlotEncoding);

  // Compute the sum b_i * T[i]
  NTL_EXEC_RANGE(lsize(table), first, last)
  for (long i = first; i < last; i++)
    products[i].multByConstant(table[i]); // p[i] = p[i]*T[i]
  NTL_EXEC_RANGE_END
  for (long i = 0; i < lsize(table); i++)
    out += products[i];
}

// A counterpart of tableLookup. The input is an encrypted table T[]
// and an array of encrypted bits I[], holding the binary representation
// of an index i into T.  This function increments by one the entry T[i].
void tableWriteIn(const CtPtrs& table,
                  const CtPtrs& idx,
                  std::vector<zzX>* unpackSlotEncoding)
{
  HELIB_TIMER_START;
  const Ctxt* ct = table.ptr2nonNull(); // find some non-null Ctxt
  long size = lsize(table);
  if (size == 0)
    return;
  std::vector<Ctxt> products(size, Ctxt(ZeroCtxtLike, *ct));
  CtPtrs_vectorCt pWrap(products); // A wrapper

  // Compute all products of encrypted bits =: b_i
  computeAllProducts(pWrap, idx, unpackSlotEncoding);

  // increment each entry of T[i] by products[i]
  NTL_EXEC_RANGE(lsize(table), first, last)
  for (long i = first; i < last; i++)
    *table[i] += products[i];
  NTL_EXEC_RANGE_END
}

// The function buildLookupTable is documented in tableLookup.h.
// The output is returned in T, size of T will be 2^{nbits_in}.
// For every signed integer x with bit-size 'nbits_in', we will have
//     T[x] = f(x * 2^{scale_in}) * 2^{-scale_out}),
// rounded to the nearest integer and truncated to 'nbits_out' bits.
// The bits are packed inside the slots, so it is assumed that each
// slot has enough room to fit these many bits. (Otherwise we only
// keep as many low-order bits as fit in a slot.)
void buildLookupTable(std::vector<zzX>& T, // encoded result is returned in T
                      std::function<double(double)> f,
                      long nbits_in, // number of precision bits
                      long scale_in, // scaling factor
                      long sign_in,  // 1: 2's complement signed, 0: unsigned
                      long nbits_out,
                      long scale_out,
                      long sign_out,
                      const EncryptedArray& ea)
{
  HELIB_TIMER_START;
  // tables of size > 2^{16} are not supported
  assertTrue(nbits_in <= 16, "tables of size > 2^{16} are not supported");
  long sz = 1L << nbits_in;
  T.resize(sz);

  double pow2_scale_in = pow2_double(scale_in);        // 2^{nbits_in}
  double pow2_neg_scale_out = pow2_double(-scale_out); // 2^{-nbits_out}

  // Compute the largest and smallest values that can be in T
  long largest_value, smallest_value;
  if (sign_out) { // values in T are encoded in 2's complement
    largest_value = (1L << (nbits_out - 1)) - 1;
    smallest_value = -(1L << (nbits_out - 1));
  } else { // values in T are all non-negative
    largest_value = (1L << nbits_out) - 1;
    smallest_value = 0;
  }

  for (long i = 0; i < sz; i++) { // Compute the entries of T
    long x;
    if (sign_in) { // indexes into T are in 2's complement
      long sign_bit = (1L << (nbits_in - 1)) & i;
      x = i - 2 * sign_bit;
    } else
      x = i; // indexes into T are all non-negative

    // Compute the value that should go in the table as rounded double
    double scaled_x = double(x) * pow2_scale_in;
    double y = round(f(scaled_x) * pow2_neg_scale_out);

    // saturated arithmetic (set to smallest or largest values)
    // NOTE: this should work fine even if y is an infinity
    long value;
    if (std::isnan(y))
      value = 0;
    else if (y > largest_value)
      value = largest_value;
    else if (y < smallest_value)
      value = smallest_value;
    else
      value = y;

    // convert to unsigned and mask to nbits_out bits
    unsigned long uvalue = value;
    uvalue &= ((1UL << nbits_out) - 1UL); // keep only bottom nbits_out bits

    packConstant(T[i], uvalue, nbits_out, ea);
  }
}

// A recursive function to compute, for an n-size array, the 2^n products
//     products[j] = \prod_{i s.t. j_i=1} array[i]
//                   \times \prod_{i s.t. j_i=0}(a-array[i])
// It is assume that 'products' size <= 2^n, else only 1st 2^n entries are set
static void recursiveProducts(const CtPtrs& products, const CtPtrs_slice& array)
{
  long nBits = lsize(array);
  long N = lsize(products);
  if (nBits == 0 || N == 0)
    return; // nothing to do

  if (N > (1L << nBits))
    N = (1L << nBits);
  else if (N < (1L << (nBits - 1)))
    nBits = NTL::NumBits(N - 1); // Ensure nBits <= ceil(log2(N))

  if (N <= 2) { // edge condition
    *products[0] = *array[0];
    products[0]->negate();
    products[0]->addConstant(NTL::ZZ(1)); // out[0] = 1-in
    if (N > 1)
      *products[1] = *array[0]; // out[1] = in
  }
  // optimization for n=2: a single multiplication instead of 4
  else if (N <= 4) {
    *products[0] = *array[1];           // x1
    products[0]->multiplyBy(*array[0]); // x1 x0

    *products[1] = *array[0];     // x0
    *products[1] -= *products[0]; // x0 - x1 x0 = (1-x1)x0

    *products[2] = *array[1];     // x1
    *products[2] -= *products[0]; // x1 - x1 x0 = x1(1-x0)

    if (N > 3)
      *products[3] = *products[0]; // x1 x0

    products[0]->addConstant(NTL::ZZ(1)); // 1 +x1 x0
    *products[0] -= *array[1];            // 1 +x1 x0 -x1
    *products[0] -= *array[0];            // 1 +x1 x0 -x1 -x0 = (1-x1)(1-x0)
  } else {                                // split the array into two parts;
    // first part is highest pow(2) < n, second part is what is left

    long n1 = 1L << (NTL::NumBits(nBits) - 1); // largest power of two <= n
    if (nBits <= n1)
      n1 = n1 / 2;               // largest power of two < n
    long k = 1L << n1;           // size of first part
    long l = 1L << (nBits - n1); // size of second part

    const Ctxt* ct = array.ptr2nonNull(); // find some non-null Ctxt
    std::vector<Ctxt> products1(k, Ctxt(ZeroCtxtLike, *ct));
    std::vector<Ctxt> products2(l, Ctxt(ZeroCtxtLike, *ct));

    // compute first part of the array
    recursiveProducts(CtPtrs_vectorCt(products1), CtPtrs_slice(array, 0, n1));

    // recursive call on second part of array
    recursiveProducts(CtPtrs_vectorCt(products2),
                      CtPtrs_slice(array, n1, nBits - n1));

    // multiplication to get all subset products
    NTL_EXEC_RANGE(lsize(products), first, last)
    for (long ii = first; ii < last; ii++) {
      long j = ii / k;
      long i = ii - j * k;
      *products[ii] = products1[i];
      products[ii]->multiplyBy(products2[j]);
    }
    NTL_EXEC_RANGE_END
  }
}

static double pow2_double(long n) // compute 2^n as double
{
  double res = 1;
  long abs_n = std::labs(n);

  for (long i = 0; i < abs_n; i++)
    res *= 2;
  if (n < 0)
    res = 1 / res;
  return res;
}

} // namespace helib


/* Perceptual image hash calculation tool based on algorithm descibed in
 * Block Mean Value Based Image Perceptual Hashing by Bian Yang, Fan Gu and Xiamu Niu
 *
 * Copyright 2014 Commons Machinery http://commonsmachinery.se/
 * Distributed under an MIT license, please see LICENSE in the top dir.
 */
#include <boost/asio/io_service.hpp>
#include <boost/bind.hpp>
#include <boost/thread/thread.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>
#include <cstring>
#include <math.h>
#include <string>
#include <iostream>
#include <vector>
#include <fstream>
#include <chrono>
#include <thread>

#ifdef MAGICKWAND_V7
#include <MagickWand/MagickWand.h>
#else
#include <wand/MagickWand.h>
#endif

// Global counter
std::atomic<int> completeCounter{0};

// comparing functions for qsort

int cmpint(const void *pa, const void *pb)
{
    int a = *(const int *) pa;
    int b = *(const int *) pb;
    return (a < b) ? -1 : (a > b);
}

int cmpfloat(const void *pa, const void *pb)
{
    float a = *(const float *) pa;
    float b = *(const float *) pb;
    return (a < b) ? -1 : (a > b);
}

float median(int *data, int n)
{
    int *sorted;
    float result;

    sorted = malloc(n * sizeof(int));
    memcpy(sorted, data, n * sizeof(int));
    qsort(sorted, n, sizeof(int), cmpint);

    if (n % 2 == 0) {
        result = (float) (sorted[n / 2] + sorted[n / 2 + 1]) / 2;
    } else {
        result = (float) sorted[n / 2];
    }

    free(sorted);
    return result;
}

float medianf(float *data, int n)
{
    float *sorted;
    float result;

    sorted = malloc(n * sizeof(float));
    memcpy(sorted, data, n * sizeof(float));
    qsort(sorted, n, sizeof(float), cmpfloat);

    if (n % 2 == 0) {
        result = (sorted[n / 2] + sorted[n / 2 + 1]) / 2;
    } else {
        result = sorted[n / 2];
    }

    free(sorted);
    return result;
}

/** Compare medians across four horizontal bands
 *
 * Output a 1 if the block is brighter than the median.
 * With images dominated by black or white, the median may
 * end up being 0 or the max value, and thus having a lot
 * of blocks of value equal to the median.  To avoid
 * generating hashes of all zeros or ones, in that case output
 * 0 if the median is in the lower value space, 1 otherwise
 */
void translate_blocks_to_bits(int *blocks, int nblocks, int pixels_per_block)
{
    float half_block_value;
    int bandsize, i, j;
    int m, v;

    half_block_value = pixels_per_block * 256 * 3 / 2;
    bandsize = nblocks / 4;

    for (i = 0; i < 4; i++) {
        m = median(&blocks[i * bandsize], bandsize);
        for (j = i * bandsize; j < (i + 1) * bandsize; j++) {
            v = blocks[j];
            blocks[j] = v > m || (abs(v - m) < 1 && m > half_block_value);
        }
    }
}

void translate_blocks_to_bitsf(float *blocks, int *result, int nblocks, int pixels_per_block)
{
    float half_block_value;
    int bandsize, i, j;
    float m, v;

    half_block_value = pixels_per_block * 256 * 3 / 2;
    bandsize = nblocks / 4;

    for (i = 0; i < 4; i++) {
        m = medianf(&blocks[i * bandsize], bandsize);
        for (j = i * bandsize; j < (i + 1) * bandsize; j++) {
            v = blocks[j];
            result[j] = v > m || (fabsf(v - m) < 1 && m > half_block_value);
        }
    }
}

/** Convert array of bits to hexadecimal string representation.
 * Hash length should be a multiple of 4.
 *
 * Returns: null-terminated hexadecimal string hash.
 */
char* bits_to_hexhash(int *bits, int nbits)
{
    int    i, j, b;
    int    len;
    int    tmp;
    char  *hex;
    char  *stmp;

    len = nbits / 4;

    hex = malloc(len + 1);
    stmp = malloc(2);
    hex[len] = '\0';

    for (i = 0; i < len; i++) {
        tmp = 0;
        for (j = 0; j < 4; j++) {
            b = i * 4 + j;
            tmp = tmp | (bits[b] << 3 >> j);
        }

        sprintf(stmp, "%1x", tmp);
        hex[i] = stmp[0];
    }

    free(stmp);
    return hex;
}

/** Calculate perceptual hash for an RGBA image using quick method.
*
* Quick method uses rounded block sizes and is less accurate in case image
* width and height are not divisible by the number of bits.
*
* Parameters:
*
* bits - number of blocks to divide the image by horizontally and vertically.
* data - RGBA image data.
* width - image width.
* height - image height.
* hash - the resulting hash will be allocated and stored in the given array as bits.
*/
void blockhash_quick(int bits, unsigned char *data, int width, int height, int **hash)
{
    int    x, y, ix, iy;
    int    ii, alpha, value;
    int    block_width;
    int    block_height;
    int   *blocks;

    block_width = width / bits;
    block_height = height / bits;

    blocks = calloc(bits * bits, sizeof(int));
    for (y = 0; y < bits; y++) {
        for (x = 0; x < bits; x++) {
            value = 0;

            for (iy = 0; iy < block_height; iy++) {
                for (ix = 0; ix < block_width; ix++) {
                    ii = ((y * block_height + iy) * width + (x * block_width + ix)) * 4;

                    alpha = data[ii+3];
                    if (alpha == 0) {
                        value += 765;
                    } else {
                        value += data[ii] + data[ii+1] + data[ii+2];
                    }
                }
            }

            blocks[y * bits + x] = value;
        }
    }

    translate_blocks_to_bits(blocks, bits * bits, block_width * block_height);
    *hash = blocks;
}

/** Calculate perceptual hash for an RGBA image using precise method.
*
* Precise method puts weighted pixel values to blocks according to pixel
* area falling within a given block and provides more accurate results
* in case width and height are not divisible by the number of bits.
*
* Parameters:
*
* bits - number of blocks to divide the image by horizontally and vertically.
* data - RGBA image data.
* width - image width.
* height - image height.
* hash - the resulting hash will be allocated and stored in the given array as bits.
*/
void blockhash(int bits, unsigned char *data, int width, int height, int **hash)
{
    float   block_width;
    float   block_height;
    float   y_frac, y_int;
    float   x_frac, x_int;
    float   x_mod, y_mod;
    float   weight_top, weight_bottom, weight_left, weight_right;
    int     block_top, block_bottom, block_left, block_right;
    int     x, y, ii, alpha;
    float   value;
    float  *blocks;
    int    *result;

    if (width % bits == 0 && height % bits == 0) {
        return blockhash_quick(bits, data, width, height, hash);
    }

    block_width = (float) width / (float) bits;
    block_height = (float) height / (float) bits;

    blocks = calloc(bits * bits, sizeof(float));
    result = malloc(bits * bits * sizeof(int));

    for (y = 0; y < height; y++) {
        y_mod = fmodf(y + 1, block_height);
        y_frac = modff(y_mod, &y_int);

        weight_top = (1 - y_frac);
        weight_bottom = y_frac;

        // y_int will be 0 on bottom/right borders and on block boundaries
        if (y_int > 0 || (y + 1) == height) {
            block_top = block_bottom = (int) floor((float) y / block_height);
        } else {
            block_top = (int) floor((float) y / block_height);
            block_bottom = (int) ceil((float) y / block_height);
        }

        for (x = 0; x < width; x++) {
            x_mod = fmodf(x + 1, block_width);
            x_frac = modff(x_mod, &x_int);

            weight_left = (1 - x_frac);
            weight_right = x_frac;

            // x_int will be 0 on bottom/right borders and on block boundaries
            if (x_int > 0 || (x + 1) == width) {
                block_left = block_right = (int) floor((float) x / block_width);
            } else {
                block_left = (int) floor((float) x / block_width);
                block_right = (int) ceil((float) x / block_width);
            }

            ii = (y * width + x) * 4;

            alpha = data[ii + 3];
            if (alpha == 0) {
                value = 765;
            } else {
                value = data[ii] + data[ii + 1] + data[ii + 2];
            }

            // add weighted pixel value to relevant blocks
            blocks[block_top * bits + block_left] += value * weight_top * weight_left;
            blocks[block_top * bits + block_right] += value * weight_top * weight_right;
            blocks[block_bottom * bits + block_left] += value * weight_bottom * weight_left;
            blocks[block_bottom * bits + block_right] += value * weight_bottom * weight_right;
        }
    }

    translate_blocks_to_bitsf(blocks, result, bits * bits, block_width * block_height);
    *hash = result;
    free(blocks);
}

void process_image(char * fn, int bits, int quick, int debug, int semi)
{
    int i;
    size_t width, height;
    unsigned char *image_data;
    int *hash;
    MagickBooleanType status;
    MagickWand *magick_wand;

    magick_wand = NewMagickWand();
    status = MagickReadImage(magick_wand, fn);

    if (status == MagickFalse) {
        printf("Error opening image file %s\n", fn);
        exit(-1);
    }

    // Remove color profiles for interoperability with other hashing tools
    MagickProfileImage(magick_wand, "*", NULL, 0);

    width = MagickGetImageWidth(magick_wand);
    height = MagickGetImageHeight(magick_wand);

    image_data = malloc(width * height * 4);

    status = MagickExportImagePixels(magick_wand, 0, 0, width, height, "RGBA", CharPixel, image_data);

    if (status == MagickFalse) {
        printf("Error converting image data to RGBA\n");
        exit(-1);
    }

    hash = malloc(bits * bits * sizeof(int));

    if (quick) {
        blockhash_quick(bits, image_data, width, height, &hash);
    } else {
        blockhash(bits, image_data, width, height, &hash);
    }

    if (debug) {
        for (i = 0; i < bits * bits; i++) {
            if (i != 0 && i % bits == 0)
                printf("\n");
            printf("%d", hash[i]);
        }
        printf("\n");
    }

    char* hex = bits_to_hexhash(hash, bits*bits);
    if (semi){
      printf("%s;%s\n", hex, fn);
    } else {
      printf("%s  %s\n", hex, fn);
    }

    free(hex);
    free(hash);
    free(image_data);

    DestroyMagickWand(magick_wand);
    completeCounter++;
}

void help() {
    printf("Usage: blockhash [-h|--help] [--threads THREADS] [-s] [--quick] [--bits BITS] [--debug] -f pathfile ...\n"
           "\n"
           "Optional arguments:\n"
           "-t, --threads         Number of threads to use\n"
           "-s, --semidelim       Use a semicolon as delimiter\n"
           "-h, --help            Show this help message and exit\n"
           "-q, --quick           Use quick hashing method.\n"
           "-b, --bits BITS       Create hash of size N^2 bits. Default: 16\n"
           "--debug               Print hashes as 2D maps (for debugging)\n"
         );
}

int main (int argc, char **argv) {
    MagickWandGenesis();

    int quick = 0;
    int debug = 0;
    int bits = 16;
    int semi = 0;
	  int threads = 4;
	  char * filelist;

    int option_index = 0;
    int c;

    struct option long_options[] = {
        {"filenames",   required_argument,  0,	'f'},
        {"quick",		no_argument,        0,	'q'},
		    {"threads",		required_argument,	0,	't'},
        {"bits",		required_argument,  0,	'b'},
        {"debug",		no_argument,        0,	'd'},
        {"semidelim", no_argument,  0, 's'},

        {0, 0, 0, 0}
    };

    if (argc < 2) {
        help();
        exit(0);
    }

    while ((c = getopt_long(argc, argv, "hqb:df:t:s",
                 long_options, &option_index)) != -1) {
        switch (c) {
		case 'f':
			filelist = optarg;
			break;
		case 't':
			threads = std::stoi(optarg);
			break;
        case 'h':
            help();
            exit(0);
            break;
        case 'q':
            quick = 1;
            break;
        case 'b':
            if (sscanf(optarg, "%d", &bits) != 1) {;
                printf("Error: couldn't parse bits argument\n");
                exit(-1);
            }
            if (bits % 4 != 0) {
                printf("Error: bits argument should be a multiple of 4\n");
                exit(-1);
            }
            break;
        case 'd':
            debug = 1;
            break;
        case 's':
            semi = 1;
            break;
        case '?':
        default:
            exit(-1);
        }
    }

	// Read file names
	std::vector<std::string> filenames;
	std::ifstream input(filelist);
	for (std::string line; getline(input, line); )
	{
		//for each line, add it to the list of files
		filenames.push_back(line);
	}

  /*
   * Create an asio::io_service and a thread_group (through pool in essence)
   */
  boost::asio::io_service ioService;
  boost::thread_group threadpool;
  /*
   * This will start the ioService processing loop. All tasks
   * assigned with ioService.post() will start executing.
   */
  boost::asio::io_service::work work(ioService);
  /*
   * Create worker threads
   */
  for (int i = 0; i < threads; ++i)
    threadpool.create_thread(boost::bind(&boost::asio::io_service::run, &ioService));

    /*
     * This will assign tasks to the thread pool.
     * More about boost::bind: "http://www.boost.org/doc/libs/1_54_0/libs/bind/bind.html#with_functions"
     */
    int counter=0;
    for (size_t i=0; i<filenames.size(); i++){
      if (counter<30000){
          ioService.post(boost::bind(process_image, filenames[i].c_str(), bits, quick, debug, semi));
          counter++;
      } else {
          counter = i - completeCounter;
          //cout << +completeCounter << " " << +c << endl;
          if (counter >30000){
              std::this_thread::sleep_for(std::chrono::milliseconds(50));
          }
          ioService.post(boost::bind(process_image, filenames[i].c_str(), bits, quick, debug, semi));
      }
    }

    // Wait for jobs to finish
    ioService.reset();
    while (completeCounter < filenames.size()){
      std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    ioService.stop();

    // Clean up
    MagickWandTerminus();;

    exit(0);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

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

using namespace std;

void test(int n, int s)
{
    mtl::compressed2D<double> A(n, n), B(n, n);
    {
	double val = 3.14;
	mtl::mat::inserter<mtl::compressed2D<double> > ins(A, int(1.2*s)), ins2(B, int(1.2*s));
	for (int i= 0; i < n*s; i++) {
	    ins[rand()%n][rand()%n] << val;
	    ins2[rand()%n][rand()%n] << val;
	}
    }
    boost::timer start;
    if (n < 10) cout << "A is \n" << A << "B is \n" << B;
    A+= B;
    if (n < 10) cout << "A is \n" << A;
    cout << "Addition took " << start.elapsed() << "s\n";
}

int main(int argc, char* argv[])
{
    test(atoi(argv[1]), atoi(argv[2]));

    return 0;
}


#define BOOST_TEST_MODULE mcmc chain
#define BOOST_TEST_DYN_LINK
#include <cmath>
#include <random>
#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <mpp/chains/mcmc_chain.hpp>

template<typename real_scalar_type>
void test_mcmc_chain(std::string const & file_name)
{
    using namespace mpp::chains;
    using namespace boost::numeric::ublas;
    typedef vector<real_scalar_type> real_vector_type;
    typedef mcmc_chain<real_scalar_type> mcmc_chain_type;
    typedef std::mt19937 rng_type;
    typedef std::normal_distribution<real_scalar_type> normal_distribution_type;

    size_t const num_dims = 3;
    size_t const num_samples = 10;
    mcmc_chain_type mc_chn(num_samples,num_dims);
    real_vector_type q_0(num_dims);
    rng_type rng;
    normal_distribution_type norm_dist;
    for(size_t i=0;i<num_samples;++i)
    {
        for(size_t j=0;j<num_dims;++j)
        {
            q_0(j) = norm_dist(rng);
        }
        mc_chn.set_sample(i,q_0,real_scalar_type(1));
    }
    mc_chn.write_samples_to_csv(file_name);

}

BOOST_AUTO_TEST_CASE(mcmc_chain)
{
    test_mcmc_chain<float>(std::string("float.chain"));
    test_mcmc_chain<double>(std::string("double.chain"));
    test_mcmc_chain<long double>(std::string("long-double.chain"));
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#define NT2_UNIT_MODULE "nt2 combinatorial toolbox - primes/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of combinatorial components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 07/03/2011
///
#include <nt2/combinatorial/include/functions/primes.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/saturate.hpp>

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/tic.hpp>
#include <nt2/include/functions/toc.hpp>

NT2_TEST_CASE_TPL ( primes_integer__1_0,  (int32_t))//NT2_INTEGRAL_TYPES)
{

  using nt2::primes;
  using nt2::tag::primes_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<primes_(T)>::type r_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;
  typedef typename nt2::meta::upgrade<T>::type u_t;

  T p[6] = {2, 3, 5, 7, 11, 13};
  nt2::table<T> myp = primes(T(15));
  // specific values tests
  NT2_DISPLAY(myp);
 for(size_t i=1; i <= 6; ++i)
   {
     NT2_TEST_EQUAL(p[i-1], myp(i));
   }

 nt2::table<T> myp1 = primes(T(100000));
 NT2_DISPLAY(primes(T(100)));
 nt2::table<T> myp9 = primes(T(9));
 NT2_DISPLAY(myp9);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto plus = [](auto x, auto y) {
        return x + y;
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto increment = partial(plus, 1);

    BOOST_HANA_CONSTEXPR_ASSERT(increment(2) == 3);
    //! [main]
}


#include <boost/graph/adjacency_list.hpp>
#include <array>
#include <utility>
#include <iostream>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g{edges.begin(), edges.end(), 4};

  std::cout << boost::num_vertices(g) << '\n';
  std::cout << boost::num_edges(g) << '\n';

  g.clear();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REVERSEBITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_REVERSEBITS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/shift_left.hpp>
#include <boost/simd/function/simd/shr.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints8_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x55); //binary: 0101...
        const utype m2  = utype(0x33); //binary: 00110011..
        const utype m4  = utype(0x0f); //binary:  4 zeros,  4 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints64_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x5555555555555555ull); //binary: 0101...
        const utype m2  = utype(0x3333333333333333ull); //binary: 00110011..
        const utype m4  = utype(0x0f0f0f0f0f0f0f0full); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff00ff00ff00ffull); //binary:  8 zeros,  8 ones ...
        const utype m16 = utype(0x0000ffff0000ffffull); //binary:  16 zeros,  16 ones ...
        const utype m32 = utype(0x00000000ffffffffull); //binary:  32 zeros,  32 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        // swap shorts ...
        v = bitwise_or(bitwise_and(shr(v, 16), m16), shift_left(bitwise_and(v, m16), 16));
        // swap ints ...
        v = bitwise_or(bitwise_and(shr(v, 32), m32), shift_left(bitwise_and(v, m32), 32));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints16_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x5555); //binary: 0101...
        const utype m2  = utype(0x3333); //binary: 00110011..
        const utype m4  = utype(0x0f0f); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff); //binary:  8 zeros,  8 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        return bitwise_cast<A0>(v);
        }
   };

   BOOST_DISPATCH_OVERLOAD(reversebits_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::ints32_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using utype = bd::as_integer_t<A0, unsigned>;
        utype v = bitwise_cast<utype>(a0);
        const utype m1  = utype(0x55555555); //binary: 0101...
        const utype m2  = utype(0x33333333); //binary: 00110011..
        const utype m4  = utype(0x0f0f0f0f); //binary:  4 zeros,  4 ones ...
        const utype m8  = utype(0x00ff00ff); //binary:  8 zeros,  8 ones ...
        const utype m16 = utype(0x0000ffff); //binary:  16 zeros,  16 ones ...
        // swap odd and even bits
        v = bitwise_or(bitwise_and(shr(v, 1), m1), shift_left(bitwise_and(v, m1), 1));
        // swap consecutive pairs
        v = bitwise_or(bitwise_and(shr(v, 2), m2), shift_left(bitwise_and(v, m2), 2));
        // swap nibbles ...
        v = bitwise_or(bitwise_and(shr(v, 4), m4), shift_left(bitwise_and(v, m4), 4));
        // swap bytes ...
        v = bitwise_or(bitwise_and(shr(v, 8), m8), shift_left(bitwise_and(v, m8), 8));
        // swap shorts ...
        v = bitwise_or(bitwise_and(shr(v, 16), m16), shift_left(bitwise_and(v, m16), 16));
        return bitwise_cast<A0>(v);
        }
   };
} } }

#endif


#ifndef UTILS_HPP
#define UTILS_HPP

#include <ctime>
#include <vector>
#include <sstream>
#include <libintl.h>
#include <FL/Fl_Window.H>
#include <boost/lexical_cast.hpp>
#include "point.hpp"

#define _(STRING) gettext(STRING)

namespace florb
{
    class utils
    {
        public:
            static unsigned long dim(unsigned int z);

            static florb::point2d<double> wsg842merc(const florb::point2d<double> &wsg84);
            static florb::point2d<unsigned long> merc2px(unsigned int z, const florb::point2d<double> &merc);
            static florb::point2d<unsigned long> wsg842px(unsigned int z, const florb::point2d<double> &wsg84);
            static florb::point2d<double> px2wsg84(unsigned int z, const florb::point2d<unsigned long> &px);
            static florb::point2d<double> merc2wsg84(const florb::point2d<double>& wsg84);
            static florb::point2d<double> px2merc(unsigned int z, const florb::point2d<unsigned long> &px);

            static bool clipline(florb::point2d<double> &p1, florb::point2d<double> &p2, const florb::point2d<double> &r1, const florb::point2d<double> &r2, bool &p1clip, bool &p2clip);

            static double dist(const florb::point2d<double> &p1, const florb::point2d<double> &p2);
            static double dist_merc(const florb::point2d<double> &p1, const florb::point2d<double> &p2);
            static double meters_per_pixel(unsigned int z, double lat);

            static time_t iso8601_2timet(const std::string& iso);
            static std::string timet2iso8601(time_t t);

            static std::vector<std::string> str_split(const std::string& str, const std::string& delimiter);
            static std::size_t str_count(const std::string& str, const std::string& token);
            static void str_replace(std::string& str, const std::string& s, const std::string& r);

            template <class T>
                static bool fromstr(const std::string& s, T& out)
                {
                    try {
                        out = boost::lexical_cast<T>(s);
                    } catch (const boost::bad_lexical_cast &e) {
                        return false;
                    }

                    return true;
                };

            static std::string pathsep();
            static std::string userdir();
            static std::string appdir();
            static bool mkdir(const std::string& path);
            static void rm(const std::string& path);
            static bool exists(const std::string& path);
            static std::string filestem(const std::string& path);
            static std::string extension(const std::string& path);
            static void touch(const std::string& path);

            static void set_window_icon(Fl_Window *w);
        private:
    };

};

#endif // UTILS_HPP


// Copyright Matt Overby 2022.
// Distributed under the MIT License.

#ifndef MCL_EIGENMAP_HPP
#define MCL_EIGENMAP_HPP 1

#include <Eigen/Dense>
#include <stdexcept>

namespace mcl
{

template <typename DerivedM, typename DerivedV>
static inline void mat_to_vec(
    const Eigen::PlainObjectBase<DerivedM> &M,
    Eigen::PlainObjectBase<DerivedV> &V)
{
    typedef typename Eigen::PlainObjectBase<DerivedM>::Scalar Scalar;
    int nr = M.rows();
    int nc = M.cols();
    if (V.size() != nr*nc) {
        V.resize(nr*nc);
    }

    if (M.derived().IsRowMajor)
    {
        V = Eigen::Map<DerivedV>((Scalar*)M.data(), M.size());
        return;
    }

    for (int c=0; c<nc; ++c) {
        for (int r=0; r<nr; ++r) {
            V[r*nc+c] = M(r,c);
        }
    }
}

// Matrix must be sized ahead of time
template <typename DerivedM, typename DerivedV>
static inline void vec_to_mat(
    const Eigen::PlainObjectBase<DerivedV> &V,
    Eigen::PlainObjectBase<DerivedM> &M)
{
    typedef typename Eigen::PlainObjectBase<DerivedV>::Scalar Scalar;
    if (M.cols()<1 || M.size() != V.size()) {
        throw std::runtime_error("mcl::vec_to_mat: M not correct size");
    }

    if (M.derived().IsRowMajor)
    {
        M = Eigen::Map<DerivedM>((Scalar*)V.data(), M.rows(), M.cols());
        return;
    }

    // Column major, need loop.
    int nr = M.rows();
    int nc = M.cols();
    for (int c=0; c<nc; ++c) {
        for (int r=0; r<nr; ++r) {
            M(r,c) = V[r*nc+c];
        }
    }
}

} // ns mcl

#endif


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST_CASE_CONTROL_ASSOCIATION_LOG_LIKELIHOOD_HPP
#define SNPTEST_CASE_CONTROL_ASSOCIATION_LOG_LIKELIHOOD_HPP

#include <vector>
#include <memory>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"

namespace metro {
	namespace regression {
		/*
		* This class implements a log-likelihood for Bernoulli logistic regression, allowing predictors
		* to take one of a finite set of values with associated probabilities. (A "missing data log-likelihood" ).
		* The outcome must be a two-column matrix containing 0s and 1s; a 1 in the 2nd column indicates
		* a non-baseline outcome and a 1 in the 1st column indicates a baseline outcome.
		*/
		struct Logistic: public LogLikelihood
		{
		public:
			typedef regression::Design::Point Point ;
			typedef regression::Design::Vector Vector ;
			typedef regression::Design::RowVector RowVector ;
			typedef regression::Design::Matrix Matrix ;
			typedef Eigen::Block< Matrix > MatrixBlock ;
			typedef Eigen::Block< Matrix const > ConstMatrixBlock ;
			typedef boost::function< std::string( std::string const& predictor_name, std::string const& outcome_name ) > GetParameterName ;
		public:
			typedef std::auto_ptr< Logistic > UniquePtr ;
			static UniquePtr create( Design& ) ;
			static UniquePtr create( Design::UniquePtr ) ;
			Logistic( Design& ) ;
			Logistic( Design::UniquePtr ) ;
			~Logistic() ;

			regression::Design& design() const { return *m_design ; }
			void set_parameter_naming_scheme( GetParameterName ) ;
			std::string get_parameter_name( std::size_t i ) const ;

			int number_of_parameters() const ;
			int number_of_outcomes() const ;
			IntegerMatrix identify_parameters() const ;

			void evaluate_at( Point const& parameters, int const numberOfDerivatives = 2 ) ;
			void evaluate( int const numberOfDerivatives = 2 ) ;

			Point const& parameters() const ;
			double get_value_of_function() const ;
			Vector get_value_of_first_derivative() const ;
			Matrix get_value_of_second_derivative() const ;

			std::string get_summary() const ;

		private:
			regression::Design* m_design ;
			bool m_design_owned ;
			GetParameterName m_get_parameter_name ;
			Point m_parameters ;
			std::vector< metro::SampleRange > m_included_samples ;
			enum State {
				e_Uncomputed = 0,
				e_ComputedFunction = 1,
				e_Computed1stDerivative = 2,
				e_Computed2ndDerivative = 4
			} ;
			uint32_t m_state ;
			Matrix m_outcome_probabilities ;
			Matrix m_A ;
			Matrix m_B ;
			Vector m_temp ;
			Matrix m_first_derivative_terms ;

			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:
			// Evaluate
			void evaluate_at_impl( Point const& parameters, std::vector< metro::SampleRange > const& included_samples, int const numberOfDerivatives ) ;
			// Calculate the probability of outcome given the genotype, parameters, and covariates.
			Vector evaluate_mean_function( Vector const& linear_combinations, Matrix const& outcomes ) const ;
			// Calculate matrix of probabilities of outcome per genotype, given the parameters.
			void calculate_outcome_probabilities( Vector const& parameters, Matrix const& phenotypes, Matrix* result ) const ;
			void compute_value_of_function( Matrix const& V, std::vector< metro::SampleRange > const& included_samples ) ;
			void compute_value_of_first_derivative( Matrix const& A, std::vector< metro::SampleRange > const& included_samples, Matrix* B ) ;
			void compute_value_of_second_derivative( Matrix const& B, std::vector< metro::SampleRange > const& included_samples ) ;
		} ;
	}
}

#endif


/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file BasicGasPricer.cpp
 * @author Gav Wood <i@gavwood.com>
 * @date 2015
 */

#pragma warning(push)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include <boost/math/distributions/normal.hpp>
#pragma warning(pop)
#pragma GCC diagnostic pop
#include "BasicGasPricer.h"
#include "BlockChain.h"
using namespace std;
using namespace dev;
using namespace dev::eth;

void BasicGasPricer::update(BlockChain const& _bc)
{
	unsigned c = 0;
	h256 p = _bc.currentHash();
	m_gasPerBlock = _bc.info(p).gasLimit();

	map<u256, u256> dist;
	u256 total = 0;

	// make gasPrice versus gasUsed distribution for the last 1000 blocks
	while (c < 1000 && p)
	{
		BlockHeader bi = _bc.info(p);
		if (bi.transactionsRoot() != EmptyTrie)
		{
			auto bb = _bc.block(p);
			RLP r(bb);
			BlockReceipts brs(_bc.receipts(bi.hash()));
			size_t i = 0;
			for (auto const& tr: r[1])
			{
				Transaction tx(tr.data(), CheckTransaction::None);
				u256 gu = brs.receipts[i].cumulativeGasUsed();
				dist[tx.gasPrice()] += gu;
				total += gu;
				i++;
			}
		}
		p = bi.parentHash();
		++c;
	}

	// fill m_octiles with weighted gasPrices
	if (total > 0)
	{
		m_octiles[0] = dist.begin()->first;

		// calc mean
		u256 mean = 0;
		for (auto const& i: dist)
			mean += i.first * i.second;
		mean /= total;

		// calc standard deviation
		u256 sdSquared = 0;
		for (auto const& i: dist)
			sdSquared += i.second * (i.first - mean) * (i.first - mean);
		sdSquared /= total;

		if (sdSquared)
		{
			long double sd = sqrt(sdSquared.convert_to<long double>());
			long double normalizedSd = sd / mean.convert_to<long double>();

			// calc octiles normalized to gaussian distribution
			boost::math::normal gauss(1.0, (normalizedSd > 0.01) ? normalizedSd : 0.01);
			for (size_t i = 1; i < 8; i++)
				m_octiles[i] = u256(mean.convert_to<long double>() * boost::math::quantile(gauss, i / 8.0));
			m_octiles[8] = dist.rbegin()->first;
		}
		else
		{
			for (size_t i = 0; i < 9; i++)
				m_octiles[i] = (i + 1) * mean / 5;
		}
	}
}