#include <boost/test/unit_test.hpp>
#include "circle.hpp"
#include <cmath>

const double accuracy = 0.00001;

// =====================test initialization circle=====================
BOOST_AUTO_TEST_SUITE(InitializeCircle)

  BOOST_AUTO_TEST_CASE(InitializeCircle_OnCorrectValue_NoError)
  {
    jianing::Circle circle {{1.1, 1.2}, 3.5};

    BOOST_REQUIRE_EQUAL(1.1, circle.getCenter().x);
    BOOST_REQUIRE_EQUAL(1.2, circle.getCenter().y);
    BOOST_REQUIRE_EQUAL(3.5, circle.getRadius());
    BOOST_CHECK_CLOSE(M_PI * 3.5 * 3.5, circle.getArea(), accuracy);
  }

  BOOST_AUTO_TEST_CASE(InitializeCircle_OnWrongValue_ThrowError)
  {
    BOOST_CHECK_THROW(jianing::Circle({3.10, 4.15}, 0.0), std::domain_error);
    BOOST_CHECK_THROW(jianing::Circle({3.10, 4.15}, -9.9), std::domain_error);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test initialization circle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectWhenInitializeCircle)
  BOOST_AUTO_TEST_CASE(TestFrameRectWhenInitializeCircle_OnCorrectValue_NoError)
  {
    jianing::Circle circle {{1.1, 1.2}, 3.5};

    BOOST_CHECK_CLOSE(3.5 * 2.0, circle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(3.5 * 2.0, circle.getFrameRect().height, accuracy);
    BOOST_REQUIRE_EQUAL(1.1, circle.getFrameRect().pos.x);
    BOOST_REQUIRE_EQUAL(1.2, circle.getFrameRect().pos.y);
  }
BOOST_AUTO_TEST_SUITE_END()

// =====================test move circle=====================
BOOST_AUTO_TEST_SUITE(MoveCirle)

  BOOST_AUTO_TEST_CASE(MoveCirle_ToPointXY_CorrectCenterAfterMove)
  {
    jianing::Circle circle {{10.0, 0.0}, 5.0};

    circle.move({-10.0, -20.0});

    BOOST_CHECK_EQUAL(-10.0, circle.getCenter().x);
    BOOST_CHECK_EQUAL(-20.0, circle.getCenter().y);
    BOOST_CHECK_EQUAL(5.0, circle.getRadius());
    BOOST_CHECK_CLOSE(M_PI * 5.0 * 5.0, circle.getArea(), accuracy);
  }

  BOOST_AUTO_TEST_CASE(MoveCirle_Bydxdy_CorrectCenterAfterMove)
  {
    jianing::Circle circle {{-5.1, 5.2}, 2.5};

    circle.move(5.0, -5.0);

    BOOST_CHECK_CLOSE(-5.1 + 5.0, circle.getCenter().x, accuracy);
    BOOST_CHECK_CLOSE(5.2 - 5.0, circle.getCenter().y, accuracy);
    BOOST_CHECK_CLOSE(M_PI * 2.5 * 2.5, circle.getArea(), accuracy);
    BOOST_CHECK_EQUAL(2.5, circle.getRadius());
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test move circle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterMoveCirle)

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterMoveCirle_ToPointXY_CorrectFrameAfterMove)
  {
    jianing::Circle circle {{10.0, 0.0}, 5.0};

    circle.move({-10.0, -20.0});

    BOOST_CHECK_CLOSE(5.0 * 2.0, circle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(5.0 * 2.0, circle.getFrameRect().height, accuracy);
    BOOST_CHECK_EQUAL(-10.0, circle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(-20.0, circle.getFrameRect().pos.y);
  }

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterMoveCirle_Bydxdy_CorrectFrameAfterMove)
  {
    jianing::Circle circle {{-5.1, 5.2}, 2.5};

    circle.move(5.0, -5.0);

    BOOST_CHECK_CLOSE(2.5 * 2.0, circle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(2.5 * 2.0, circle.getFrameRect().height, accuracy);
    BOOST_CHECK_CLOSE(-5.1 + 5.0, circle.getFrameRect().pos.x, accuracy);
    BOOST_CHECK_CLOSE(5.2 - 5.0, circle.getFrameRect().pos.y, accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test scale circle=====================
BOOST_AUTO_TEST_SUITE(ScaleCircle)

  BOOST_AUTO_TEST_CASE(ScaleCircle_OnWrongCoefficientNegativeOrZero_ThrowError)
  {
    jianing::Circle circle {{3.10, 4.15}, 9.26};

    BOOST_CHECK_THROW(circle.scale(0.0), std::domain_error);
    BOOST_CHECK_THROW(circle.scale(-1.8), std::domain_error);
  }

  BOOST_AUTO_TEST_CASE(ScaleCircle_OnCorrectCoefficient_CorrectRadiusAfterScale)
  {
    jianing::Circle circle {{3.3, -3.3}, 1.5};

    circle.scale(2.2);

    BOOST_CHECK_EQUAL(3.3, circle.getCenter().x);
    BOOST_CHECK_EQUAL(-3.3, circle.getCenter().y);
    BOOST_CHECK_CLOSE(1.5 * 2.2, circle.getRadius(), accuracy);
    BOOST_CHECK_CLOSE(M_PI * 1.5 * 2.2 * 1.5 *2.2, circle.getArea(), accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test scale circle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterScaleCircle)

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterScaleCircle_OnCorrectCoefficient_CorrectCenerRadius)
  {
    jianing::Circle circle {{3.3, -3.3}, 1.5};

    circle.scale(2.2);

    BOOST_CHECK_CLOSE(1.5 * 2.0 * 2.2, circle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(1.5 * 2.0 * 2.2, circle.getFrameRect().height, accuracy);
    BOOST_CHECK_EQUAL(3.3, circle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(-3.3, circle.getFrameRect().pos.y);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test rotate circle=====================
BOOST_AUTO_TEST_SUITE(RotateCircle)

  BOOST_AUTO_TEST_CASE(RotateCircle_InPositiveDirection_CorrectWigthHeightXYAfterRotate)
  {
    jianing::Circle circle {{2.4, -6.7}, 9.8};

    circle.rotate(73);

    BOOST_CHECK_EQUAL(2.4, circle.getCenter().x);
    BOOST_CHECK_EQUAL(-6.7, circle.getCenter().y);
    BOOST_CHECK_EQUAL(9.8, circle.getRadius());
    BOOST_CHECK_CLOSE(M_PI * 9.8 * 9.8, circle.getArea(), accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()

// =====================test rotate circle(FrameRect)=====================
BOOST_AUTO_TEST_SUITE(TestFrameRectAfterRotateCircle)

  BOOST_AUTO_TEST_CASE(TestFrameRectAfterRotateCircle_InOppositeDirection_CorrectFrameAfterRotate)
  {
    jianing::Circle circle {{-1.3, -3.4}, 4.7};

    circle.rotate(-974);

    BOOST_CHECK_EQUAL(-1.3, circle.getFrameRect().pos.x);
    BOOST_CHECK_EQUAL(-3.4, circle.getFrameRect().pos.y);
    BOOST_CHECK_CLOSE(4.7 * 2.0, circle.getFrameRect().width, accuracy);
    BOOST_CHECK_CLOSE(4.7 * 2.0, circle.getFrameRect().height, accuracy);
  }

BOOST_AUTO_TEST_SUITE_END()


#include "graycode.h"
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/math/special_functions/round.hpp>

bool CalculateGP(cv::Mat& absPhase, std::vector<cv::Mat> &images, int startGray, int endGray, int startPhase)
{
    cv::Mat gray_code(absPhase.rows, absPhase.cols, CV_8UC1);
    cv::Mat phase(absPhase.rows, absPhase.cols, CV_8UC1);
    cv::Mat mask(absPhase.rows, absPhase.cols, CV_8UC1);

    if(!CalculateGrayCodeImg(gray_code, images, startGray, endGray))
    {
        std::cout << "Error creating gray code image";
        return false;
    }
    cv::imwrite("graycode.jpg", gray_code);

    CalculateAbsolutePhase(phase, images, startPhase);

    cv::imwrite("phase.jpg", phase);

    MaskEvaluation(mask, images[startPhase], images[startPhase+1], images[startPhase+2], images[startPhase+3], 5, 255, 10); //defults
    BinaryAndOperation(phase, mask);
    BinaryAndOperation(gray_code, mask);

    cv::imwrite("phase2.jpg", phase);

    EvaluateAbsPhase(absPhase, phase, gray_code);

    return false;
}

bool CalculateGrayCodeImg( cv::Mat& code_img, std::vector<cv::Mat>& images, long StartIndex, long EndIndex )
{
    unsigned char *normal = NULL;
    unsigned char *invers = NULL;
    unsigned char *result = NULL;
    long           j, h, BitPlane;
    unsigned long  k;

    // Check if we have correct amount of images
    if( static_cast<long>(images.size()) <= EndIndex || (EndIndex-StartIndex+1)!=16 )
    {
        std::cout << "not enough images provided\n";
        return false;
    }

    for( int i=StartIndex; i<=EndIndex; i++ )
    {
        if( images[i].type() != CV_8UC1 )
        {
			cv::cvtColor(images[i], images[i], CV_BGR2GRAY);
            //std::cout << "Wrong image type\n";
            //return false;
        }
    }

    unsigned int width = images[0].cols;
    unsigned int height = images[0].rows;

    // Image size and format test
    if( code_img.type() != CV_8UC1 || code_img.cols != width || code_img.rows != height )
    {
        std::cout << "wrong format of gray image\n";
        return false;
    }

    for( unsigned int i=0; i < height; i++ )
    {
        result = (unsigned char*)code_img.row(i).data;
        memset(result, 0, code_img.cols );

        for( j=0; j<=7; j++ )
        {
            normal = (unsigned char*)images[2*j+StartIndex].row(i).data;
            invers = (unsigned char*)images[2*j+1+StartIndex].row(i).data;
            BitPlane = 1 << (7-j);

            for( k=0; k < width; k++ )
            {
                if (normal[k] > invers[k]){
                    result[k] = (unsigned char)( result[k] | BitPlane );
                }
            }
        }

        for( k=0; k < width; k++ )
        {
            h = result[k];
            // inverse graycode calculation
            result[k] = (unsigned char)( LinearCode( h & 255 ) );
        }
    }
    return true;
}

bool CalculateAbsolutePhase(cv::Mat& phase_img, std::vector<cv::Mat>& images, int offset)
{
    unsigned char *P;
    unsigned char *Q;
    unsigned char *S;
    unsigned char *T;
    unsigned char *result;

    long	i,j;
    long	A,B;
    double	Scale;
    long	ImgWidth, ImgHeight;

    ImgWidth = images[0].cols;
    ImgHeight = images[0].rows;

    if( phase_img.type() != CV_8UC1 || phase_img.cols != ImgWidth || phase_img.rows != ImgHeight )
    {
        std::cout << "wrong format of phase image";
        return false;
    }

    Scale = 128.0/M_PI;

    // all rows
    for( i=0; i<ImgHeight; i++ )
    {
        P = images[offset+0].row( i ).data;
        Q = images[offset+1].row( i ).data;
        S = images[offset+2].row( i ).data;
        T = images[offset+3].row( i ).data;
        result = (unsigned char*)phase_img.row( i ).data;
        // all columns
        for( j=0; j<ImgWidth; j++ ) {
            A = S[j] - P[j];
            B = T[j] - Q[j];
            if ((A == 0) && (B == 0)) result[j] = (unsigned char)(0);
            else result[j] = (unsigned char)boost::math::lround(atan2((double)A, (double)B)*Scale);
        }
    }
    return true;
}

void MaskEvaluation(cv::Mat& Mask, const cv::Mat& Phase1, const cv::Mat& Phase2, const cv::Mat& Phase3, const cv::Mat& Phase4,
                           int DynamicThreshold, int MaximumThreshold, int SinusThreshold )
{
    unsigned char* P;
    unsigned char* Q;
    unsigned char* S;
    unsigned char* T;
    unsigned char* R;
    long W,H,I,J;
    long Min,Max;

    W = (long)(Mask.cols);
    H = (long)(Mask.rows);

    for (I = 0; I < H; I++)
    {
        P = (unsigned char*) Phase1.row(I).data;
        Q = (unsigned char*) Phase2.row(I).data;
        S = (unsigned char*) Phase3.row(I).data;
        T = (unsigned char*) Phase4.row(I).data;
        R = (unsigned char*) Mask.row(I).data;

        for (J = 0; J < W; J++)
        {
            Min = (*P);
            Max = (*P);
            if ((*Q) < Min) Min = (*Q); else if ((*Q) > Max) Max = (*Q);
            if ((*S) < Min) Min = (*S); else if ((*S) > Max) Max = (*S);
            if ((*T) < Min) Min = (*T); else if ((*T) > Max) Max = (*T);

            if( (Max-Min)>=DynamicThreshold )
            {
                if( MaximumThreshold==255 ) {
                    if( (*P) <MaximumThreshold && (*Q)<MaximumThreshold && (*S)<MaximumThreshold && (*T)<MaximumThreshold )
                        *R = (unsigned char)(255);
                    else if( abs( ( (*P) + (*S) ) - ( (*Q) + (*T) ) ) <= SinusThreshold )	//ist die Phase sinusf\F6rmig?
                        *R = (unsigned char)(255);
                    else
                        *R = (unsigned char)(0);
                }
                else {
                    if( (*P)<=MaximumThreshold && (*Q)<=MaximumThreshold && (*S)<=MaximumThreshold && (*T)<=MaximumThreshold )
                        *R = (unsigned char)(255);
                    else
                        *R = (unsigned char)(0);
                }
            }
            else *R = (unsigned char)(0);

            P++; Q++; S++; T++; R++;
        }
    }
}

unsigned int BinaryAND(cv::Mat& dst, const cv::Mat& src)
{
    unsigned char *S;
    unsigned char *D;
    long i;
    long j;
    long H, W;
    unsigned int errval;

    W = (long)( dst.cols );
    H = (long)( dst.rows );

    // rows
    for( i = 0; i < H; i++)
    {
        D = (unsigned char*) dst.row(i).data;
        S = (unsigned char*) src.row(i).data;
        // cols
        for (j = 0; j < W; j++)
        {
            unsigned char dval = (D[j]);
            unsigned char sval = (S[j]);
            D[j] = dval & sval;
            //D[j] &= S[j];
        }
    }
    return 0;
}

void EvaluateAbsPhase(cv::Mat& AbsPhase, cv::Mat& Phase, cv::Mat& GCode)
{
    unsigned char* GC;
    unsigned char* Ph;
    unsigned short* APh;
    long I,j;
    long c,p,PRI;
    long W,H;

    W = (long)(GCode.cols);
    H = (long)(GCode.rows);

    PRI = 64;

    for(j=0; j < H  ; j++)
    {
        GC = (unsigned char*)GCode.row(j).data;
        Ph = (unsigned char*)Phase.row(j).data;
        APh = (unsigned short*)AbsPhase.row(j).data;

        for (I = 0; I < W; I++)
        {
            c = (*GC);
            p = (char)(*Ph);
            if ((c < 1)||(c > 254))
            {
                *APh = 0;
            }
            else
            {
                if (p < -PRI)
                {
                    if (c & 1)
                    {
                        c++;
                    }
                }
                else
                {
                    if (p > PRI)
                    {
                        if (!(c & 1))
                        {
                            c--;
                        }
                    }
                }
                *APh = (unsigned short)(((c >> 1) << 8) + p);
            }
            GC++; Ph++; APh++;
        }// endfor
    }// end for j
}

unsigned long LinearCode(unsigned long n)
{
    unsigned long idiv;
    int ish = 1;
    unsigned long ans = n;

    for(;;)
    {
        ans ^= ( idiv = ans >> ish );
        if( idiv <= 1 || ish == 16 )
            return ans;
        ish <<= 1;
    }
}

double BLInterpolate(double x, double y, const cv::Mat &Phase)
{
    //bilinear interpolation
    double dx = x-(int)x;
    double dy = y-(int)y;

    short ptl = Phase.at<short>((int)y , (int)x);
    short ptr = Phase.at<short>((int)y, (int)x+1);
    short pbl = Phase.at<short>((int)y+1, (int)x);
    short pbr = Phase.at<short>((int)y+1, (int)x+1);

    double weight_tl = (1.0 - dx) * (dy);
    double weight_tr = (dx)       * (dy);
    double weight_bl = (1.0 - dx) * (1.0 - dy);
    double weight_br = (dx)       * (1.0 - dy);

    return (ptl*weight_tl)+(ptr*weight_tr)+(pbl*weight_bl)+(pbr*weight_br);
}

unsigned int BinaryAndOperation(cv::Mat& dst, const cv::Mat& src)
{
    unsigned char *S;
    unsigned char *D;
    long i;
    long j;
    long H, W;
    unsigned int errval;

    // check image compatibility
    //if( (errval = CompatImages( dst, src, PIX_8BIT ) ) != OK ) return errval;

    W = (long)( dst.cols );
    H = (long)( dst.rows );

    // rows
    for( i = 0; i < H; i++)
    {
        D = (unsigned char*) dst.row(i).data;
        S = (unsigned char*) src.row(i).data;
        // cols
        for (j = 0; j < W; j++)
        {
            unsigned char dval = (D[j]);
            unsigned char sval = (S[j]);
            D[j] = dval & sval;
            //D[j] &= S[j];
        }
    }
    return 0;
}


// -*- coding: utf-8 -*-
#define CATCH_CONFIG_MAIN

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <catch.hpp>
#include <netoptim/min_cycle_ratio.hpp>
#include <netoptim/neg_cycle.hpp> // import negCycleFinder
#include <py2cpp/nx2bgl.hpp>
#include <utility> // for std::pair

using graph_t = boost::adjacency_list<
    boost::listS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_weight_t, int, boost::property<boost::edge_index_t, int>>>;
using Vertex  = boost::graph_traits<graph_t>::vertex_descriptor;
using Edge_it = boost::graph_traits<graph_t>::edge_iterator;

static xn::grAdaptor<graph_t> create_test_case1()
{
    using Edge           = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge    edge_array[] = {Edge(A, B), Edge(B, C), Edge(C, D), Edge(D, E), Edge(E, A)};
    int            weights[]    = {-5, 1, 1, 1, 1};
    int            num_arcs     = sizeof(edge_array) / sizeof(Edge);
    static graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t>(g);
}

static xn::grAdaptor<graph_t> create_test_case2()
{
    using Edge           = std::pair<int, int>;
    const auto num_nodes = 5;
    enum nodes
    {
        A,
        B,
        C,
        D,
        E
    };
    static Edge    edge_array[] = {Edge(A, B), Edge(B, C), Edge(C, D), Edge(D, E), Edge(E, A)};
    int            weights[]    = {2, 1, 1, 1, 1};
    int            num_arcs     = sizeof(edge_array) / sizeof(Edge);
    static graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t>(g);
}

static xn::grAdaptor<graph_t> create_test_case_timing()
{
    using Edge           = std::pair<int, int>;
    const auto num_nodes = 3;
    enum nodes
    {
        A,
        B,
        C
    };
    static Edge    edge_array[] = {Edge(A, B), Edge(B, A), Edge(B, C), Edge(C, B),
                                Edge(B, C), Edge(C, B), Edge(C, A), Edge(A, C)};
    int            weights[]    = {7, 0, 3, 1, 6, 4, 2, 5};
    int            num_arcs     = sizeof(edge_array) / sizeof(Edge);
    static graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
    return xn::grAdaptor<graph_t>(g);
}

bool do_case(xn::grAdaptor<graph_t>& G)
{
    using edge_t = typename xn::grAdaptor<graph_t>::edge_t;

    auto get_weight = [](const xn::grAdaptor<graph_t>& G, const edge_t& e) -> int {
        const auto& weightmap = boost::get(boost::edge_weight, G);
        return weightmap[e];
    };

    negCycleFinder N(G, get_weight);
    auto           cycle = N.find_neg_cycle();
    return !cycle.empty();
}

TEST_CASE("Test Negative Cycle", "[test_neg_cycle]")
{
    xn::grAdaptor<graph_t> G = create_test_case1();
    // boost::property_map<graph_t, boost::edge_weight_t>::type weightmap =
    // boost::get(boost::edge_weight, G); std::vector<Vertex>
    // p(boost::num_vertices(G));
    bool hasNeg = do_case(G);
    CHECK(hasNeg);

    // G = xn::path_graph(5, create_using=xn::DiGraph());
    // hasNeg = do_case(G);
    // CHECK(!hasNeg);
}

TEST_CASE("Test No Negative Cycle", "[test_neg_cycle]")
{
    xn::grAdaptor<graph_t> G      = create_test_case2();
    bool                   hasNeg = do_case(G);
    CHECK(!hasNeg);
}

TEST_CASE("Test Timing Graph", "[test_neg_cycle]")
{
    xn::grAdaptor<graph_t> G      = create_test_case_timing();
    bool                   hasNeg = do_case(G);
    CHECK(!hasNeg);
}


/**
* @file: matrix_generator.hpp
* @brief:
* @author: Changjiang Cai, ccai1@stevens.edu, caicj5351@gmail.com
* @version: 0.0.1
* @creation date: 17-12-2015
* @last modified: Thu 10 Mar 2016 09:32:02 AM EST
*/

// some of the codes are generated by Boxiang Do.
#ifndef matrix_generator_hpp
#define matrix_generator_hpp

#include <stdio.h>
#include <vector>
#include <map>
#include <algorithm>
#include "truth_discovery.hpp"
#include <random>
#include <boost/math/special_functions/beta.hpp>

#define UNIFORM_DISTRIBUTION 0
#define NORMAL_DISTRIBUTION 1
#define NEARLY_ZERO 1.0E-5

#define LOW_QUALITY 0
#define MEDIUM_QUALITY 1
#define HIGH_QUALITY 2

#define low_guassian 1
#define medium_guassian  0.1
#define high_guassian  0.01
#define high_uniform_left  0
#define high_uniform_right 0.2
#define medium_uniform_left  0.1
#define medium_uniform_right 0.3
#define low_uniform_left  0.2
#define low_uniform_right  0.4

/*
double low_guassian = 1;
double medium_guassian = 0.1;
double high_guassian = 0.01;
double high_uniform_left = 0;
double high_uniform_right = 0.2;
double medium_uniform_left = 0.1;
double medium_uniform_right = 0.3;
double low_uniform_left = 0.2;
double low_uniform_right = 0.4;
*/

using namespace std;
bool generate_sim_vote_for_one_task_for_CrowdBT(
	const int & n, // vertex number
	const int & rank_o_i,
	const int & rank_o_j, // less rank is better.
	const double & a, //Reta distribution parameter.
	const double & b //Beta distribution parameter.
	);

bool generate_sim_vote_for_one_task(
	const int & n, // vertex number
	const int & rank_o_i,
	const int & rank_o_j, // less rank is better.
	const double& stddev /*Gaussian variance to control the worker's quality.*/
	);

vector<vector<int>> generate_sim_vote(const int& n, bool **GT,
	// ground_truth: <key = object_i, value = rank of object_i>;
	const map<int, int>& ground_truth,
	const double& stddev, /*Gaussian variance to control the worker's quality.*/
	const int & max_per_worker // maximum number of pairwise comparison per worker can do.
	// if max_per_worker < 0, that means doing all the pairwise comparasions.
	);

// for all the workers, gathering their voting results.
vector<vector<vector<int>>> generate_sim_votes(
	const int& n, const int& w, bool **GT,
	const map<int, int>& ground_truth, const int& distribution,
	const int& quality, // different levels of error-rate for the worker's quality.
	const int & max_per_worker /*maximum number of pairwise comparison per worker can do.*/
	);

void generate_sim_votes_taskID_who(
	const int& n, const int& w, bool **GT,
	const vector<vector<vector<int>>> & matrice,
	// store the tasks;
	// The task-IDs' are assigned based on way the double-for-loop is executed.
	vector<vector <td_wieghts::client_vote>> & v_client_votes
	);

vector<vector <td_wieghts::client_vote>>
generate_aggregated_matrix_SATD(bool **GT,
	const vector<vector<vector<int>>> & matrice,
	std::vector<std::vector<double>> & gp,
	vector<double> & worker_weights,
	td_wieghts::truth_discovery * td,
    const int & max_task_num,
	int & count, // Truth discovery execution times.
	/*Smothing_Type : */
	const int & smothingType = DIRECT_USE_WEIGHTS
	);

double small_prob(const double & worker_weight, const double & mean,
	const double & prob_threshold, /*a threshold of the small_prob to be generated.*/
	const double & stddev_scalar);

void Smoothing_UpdateProb_with_Workers_Quality(
	const vector<double> & worker_weights,
	bool **GT, const int & n, /*object number*/
	const double & prob_threshold, /*a threshold of the small_prob to be generated.*/
	const double & mean, const double & stddev_scalar,
	const vector<vector <td_wieghts::client_vote>> & v_client_votes,
	vector<vector<double>> & gtc);

/* You have to specify the default values
* for the arguments only in the declaration
* but not in the definition.*/
std::vector<std::vector<double> > generate_aggregated_matrix(
	const int& n, const int& w, bool **GT,
	const std::map<int, int>& ground_truth, const int& distribution,
	const int& quality, // different levels of error-rate for the worker's quality.
	const int & max_per_worker, // maximum number of pairwise comparison per worker can do.
	/*Smothing_Type : */
	const int & Smothing_Type = DIRECT_USE_WEIGHTS
	);

std::vector<std::vector<double> >
generate_aggregated_matrix(
const std::vector<std::vector<std::vector<int>>> & matrice,
/*Smothing_Type : */
const int & smothingType = DIRECT_USE_WEIGHTS
);

// added by CCJ
std::vector<std::vector<int> >
generate_aggregated_voting_matrix(const vector<vector<vector<int>>> &  matrice);

#endif /* matrix_generator_hpp */


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_uncalibrated_absolute_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/rotation.h>
#include <memory>
#include <vector>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/four_point_focal_length.h"
#include "theia/sfm/types.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing the uncalibrated absolute pose from 4 feature
// correspondences. The feature correspondences should be normalized such that
// the principal point is at (0, 0).
class UncalibratedAbsolutePoseEstimator
    : public Estimator<FeatureCorrespondence2D3D, Matrix3x4d> {
 public:
  UncalibratedAbsolutePoseEstimator() {}

  // 3 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 4; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<Matrix3x4d>* absolute_poses) const {
    const std::vector<Eigen::Vector2d> features = {correspondences[0].feature,
                                                   correspondences[1].feature,
                                                   correspondences[2].feature,
                                                   correspondences[3].feature};
    const std::vector<Eigen::Vector3d> world_points = {
        correspondences[0].world_point,
        correspondences[1].world_point,
        correspondences[2].world_point,
        correspondences[3].world_point};

    const int num_solutions =
        FourPointPoseAndFocalLength(features, world_points, absolute_poses);
    return num_solutions > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const Matrix3x4d& absolute_pose) const {
    // The reprojected point is computed as R * (X - c) where R is the camera
    // rotation, c is the position, and X is the 3D point.
    const Eigen::Vector2d reprojected_feature =
        (absolute_pose * correspondence.world_point.homogeneous())
            .eval()
            .hnormalized();
    return (reprojected_feature - correspondence.feature).squaredNorm();
  }

 private:
  DISALLOW_COPY_AND_ASSIGN(UncalibratedAbsolutePoseEstimator);
};

}  // namespace

bool EstimateUncalibratedAbsolutePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    UncalibratedAbsolutePose* absolute_pose,
    RansacSummary* ransac_summary) {
  UncalibratedAbsolutePoseEstimator absolute_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<UncalibratedAbsolutePoseEstimator> >
      ransac = CreateAndInitializeRansacVariant(
          ransac_type, ransac_params, absolute_pose_estimator);
  // Estimate the absolute pose.
  Matrix3x4d projection_matrix;
  const bool success = ransac->Estimate(
      normalized_correspondences, &projection_matrix, ransac_summary);

  // Recover the focal length and pose.
  Eigen::Matrix3d calibration_matrix;
  Eigen::Vector3d rotation;
  DecomposeProjectionMatrix(projection_matrix,
                            &calibration_matrix,
                            &rotation,
                            &absolute_pose->position);

  // Convert angle-axis rotation to rotation matrix.
  ceres::AngleAxisToRotationMatrix(
      rotation.data(),
      ceres::ColumnMajorAdapter3x3(absolute_pose->rotation.data()));

  absolute_pose->focal_length =
      calibration_matrix(0, 0) / calibration_matrix(2, 2);

  return success;
}

}  // namespace theia


/*
 * Copyright (c) 2013-, Stephen Miller
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms,
 * with or without modification, are permitted provided
 * that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the
 * above copyright notice, this list of conditions
 * and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the
 * above copyright notice, this list of conditions and
 * the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the
 * names of its contributors may be used to endorse or
 * promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS
 * AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
 * THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <pcl/console/print.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/pcl_macros.h>

#include <boost/filesystem/convenience.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <string>
#include <vector>

void
getIntrinsics (const pcl::PointCloud<pcl::PointXYZ> &cloud,
              float &fx,
              float &fy,
              float &cx,
              float &cy,
              float &reproj_error)
{
  Eigen::MatrixXf A = Eigen::MatrixXf::Zero (2*cloud.size (), 4);
  Eigen::VectorXf b = Eigen::VectorXf::Zero (2*cloud.size ());
  size_t idx = 0;
  float minX, minY, minZ;
  minX = minY = minZ = std::numeric_limits<float>::infinity();
  float maxX, maxY, maxZ;
  maxX = maxY = maxZ = -std::numeric_limits<float>::infinity();
  for (int x = 0 ; x < cloud.width; x++)
  {
    for (int y = 0; y < cloud.height; y++)
    {
      const pcl::PointXYZ &pt = cloud (x,y);
      if (pcl_isnan (pt.x) || pcl_isnan (pt.y) || pcl_isnan (pt.z) || (pt.x == 0) || (pt.y == 0))
        continue;
      if (pt.x > maxX) maxX = pt.x;
      if (pt.y > maxY) maxY = pt.y;
      if (pt.z > maxZ) maxZ = pt.z;
      if (pt.x < minX) minX = pt.x;
      if (pt.y < minY) minY = pt.y;
      if (pt.z < minZ) minZ = pt.z;
      A (idx, 0) = pt.z;
      A (idx, 2) = pt.x;
      b (idx) = pt.z * x;
      idx++;
      A (idx, 1) = pt.z;
      A (idx, 3) = pt.y;
      b (idx) = pt.z * y;
      idx++;
    }
  }
  // Solve A*x = b;
  Eigen::Vector4f X;
  X = (A.transpose () * A).inverse () * A.transpose () * b;
  cx = X (0);
  cy = X (1);
  fx = X (2);
  fy = X (3);
  reproj_error = (A*X - b).squaredNorm () / (fx*fx*idx/2);
  PCL_INFO ("Bounds:\n");
  PCL_INFO ("X: [%f, %f]\n",minX,maxX);
  PCL_INFO ("Y: [%f, %f]\n",minY,maxY);
  PCL_INFO ("Z: [%f, %f]\n",minZ,maxZ);
}

int
main (int argc, char** argv)
{
  if (argc != 2)
  {
    PCL_INFO ("Usage: %s input_organized_cloud.pcd\n", argv[0]);
    return (1);
  }
  std::string pcd_file = argv[1];
  pcl::PointCloud<pcl::PointXYZ> cloud;
  PCL_INFO ("Loading cloud %s\n", pcd_file.c_str ());
  pcl::io::loadPCDFile (pcd_file, cloud);
  float fx, fy, cx, cy, reproj_error;
  getIntrinsics (cloud, fx, fy, cx, cy, reproj_error);
  PCL_INFO ("Width: %d\n", cloud.width);
  PCL_INFO ("Height: %d\n", cloud.height);
  PCL_INFO ("fx: %f\n", fx);
  PCL_INFO ("fy: %f\n", fy);
  PCL_INFO ("cx: %f\n", cx);
  PCL_INFO ("cy: %f\n", cy);
  PCL_INFO ("Total reprojection error: %f\n", reproj_error);
  return (0);
}


#include <cmath>
#include <boost/math/distributions/negative_binomial.hpp>
#include "dist.hpp"

namespace nbinomPlotPy {
    Density get_pdf(RealType size, RealType prob, RealType upper, RealType step) {
        Density density;

        try {
            if (step > 0) {
                boost::math::negative_binomial_distribution<> dist(size, prob);
                for (RealType x = 0.0; x <= upper; x += step) {
                    density.push_back(boost::math::pdf(dist, x));
                }
            }
        } catch (std::exception& e) {
            // Bad size or prob parameters
        }

        return density;
    }
}

/*
Local Variables:
mode: c++
coding: utf-8-unix
tab-width: nil
c-file-style: "stroustrup"
End:
*/


#include <vector>

#include <glm/glm.hpp>

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

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    const Vec3 v[4] = {Vec3(0), Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    const Mat3x2 m[3][3] = {
        { Mat3x2(v[1], v[0]), Mat3x2(v[2], v[0]), Mat3x2(v[3], v[0])},
        { Mat3x2(v[0], v[1]), Mat3x2(v[0], v[2]), Mat3x2(v[0], v[3])},
        { -Mat3x2(v[1], v[1]), -Mat3x2(v[2], v[2]), -Mat3x2(v[3], v[3])}
    };
}

namespace BalloonFEM
{
    void Engine::computeFilmForces(ObjState &state, Vvec3 &f_sum)
    {
        Vvec3 &pos = state.world_space_pos;

		/* compute film elastic force */

		for (MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
		{
			for (PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
			{
				iVec3 &id = p->v_id;
				Vec3 &v0 = pos[id[0]];
				Vec3 &v1 = pos[id[1]];
				Vec3 &v2 = pos[id[2]];

				/* calculate deformation in world space */
				Mat3x2 Ds = Mat3x2(v0 - v2, v1 - v2);

				/* calculate deformation gradient */
				Mat3x2 F = Ds * p->Bm;

				/* calculate Piola for this tetra */
				Mat3x2 P = m_film_model->Piola(F);

				/* calculate forces contributed from this tetra */
				Mat3x2 H = - p->volume() * P * transpose(p->Bm);

				f_sum[id[0]] += H[0];
				f_sum[id[1]] += H[1];
				f_sum[id[2]] -= H[0] + H[1];
			}
		}

    }

    SpMat Engine::computeFilmDiffMat(ObjState &state)
    {
       	printf("building film force differential matrix \n");
		/* project from constrained freedom state to world space */
		Vvec3 &pos = state.world_space_pos;

	    std::vector<T> coefficients;
		coefficients.clear();
		size_t count_film = 0;
		for (size_t i = 0; i < m_tetra->films.size(); i++)
			count_film += m_tetra->films[i].pieces.size();
		coefficients.reserve( 9 * 9 * count_film);

		for (MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
			for (PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
			{
				/* assgin world space position */
				iVec3 &id = p->v_id;
				Vec3 &v0 = pos[id[0]];
				Vec3 &v1 = pos[id[1]];
				Vec3 &v2 = pos[id[2]];

				/* calculate deformation in world space */
				Mat3x2 Ds = Mat3x2(v0 - v2, v1 - v2);

				/* calculate deformation gradient */
				Mat3x2 F = Ds * p->Bm;

				/* i is index of vertex, j is index of dimention */
				for (size_t i = 0; i < 3; i++)
				for (size_t j = 0; j < 3; j++)
				{
					/* calculate delta deformation in world space */
					Mat3x2 dDs = m[i][j];

					/* calculate delta deformation gradient */
					Mat3x2 dF = dDs * p->Bm;

					/* calculate delta Piola */
					Mat3x2 dP = m_film_model->StressDiff(F, dF);

					/* calculate forces contributed from this tetra */
					Mat3x2 dH = - p->volume() * dP * transpose(p->Bm);

					for (size_t w = 0; w < 2; w++)
					for (size_t l = 0; l < 3; l++)
						coefficients.push_back(T(3 * id[w] + l, 3 * id[i] + j, dH[w][l]));

					Vec3 df_3 = - dH[0] - dH[1];
					for (size_t l = 0; l < 3; l++)
						coefficients.push_back(T(3 * id[2] + l, 3 * id[i] + j, df_3[l]));
				}
			}
		}

		SpMat E( 3 * pos.size(), 3 * pos.size());
		E.setFromTriplets(coefficients.begin(), coefficients.end());

        return E;
    }
}


#include "cor_algorithm/sources/utilities.h"
#include "cor_system/sources/logger.h"
//#include "cor_type/sources/math/vector2_tmpl_impl.h"
//#include "cor_type/sources/primitive/box_tmpl_impl.h"
#include "cor_type/sources/primitive/o_box.h"

#define BOOST_TEST_NO_LIB
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(obox)

BOOST_AUTO_TEST_CASE(obox2d)
{

    typedef cor::type::OBox2F B;

    B b0(B::Matrix::rot_z(0.1f) * B::Matrix::translate(0.0f, 0.0f, 0.0f), B::Box(-0.5f, -0.5f, 1.0f, 1.0f));

    auto va = b0.get_vertices();

    cor::RInt32 i;
    cor::RInt32 j;

    for(i = 0 ; i < 2 ; i++)
    {
        for(j = 0 ; j < 2 ; j++)
        {
            B b1(B::Matrix::rot_z(0.1f) * B::Matrix::translate(j - 0.5f, i - 0.5f, 0.0f), B::Box(-0.5f, -0.5f, 1.0f, 1.0f));
            BOOST_CHECK(b0.is_cross(b1));

        }

    }

    for(i = 0 ; i < 2 ; i++)
    {
        for(j = 0 ; j < 2 ; j++)
        {
            B b1(B::Matrix::rot_z(0.1f) * B::Matrix::translate(j  * 3 - 1.5f, i * 3 - 1.5f, 0.0f), B::Box(-0.5f, -0.5f, 1.0f, 1.0f));
            BOOST_CHECK(!b0.is_cross(b1));

        }

    }

}

BOOST_AUTO_TEST_CASE(obox2d_distance)
{
    typedef cor::type::OBox2F B;
    typedef cor::type::Vector2F V;

    B b0(B::Matrix::translate(0.0f, 0.0f, 0.0f) *
        B::Matrix::rot_z(0.1f) * B::Matrix::scale(1.5f, 1.5f, 1.0f),
            B::Box(-0.5f, -0.5f, 1.0f, 1.0f));

    V v(cosf(0.1f) - sinf(0.1f), sinf(0.1f) + cosf(0.1f));
    auto d = b0.get_distance(v);
    BOOST_CHECK_CLOSE(d, 0.5f / sqrtf(2.0f), 0.0001f);

}

BOOST_AUTO_TEST_CASE(obox2d_aabb)
{
    typedef cor::type::OBox2F B;
    typedef cor::type::Vector2F V;

    auto rot = cor::PI / 3;
    B b0(B::Matrix::translate(0.0f, 1.0f, 0.0f) *
        B::Matrix::rot_z(rot) * B::Matrix::scale(2.0f, 2.0f, 2.0f),
            B::Box(-0.5f, -0.5f, 1.0f, 1.0f));

    auto aabb = b0.get_aabb();

    V v((cosf(rot) + sinf(rot)), (sinf(rot) + cosf(rot) + 1.0f));

    BOOST_CHECK_CLOSE(aabb.get_max().x, v.x, 0.0001f);
    BOOST_CHECK_CLOSE(aabb.get_max().y, v.y, 0.0001f);

}

BOOST_AUTO_TEST_SUITE_END()


/*
Copyright 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Test lexicographical.hpp with an example: the Viterbi semiring.
*/

#define BOOST_TEST_MODULE test_math_lexicographical_fast
#include "utility/test/boost_unit_test.hpp"

#include "math/lexicographical.hpp"

#include <string>
#include <ostream>
#include <iostream>

#include <boost/mpl/assert.hpp>

#include "range/std/container.hpp"

#include "math/max_semiring.hpp"
#include "math/sequence.hpp"
#include "math/cost.hpp"

BOOST_AUTO_TEST_SUITE (test_suite_lexicographical_viterbi)

/**
Two paths diverge in a wood.
We are a computer, so we can travel both.
One path has (1, a) and (1/2, b) as labels.
This is the path more travelled by.
The other path has (1/4, c) and (1, d) as labels.
This is the path less travelled by.
Way leads on to way, and both paths lead to the same place.
This is a transition with (1/4, e) as a label.

The Viterbi semiring, summing over all paths, should return the complete path
more travelled by.
That complete path has (1/8, abe) as its label.
The other path, at (1/16, cde), is worse.
*/
BOOST_AUTO_TEST_CASE (test_viterbi) {
    typedef math::lexicographical <math::over <
        math::max_semiring <float>,
        math::single_sequence <char>>> viterbi_semiring;

    viterbi_semiring v_1_a (1, 'a');
    viterbi_semiring v_0_5_b (0.5, 'b');
    viterbi_semiring v_0_25_c (0.25, 'c');
    viterbi_semiring v_1_d (1, 'd');
    viterbi_semiring v_0_25_e (0.25, 'e');

    auto result = ((v_1_a * v_0_5_b) + (v_0_25_c * v_1_d)) * v_0_25_e;
    std::cout << result << std::endl;
    BOOST_MPL_ASSERT ((std::is_same <decltype (result),
        math::lexicographical <math::over <
            math::max_semiring <float>, math::sequence <char>>>>));
    BOOST_CHECK_EQUAL (range::at_c <0> (result.components()).value(), .125);
    BOOST_CHECK (range::at_c <1> (result.components())
        == math::sequence <char> (std::string ("abe")));
}

/**
Now we are Frost, and we use a cost.
That makes all the difference.
We then take the path less travelled by.
Its label is (1.5, cde).
That is better than (1.75, abe).
*/
BOOST_AUTO_TEST_CASE (test_cost) {
    typedef math::lexicographical <math::over <
        math::cost <float>,
        math::single_sequence <char>>> cost_semiring;

    cost_semiring v_1_a (1, 'a');
    cost_semiring v_0_5_b (0.5, 'b');
    cost_semiring v_0_25_c (0.25, 'c');
    cost_semiring v_1_d (1, 'd');
    cost_semiring v_0_25_e (0.25, 'e');

    auto result = ((v_1_a * v_0_5_b) + (v_0_25_c * v_1_d)) * v_0_25_e;
    std::cout << result << std::endl;
    BOOST_MPL_ASSERT ((std::is_same <decltype (result),
        math::lexicographical <math::over <
            math::cost <float>, math::sequence <char>>>>));
    BOOST_CHECK_EQUAL (range::at_c <0> (result.components()).value(), 1.5);
    BOOST_CHECK (range::at_c <1> (result.components())
        == math::sequence <char> (std::string ("cde")));
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Math

#include "math.hpp"

#include <iostream>
#include <ostream>
#include <sstream>
#include <boost/test/included/unit_test.hpp>

using namespace Math;

const quat<float> R0{0,0,0,0}, R1{1,0,0,0},
	  i{0,1,0,0}, j{0,0,1,0}, k{0,0,0,1};
const dual<float> E0{R0}, E{R0, 1},
	  Ei = i*E, Ej = j*E, Ek = k*E;

BOOST_AUTO_TEST_CASE(quaternions) {
	BOOST_REQUIRE(i*j == k);
	BOOST_REQUIRE(j*i == -k);
	BOOST_REQUIRE(i*k == -j);
	BOOST_REQUIRE(k*i == j);
	BOOST_REQUIRE(j*k == i);
	BOOST_REQUIRE(k*j == -i);
	BOOST_REQUIRE(i*i == -R1);
	BOOST_REQUIRE(j*j == -R1);
	BOOST_REQUIRE(k*k == -R1);
}

BOOST_AUTO_TEST_CASE(dual_quaternions) {
	BOOST_REQUIRE(E != 0);
	BOOST_REQUIRE(E*E == 0);
	BOOST_REQUIRE(k*E*i == k*i*E);
	BOOST_REQUIRE(k*(E*i) == k*E*i);
	BOOST_REQUIRE(k*(E*i) == E*(k*i));
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <numeric>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) (a).begin(), (a).end()
#define AALL(a, n) (a), ((a) + (n))
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define MODNUM (INT(1e9 + 7))
#define MOD(x) ((x) % MODNUM)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

const int INF = 2e9;
const double EPS = 1e-10;
const double PI = acos(-1.0);

const int dx[] = {-1, 0, 1, 0};
const int dy[] = {0, -1, 0, 1};

template <typename T>
int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T>
void chmax(T& m, T x) {
	m = max(m, x);
}

template <typename T>
void chmin(T& m, T x) {
	m = min(m, x);
}

template <typename T>
T square(T x) {
	return x * x;
}

inline int toInt(string s) {
	int v;
	istringstream sin(s);
	sin >> v;
	return v;
}

// modを取りつつ二項係数を計算する関数を返す
auto make_mod_comb(long long mod) {
	const int COMB_MAX = 1100000;
	vector<long long> fact(COMB_MAX);
	vector<long long> fact_inv(COMB_MAX);
	vector<long long> inv(COMB_MAX);

	fact[0] = fact[1] = 1;
	fact_inv[0] = fact_inv[1] = 1;
	inv[1] = 1;

	for(int i = 2; i < COMB_MAX; i++) {
		fact[i] = (fact[i - 1] * i) % mod;
		inv[i] = mod - (inv[mod % i] * (mod / i)) % mod;
		fact_inv[i] = (fact_inv[i - 1] * inv[i]) % mod;
	}

	return [mod = mod, fact = move(fact), fact_inv = move(fact_inv)](
			   const long long n, const long long r) {
		if(n < r || n < 0 || r < 0) {
			return 0LL;
		}
		return (fact[n] * ((fact_inv[r] * fact_inv[n - r]) % mod)) % mod;
	};
}

int main() {
	int r1, c1, r2, c2;
	scanf("%d %d %d %d", &r1, &c1, &r2, &c2);
	auto mod_comb = make_mod_comb(MODNUM);
	ll result = 0;
	RANGE(i, r1, r2 + 1) {
		RANGE(j, c1, c2 + 1) { result = MOD(result + mod_comb(i + j, i)); }
	}
	printf("%lld\n", result);
	return 0;
}


#include "options.hpp"
#include "option_error.hpp"

#include <boost/format.hpp>

#include <cmath>

static inline bool is_sane(double x) {
	return !(std::isnan(x) || std::isinf(x));
}

static inline bool is_power_of_2(unsigned x) {
	return (x & (x - 1u)) == 0u;
}

void options::check() {
#define THROW_INVALID(x) throw option_error((boost::format("invalid value for '" #x "' - %d") % (x)).str())

	if ((size < 64u) || !is_power_of_2(size)) {
		THROW_INVALID(size);
	}

	if (!is_sane(resolution) || (resolution <= 0.0)) {
		THROW_INVALID(resolution);
	}

	if (!is_sane(brightness) || (brightness <= 0.0)) {
		THROW_INVALID(brightness);
	}

	if (!is_sane(start_frequency) || (start_frequency < 0.0)) {
		THROW_INVALID(start_frequency);
	}

	if (!is_sane(cut_down_frequency) || (cut_down_frequency <= 0.0) ||
		(cut_down_frequency - start_frequency < 1.0)) {
		THROW_INVALID(cut_down_frequency);
	}

#undef THROW_INVALID
}


#include "sargparse/File.h"
#include "sargparse/Parameter.h"

#include "stl/STLParser.h"
#include "global_parameters.h"

#include "tetrahedron.h"
#include "transformer.h"

#include <iostream>
#include <vector>
#include <armadillo>

void print_mass_properties();
sargp::Command cmd { "mass_properties", "show mass properties", print_mass_properties };

auto totalMass = cmd.Parameter<std::optional<double>>({}, "total_mass", "total mass of the object (used to calculate mass properties)");
auto density = cmd.Parameter<std::optional<double>>({}, "density", "density of the object (used to calculate mass properties)");
auto tensorPerspectiveOrigin = cmd.Flag("tensor_from_origin", "print the tensor from the perspective of the origin frame");

auto skew(arma::colvec3 const& v)
{
    return arma::mat33 {
        { 0, -v(2), v(1) },
        { v(2), 0, -v(0) },
        { -v(1), v(0), 0 },
    };
}

void print_mass_properties()
{
    if (not inFiles) {
        throw std::runtime_error("in has to be specified!");
    }

    kinematicTree::visual::stl::STLParser parser;
    kinematicTree::visual::mesh::Mesh mesh;
    for (auto const& file : *inFiles) {
        std::cout << "loading : " << file << "\n";
        auto subMesh = parser.parse(file);
		for (auto const& facet : subMesh.getFacets()) {
			mesh.addFacet(facet);
		}
    }

	auto transform = getTransform();
    transform.print("applying transform");
    mesh.applyTransform(transform);

    arma::mat33 inertia_tensor = arma::zeros(3, 3);
    double totalVolume {};
    arma::colvec3 com {};

    for (auto const& facet : mesh.getFacets()) {
        if (facet.mVertices.size() < 3) {
            continue;
        }
        auto const& base = facet.mVertices[0];
        for (auto i { 1 }; i < facet.mVertices.size() - 1; ++i) {
            auto tetrahedron = Tetrahedron { { base, facet.mVertices[i], facet.mVertices[i + 1] } };

            totalVolume += tetrahedron.volume;
            com += tetrahedron.com * tetrahedron.volume;
            inertia_tensor += tetrahedron.normed_inertia_tensor;
        }
    }

    com = com / totalVolume;

    std::cout << "volume: " << totalVolume << "\n";
    com.print("COM");
    std::cout << "\n\n";

    if (*density) {
        auto totMass = totalVolume * **density;
        std::cout << "properties by given density: (" << **density << ")\n\n";
        std::cout << "total mass: " << totMass << "\n";
        arma::mat33 I = inertia_tensor * **density;

        if (not *tensorPerspectiveOrigin) {
            // move the inertia_tensor to the COM
            I += totMass * skew(com) * skew(com);
        }

        I.print("inertia tensor");
    }

    if (*totalMass) {
        auto density = **totalMass / totalVolume;
        std::cout << "properties by given total mass: (" << **totalMass << ")\n\n";
        std::cout << "density: " << density << "\n";
        arma::mat33 I = inertia_tensor * density;

        // move the inertia_tensor to the COM
        if (not *tensorPerspectiveOrigin) {
            I += **totalMass * skew(com) * skew(com);
        }

        I.print("inertia tensor");
    }
}


#include <dirent.h>
#include <iostream>
#include <vector>
#include <string>
#include <utility>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Geometry>

#include <opencv2/opencv.hpp>
#define pi acos(-1)

const int G_land_num = 74;
const int G_train_pic_id_num = 3300;
const int G_nShape = 47;
const int G_nVerts = 11510;
const int G_nFaces = 11540;
const int G_test_num = 77;
const int G_iden_num = 77;
const int G_inner_land_num = 59;
const int G_line_num = 50;
const int G_jaw_land_num = 20;
#define normalization
struct Target_type {
	Eigen::VectorXf exp;
	Eigen::RowVector3f tslt;
	Eigen::Matrix3f rot;
	Eigen::MatrixX2f dis;

};

struct DataPoint
{
	cv::Mat image;
	cv::Rect face_rect;
	std::vector<cv::Point2d> landmarks;
	//std::vector<cv::Point2d> init_shape;
	Target_type shape, init_shape;
	Eigen::VectorXf user;
	Eigen::RowVector2f center;
	Eigen::MatrixX2f land_2d;
#ifdef posit
	float f;
#endif // posit
#ifdef normalization
	Eigen::MatrixX3f s;
#endif

	Eigen::VectorXi land_cor;
};

void load_lv(std::string name, DataPoint &temp) {
	std::cout << "load coefficients...file:" << name << "\n";
	FILE *fp;
	fopen_s(&fp, name.c_str(), "rb");

	temp.user.resize(G_iden_num);
	for (int j = 0; j < G_iden_num; j++)
		fread(&temp.user(j), sizeof(float), 1, fp);
	std::cout << temp.user << "\n";
	system("pause");
	temp.land_2d.resize(G_land_num, 2);
	for (int i_v = 0; i_v < G_land_num; i_v++) {
		fread(&temp.land_2d(i_v, 0), sizeof(float), 1, fp);
		fread(&temp.land_2d(i_v, 1), sizeof(float), 1, fp);
	}

	fread(&temp.center(0), sizeof(float), 1, fp);
	fread(&temp.center(1), sizeof(float), 1, fp);

	temp.shape.exp.resize(G_nShape);
	for (int i_shape = 0; i_shape < G_nShape; i_shape++)
		fread(&temp.shape.exp(i_shape), sizeof(float), 1, fp);

	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
		fread(&temp.shape.rot(i, j), sizeof(float), 1, fp);

	for (int i = 0; i < 3; i++) fread(&temp.shape.tslt(i), sizeof(float), 1, fp);

	temp.land_cor.resize(G_land_num);
	for (int i_v = 0; i_v < G_land_num; i_v++) fread(&temp.land_cor(i_v), sizeof(int), 1, fp);

	temp.s.resize(2, 3);
	for (int i = 0; i < 2; i++) for (int j = 0; j < 3; j++)
		fread(&temp.s(i, j), sizeof(float), 1, fp);

	temp.shape.dis.resize(G_land_num, 2);
	for (int i_v = 0; i_v < G_land_num; i_v++) {
		fread(&temp.shape.dis(i_v, 0), sizeof(float), 1, fp);
		fread(&temp.shape.dis(i_v, 1), sizeof(float), 1, fp);
	}
	std::cout << temp.shape.dis << "\n";
	system("pause");
	fclose(fp);
	puts("load successful!");
}

//assume the be could not be more than 90
void cal_uler_angle(Eigen::Matrix3f R) {
	Eigen::Vector3f x, y, z,t;
	x = R.row(0).transpose();
	y = R.row(1).transpose();
	z = R.row(2).transpose();
	float al, be, ga, gaw;
	if (fabs(1 - z(2)*z(2)) < 1e-3) {
		ga=gaw=be = 0;
		al = acos(x(0));
		if (y(0) < 0) al = 2 * pi - al;
	}
	else {

		be = acos(z(2));
		al = acos(std::max(std::min(float(1.0),z(1) / sqrt(1 - z(2)*z(2))),float(-1.0)));

		if (z(0) < 0) al = 2 * pi - al;//according to the sin(al)

		t(0) = cos(al), t(1) = sin(al), t(2) = 0;
		t.normalize();
		x.normalize();
		//t.normalized();
		ga = acos(t.dot(x));
		gaw = acos(std::max(std::min(float(1.0), -y(2) / sqrt(1 - z(2)*z(2))), float(-1.0)));

		printf("%.10f %.10f %.10f\n", -y(2), sqrt(1 - z(2)*z(2)), -y(2) / sqrt(1 - z(2)*z(2)));
		if (x(2) < 0) ga = 2 * pi - ga, gaw = 2 * pi - gaw;//according to the sin(ga)
	}
	std::cout << R << "\n----------------------\n";
	printf("%.10f %.10f %.10f %.10f %.10f\n",z(2), al/pi*180, be / pi * 180, ga / pi * 180, gaw / pi * 180);
	system("pause");
}

Eigen::Matrix3f get_r_from_angle(float angle, int axis) {
	Eigen::Matrix3f ans;
	ans.setZero();
	ans(axis, axis) = 1;
	int idx_x = 0, idx_y = 1;
	if (axis == 0)
		idx_x = 1, idx_y = 2;
	else
		if (axis == 2)
			idx_x = 0, idx_y = 1;
		else
			idx_x = 0, idx_y = 2;
	ans(idx_x, idx_x) = cos(angle), ans(idx_x, idx_y) = -sin(angle), ans(idx_y, idx_x) = sin(angle), ans(idx_y, idx_y) = cos(angle);
	return ans;
}

Eigen::Matrix3f get_r_from_angle(const Eigen::Vector3f &angle) {
	Eigen::Matrix3f ans;
	float Sa = sin(angle(0)), Ca = cos(angle(0)), Sb = sin(angle(1)),
		Cb = cos(angle(1)), Sc = sin(angle(2)), Cc = cos(angle(2));

	ans(0, 0) = Ca * Cc - Sa * Cb*Sc;
	ans(0, 1) = -Sa * Cc - Ca * Cb*Sc;
	ans(0, 2) = Sb * Sc;
	ans(1, 0) = Ca * Sc + Sa * Cb*Cc;
	ans(1, 1) = -Sa * Sc + Ca * Cb*Cc;
	ans(1, 2) = -Sb * Cc;
	ans(2, 0) = Sa * Sb;
	ans(2, 1) = Ca * Sb;
	ans(2, 2) = Cb;
	return ans;
}
void test_r(DataPoint data) {
	Eigen::Matrix3f rot;
	rot = get_r_from_angle(data.shape.tslt(2), 2)*get_r_from_angle(data.shape.tslt(1), 0)*get_r_from_angle(data.shape.tslt(0), 2);
	std::cout << rot << "\n";
	std::cout << get_r_from_angle(data.shape.tslt) << "\n";
	system("pause");
}

int main() {

	DataPoint data;
	data.shape.tslt << 1, 20, 0.5;
	test_r(data);
	//load_lv("./test/pose_4_t108.lv",data);//data/test_debug_lv_005_04_03_051_05
	//cal_uler_angle(data.shape.rot);

	return 0;
}
//g++ -Wall -std=c++11 `pkg-config --cflags opencv` -o deal deal_falut.cpp `pkg-config --libs opencv`


//
// Copyright (c) 2019 INRIA
//

#include "pinocchio/fwd.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/multibody/liegroup/liegroup.hpp"
#include "pinocchio/multibody/liegroup/liegroup-algo.hpp"

#include <casadi/casadi.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
{
  typedef casadi::SX AD_double;
  typedef pinocchio::JointCollectionDefaultTpl<AD_double> JointCollectionAD;
  typedef pinocchio::JointCollectionDefaultTpl<double> JointCollection;

  typedef pinocchio::SE3Tpl<AD_double> SE3AD;
  typedef pinocchio::MotionTpl<AD_double> MotionAD;
  typedef pinocchio::SE3Tpl<double> SE3;
  typedef pinocchio::MotionTpl<double> Motion;
  typedef pinocchio::ConstraintTpl<Eigen::Dynamic,double> ConstraintXd;

  typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
  typedef Eigen::Matrix<AD_double,6,1> Vector6AD;

  typedef JointCollectionAD::JointModelRX JointModelRXAD;
  typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
//  typedef JointModelRXAD::TangentVector_t TangentVectorAD;
  typedef JointCollectionAD::JointDataRX JointDataRXAD;

  typedef JointCollection::JointModelRX JointModelRX;
  typedef JointModelRX::ConfigVector_t ConfigVector;
  typedef JointModelRX::TangentVector_t TangentVector;
  typedef JointCollection::JointDataRX JointDataRX;

  JointModelRX jmodel; jmodel.setIndexes(0,0,0);
  JointDataRX jdata(jmodel.createData());

  JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
  JointDataRXAD jdata_ad(jmodel_ad.createData());

  typedef pinocchio::LieGroup<JointModelRX>::type JointOperation;
  ConfigVector q(jmodel.nq()); JointOperation().random(q);

  casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
  ConfigVectorAD q_ad(jmodel.nq());
  for(Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
  {
    q_ad[k] = cs_q(k);
  }

  // Zero order
  jmodel_ad.calc(jdata_ad,q_ad);
  jmodel.calc(jdata,q);

  SE3 M1(jdata.M);
  SE3AD M2(jdata_ad.M);

  casadi::SX cs_trans(3,1);
  for(Eigen::DenseIndex k = 0; k < 3; ++k)
  {
    cs_trans(k) = M2.translation()[k];
  }
  casadi::SX cs_rot(3,3);
  for(Eigen::DenseIndex i = 0; i < 3; ++i)
  {
    for(Eigen::DenseIndex j = 0; j < 3; ++j)
    {
      cs_rot(i,j) = M2.rotation()(i,j);
    }
  }

  casadi::Function eval_placement("eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
  std::cout << "Joint Placement = " << eval_placement << std::endl;

  std::vector<double> q_vec((size_t)jmodel.nq());
  Eigen::Map<ConfigVector>(q_vec.data(),jmodel.nq(),1) = q;
  casadi::DMVector res = eval_placement(casadi::DMVector {q_vec});
  std::cout << "M(q)=" << res << std::endl;

  BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
  BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));

  // First order
  casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
  TangentVector v(TangentVector::Random(jmodel.nv()));
  VectorXAD v_ad(jmodel_ad.nv());

  std::vector<double> v_vec((size_t)jmodel.nv());
  Eigen::Map<TangentVector>(v_vec.data(),jmodel.nv(),1) = v;

  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
  {
    v_ad[k] = cs_v(k);
  }

  jmodel.calc(jdata,q,v);
  Motion m(jdata.v);
  ConstraintXd Sref(jdata.S.matrix());

  jmodel_ad.calc(jdata_ad,q_ad,v_ad);
  Vector6AD Y;
  MotionAD m_ad(jdata_ad.v);

  casadi::SX cs_vel(6,1);
  for(Eigen::DenseIndex k = 0; k < 6; ++k)
  {
    cs_vel(k) = m_ad.toVector()[k];
  }
  casadi::Function eval_velocity("eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
  std::cout << "Joint Velocity = " << eval_velocity << std::endl;

  casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
  std::cout << "v(q,v)=" << res_vel << std::endl;

  BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
  BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data()+3)));

  casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
  casadi::Function eval_S("eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
  std::cout << "S = " << eval_S << std::endl;

  casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
  std::cout << "res_S:" << res_S << std::endl;
  ConstraintXd::DenseBase Sref_mat = Sref.matrix();

  for(Eigen::DenseIndex i = 0; i < 6; ++i)
  {
    for(Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
      BOOST_CHECK(std::fabs(Sref_mat(i,j) - (double)res_S[0](i,j)) <= Eigen::NumTraits<double>::dummy_precision());
  }
}

template<typename JointModel_> struct init;

template<typename JointModel_>
  struct init
  {
    static JointModel_ run()
    {
      JointModel_ jmodel;
      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return "default " + JointModel_::classname();
    }
  };

  template<typename Scalar, int Options>
  struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
  {
    typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;

    static JointModel run()
    {
      typedef typename JointModel::Vector3 Vector3;
      JointModel jmodel(Vector3::Random().normalized());

      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

  template<typename Scalar, int Options>
  struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
  {
    typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;

    static JointModel run()
    {
      typedef typename JointModel::Vector3 Vector3;
      JointModel jmodel(Vector3::Random().normalized());

      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

  template<typename Scalar, int Options>
  struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
  {
    typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;

    static JointModel run()
    {
      typedef typename JointModel::Vector3 Vector3;
      JointModel jmodel(Vector3::Random().normalized());

      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

  template<typename Scalar, int Options, template<typename,int> class JointCollection>
  struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
  {
    typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

    static JointModel run()
    {
      typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
      JointModel jmodel((JointModelRX()));

      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

  template<typename Scalar, int Options, template<typename,int> class JointCollection>
  struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
  {
    typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

    static JointModel run()
    {
      typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
      typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
      JointModel jmodel((JointModelRX()));
      jmodel.addJoint(JointModelRY());

      jmodel.setIndexes(0,0,0);
      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

  template<typename JointModel_>
  struct init<pinocchio::JointModelMimic<JointModel_> >
  {
    typedef pinocchio::JointModelMimic<JointModel_> JointModel;

    static JointModel run()
    {
      JointModel_ jmodel_ref = init<JointModel_>::run();

      JointModel jmodel(jmodel_ref,1.,0.);

      return jmodel;
    }

    static std::string name()
    {
      return JointModel::classname();
    }
  };

struct TestADOnJoints
{
  template<typename JointModel_>
  void operator()(const pinocchio::JointModelBase<JointModel_> &) const
  {
    JointModel_ jmodel = init<JointModel_>::run();
    jmodel.setIndexes(0,0,0);
    test(jmodel);
  }

  // TODO: get the nq and nv quantity from LieGroups
  template<typename JointModel_>
  static void test(const pinocchio::JointModelMimic<JointModel_> & /*jmodel*/)
  { /* do nothing */ }

  template<typename JointModel>
  static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
  {
    std::cout << "--" << std::endl;
    std::cout << "jmodel: " << jmodel.shortname() << std::endl;

    typedef casadi::SX AD_double;

    typedef pinocchio::SE3Tpl<AD_double> SE3AD;
    typedef pinocchio::MotionTpl<AD_double> MotionAD;
    typedef pinocchio::SE3Tpl<double> SE3;
    typedef pinocchio::MotionTpl<double> Motion;
    typedef pinocchio::ConstraintTpl<Eigen::Dynamic,double> ConstraintXd;

    typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
    typedef Eigen::Matrix<AD_double,6,1> Vector6AD;

    typedef typename pinocchio::CastType<AD_double,JointModel>::type JointModelAD;
    typedef typename JointModelAD::JointDataDerived JointDataAD;

    typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;

    typedef typename JointModel::JointDataDerived JointData;
    typedef typename JointModel::ConfigVector_t ConfigVector;
    typedef typename JointModel::TangentVector_t TangentVector;

    JointData jdata(jmodel.createData());
    pinocchio::JointDataBase<JointData> & jdata_base = jdata;

    JointModelAD jmodel_ad = jmodel.template cast<AD_double>();
    JointDataAD jdata_ad(jmodel_ad.createData());
    pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;

    ConfigVector q(jmodel.nq());

    ConfigVector lb(ConfigVector::Constant(jmodel.nq(),-1.));
    ConfigVector ub(ConfigVector::Constant(jmodel.nq(),1.));

    typedef pinocchio::RandomConfigurationStep<pinocchio::LieGroupMap,ConfigVector,ConfigVector,ConfigVector> RandomConfigAlgo;
    RandomConfigAlgo::run(jmodel.derived(),typename RandomConfigAlgo::ArgsType(q,lb,ub));

    casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
    ConfigVectorAD q_ad(jmodel.nq());
    for(Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
    {
      q_ad[k] = cs_q(k);
    }

    // Zero order
    jmodel_ad.calc(jdata_ad,q_ad);
    jmodel.calc(jdata,q);

    SE3 M1(jdata_base.M());
    SE3AD M2(jdata_ad_base.M());

    casadi::SX cs_trans(3,1);
    for(Eigen::DenseIndex k = 0; k < 3; ++k)
    {
      cs_trans(k) = M2.translation()[k];
    }
    casadi::SX cs_rot(3,3);
    for(Eigen::DenseIndex i = 0; i < 3; ++i)
    {
      for(Eigen::DenseIndex j = 0; j < 3; ++j)
      {
        cs_rot(i,j) = M2.rotation()(i,j);
      }
    }

    casadi::Function eval_placement("eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
    std::cout << "Joint Placement = " << eval_placement << std::endl;

    std::vector<double> q_vec((size_t)jmodel.nq());
    Eigen::Map<ConfigVector>(q_vec.data(),jmodel.nq(),1) = q;
    casadi::DMVector res = eval_placement(casadi::DMVector {q_vec});
    std::cout << "M(q)=" << res << std::endl;

    BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
    BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));

    // First order
    casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
    TangentVector v(TangentVector::Random(jmodel.nv()));
    VectorXAD v_ad(jmodel_ad.nv());

    std::vector<double> v_vec((size_t)jmodel.nv());
    Eigen::Map<TangentVector>(v_vec.data(),jmodel.nv(),1) = v;

    for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
    {
      v_ad[k] = cs_v(k);
    }

    jmodel.calc(jdata,q,v);
    Motion m(jdata_base.v());
    ConstraintXd Sref(jdata_base.S().matrix());

    jmodel_ad.calc(jdata_ad,q_ad,v_ad);
    Vector6AD Y;
    MotionAD m_ad(jdata_ad_base.v());

    casadi::SX cs_vel(6,1);
    for(Eigen::DenseIndex k = 0; k < 6; ++k)
    {
      cs_vel(k) = m_ad.toVector()[k];
    }
    casadi::Function eval_velocity("eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
    std::cout << "Joint Velocity = " << eval_velocity << std::endl;

    casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
    std::cout << "v(q,v)=" << res_vel << std::endl;

    BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
    BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data()+3)));

    casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
    casadi::Function eval_S("eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
    std::cout << "S = " << eval_S << std::endl;

    casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
    std::cout << "res_S:" << res_S << std::endl;
    ConstraintXd::DenseBase Sref_mat = Sref.matrix();

    for(Eigen::DenseIndex i = 0; i < 6; ++i)
    {
      for(Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
        BOOST_CHECK(std::fabs(Sref_mat(i,j) - (double)res_S[0](i,j)) <= Eigen::NumTraits<double>::dummy_precision());
    }

    std::cout << "--" << std::endl << std::endl;
  }
};

BOOST_AUTO_TEST_CASE(test_all_joints)
{
  typedef pinocchio::JointCollectionDefault::JointModelVariant JointModelVariant;
  boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());

  TestADOnJoints()(pinocchio::JointModel());
}

BOOST_AUTO_TEST_SUITE_END()


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXcf A = MatrixXcf::Random(4,4);
HessenbergDecomposition<MatrixXcf> hd(4);
hd.compute(A);
cout << "The matrix H in the decomposition of A is:" << endl << hd.matrixH() << endl;
hd.compute(2*A); // re-use hd to compute and store decomposition of 2A
cout << "The matrix H in the decomposition of 2A is:" << endl << hd.matrixH() << endl;

  return 0;
}


#include <memory>
#define CATCH_CONFIG_MAIN
#include <catch.hpp>
#include <tbb/tbb.h>

#include <Eigen/Dense>
#include <Eigen/src/Eigenvalues/ComplexEigenSolver.h>
#include <Eigen/src/QR/HouseholderQR.h>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "common.hpp"

#include "yavque/Circuit.hpp"
#include "yavque/operators.hpp"

tbb::global_control gc(tbb::global_control::max_allowed_parallelism, 2);

Eigen::MatrixXcd matrix_log(const Eigen::MatrixXcd& m)
{
	Eigen::ComplexEigenSolver<Eigen::MatrixXcd> solver(m);
	Eigen::MatrixXcd u = solver.eigenvectors();

	Eigen::VectorXcd d = solver.eigenvalues();
	d.array() = d.array().log().eval();
	return u * d.asDiagonal() * u.adjoint();
}

TEST_CASE("test single qubit operator", "[single-qubit-operator]")
{
	using namespace yavque;
	constexpr uint32_t N = 10;
	constexpr uint32_t dim = 1u << N;
	constexpr cx_double I(0, 1.0);
	std::random_device rd;
	std::default_random_engine re{rd()};
	std::uniform_int_distribution<uint32_t> index_dist(0, N - 1);

	// test using sparse matrix construction
	for(uint32_t instance_idx = 0; instance_idx < 100; ++instance_idx)
	{
		auto op = random_unitary(2, re);
		auto idx = index_dist(re);
		auto m1 = SingleQubitOperator(op, N, idx);

		auto st = random_vector(dim, re);

		edp::LocalHamiltonian<cx_double> lh(N, 2);
		lh.addOneSiteTerm(idx, op.sparseView());
		auto m = edp::constructSparseMat<cx_double>(dim, lh);

		REQUIRE((m1.apply_right(st) - m * st).norm() < 1e-6);

		m1.dagger_in_place();
		REQUIRE((m1.apply_right(st) - m.adjoint() * st).norm() < 1e-6);
	}

	// test using U = e^{-I H}
	for(uint32_t instance_idx = 0; instance_idx < 100; ++instance_idx)
	{
		auto op = random_unitary(2, re);
		auto idx = index_dist(re);
		auto m1 = SingleQubitOperator(op, N, idx);

		auto m2 = SingleQubitHamEvol(
			std::make_shared<DenseHermitianMatrix>(I * matrix_log(op)), N, idx);
		m2.set_variable_value(1.0);

		auto st = random_vector(dim, re);
		REQUIRE((m1.apply_right(st) - m2.apply_right(st)).norm() < 1e-6);
		m1.dagger_in_place();
		m2.dagger_in_place();
		REQUIRE((m1.apply_right(st) - m2.apply_right(st)).norm() < 1e-6);
	}
}

TEST_CASE("test derivative of single qubit ham evol", "[single-qubit-ham-evol]")
{
	using namespace yavque;
	constexpr uint32_t N = 10;
	constexpr uint32_t dim = 1u << N;
	constexpr cx_double I(0, 1.0);
	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<double> ndist;
	std::uniform_int_distribution<uint32_t> index_dist(0, N - 1);

	SECTION("test using pauli-x")
	{
		for(uint32_t instance_idx = 0; instance_idx < 10; ++instance_idx)
		{
			auto idx = index_dist(re);
			auto m = SingleQubitHamEvol(std::make_shared<DenseHermitianMatrix>(pauli_x()),
			                            N, idx);

			double val = ndist(re);
			m.set_variable_value(val);

			auto st = random_vector(dim, re);
			Eigen::VectorXcd grad1 = m.log_deriv()->apply_right(m.apply_right(st));

			// from parameter shift rule
			m.get_variable() += M_PI / 2;
			Eigen::VectorXcd grad2 = m.apply_right(st);
			m.get_variable() = val - M_PI / 2;
			grad2 -= m.apply_right(st);
			grad2 /= 2.0;

			REQUIRE((grad1 - grad2).norm() < 1e-6);
		}
	}
	SECTION("test using pauli-y")
	{
		for(uint32_t instance_idx = 0; instance_idx < 10; ++instance_idx)
		{
			auto idx = index_dist(re);
			auto m = SingleQubitHamEvol(std::make_shared<DenseHermitianMatrix>(pauli_y()),
			                            N, idx);

			double val = ndist(re);
			m.set_variable_value(val);

			auto st = random_vector(dim, re);
			Eigen::VectorXcd grad1 = m.log_deriv()->apply_right(m.apply_right(st));

			// from parameter shift rule
			m.get_variable() += M_PI / 2;
			Eigen::VectorXcd grad2 = m.apply_right(st);
			m.get_variable() = val - M_PI / 2;
			grad2 -= m.apply_right(st);
			grad2 /= 2.0;

			REQUIRE((grad1 - grad2).norm() < 1e-6);
		}
	}
	SECTION("test using pauli-z")
	{
		for(uint32_t instance_idx = 0; instance_idx < 10; ++instance_idx)
		{
			auto idx = index_dist(re);
			auto m = SingleQubitHamEvol(std::make_shared<DenseHermitianMatrix>(pauli_z()),
			                            N, idx);

			double val = ndist(re);
			m.set_variable_value(val);

			auto st = random_vector(dim, re);
			Eigen::VectorXcd grad1 = m.log_deriv()->apply_right(m.apply_right(st));

			// from parameter shift rule
			m.get_variable() += M_PI / 2;
			Eigen::VectorXcd grad2 = m.apply_right(st);
			m.get_variable() = val - M_PI / 2;
			grad2 -= m.apply_right(st);
			grad2 /= 2.0;

			REQUIRE((grad1 - grad2).norm() < 1e-6);
		}
	}

	SECTION("test using random hermitian matrix")
	{
		for(uint32_t instance_idx = 0; instance_idx < 100; ++instance_idx)
		{
			auto idx = index_dist(re);

			Eigen::Vector3d coeffs;
			for(uint32_t i = 0; i < 3; ++i)
			{
				coeffs(i) = ndist(re);
			}

			Eigen::MatrixXcd ham = coeffs(0) * pauli_x();
			ham += coeffs(1) * pauli_y();
			ham += coeffs(2) * pauli_z();

			auto m
				= SingleQubitHamEvol(std::make_shared<DenseHermitianMatrix>(ham), N, idx);

			double val = ndist(re);
			m.set_variable_value(val);

			auto st = random_vector(dim, re);
			Eigen::VectorXcd grad1 = m.log_deriv()->apply_right(m.apply_right(st));

			// calc grad from parameter shift rule
			Eigen::VectorXcd grad2;
			{
				double t = coeffs.norm();
				m.get_variable() += M_PI / 2 / t;
				grad2 = m.apply_right(st);
				m.get_variable() = val - M_PI / 2 / t;
				grad2 -= m.apply_right(st);

				grad2 *= t / 2.0;
			}

			REQUIRE((grad1 - grad2).norm() < 1e-6);
		}
	}
}


/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef MATRIX_INVERSION_HPP
#define MATRIX_INVERSION_HPP

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#if defined(BOOST_UBLAS_TYPE_CHECK)
#define MATRIX_INVERSION_SAVE BOOST_UBLAS_TYPE_CHECK
#undef BOOST_UBLAS_TYPE_CHECK
#define BOOST_UBLAS_TYPE_CHECK 0
#include <boost/numeric/ublas/lu.hpp>
#undef BOOST_UBLAS_TYPE_CHECK
#define BOOST_UBLAS_TYPE_CHECK MATRIX_INVERSION_SAVE
#else
#define BOOST_UBLAS_TYPE_CHECK 0
#include <boost/numeric/ublas/lu.hpp>
#undef BOOST_UBLAS_TYPE_CHECK
#endif

#ifdef USE_LAPACK
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/lapack/gesv.hpp>
#include <boost/numeric/bindings/blas/blas3.hpp>
namespace bnb = boost::numeric::bindings;
#endif

#ifdef USE_ATLAS
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
namespace bnb = boost::numeric::bindings;
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T>
bool invert(const ub::matrix<T>& a, ub::matrix<T>& b) {
	ub::matrix<T> tmp(a);
	ub::permutation_matrix<> pm(tmp.size1());

	if (ub::lu_factorize(tmp, pm) != 0) return false;

	b = ub::identity_matrix<T>(tmp.size1());

	try {
		ub::lu_substitute(tmp, pm, b);
	}
	catch (...) {
		return false;
	}

	return true;
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
bool invert(const ub::matrix<double>& a, ub::matrix<double>& b) {
	ub::matrix<double, ub::column_major> tmp(a);
	ub::permutation_matrix<int> pm(tmp.size1());

	#ifdef USE_LAPACK
	if (bnb::lapack::getrf(tmp, pm) != 0) return false;
	if (bnb::lapack::getri(tmp, pm) != 0) return false;
	#endif
	#ifdef USE_ATLAS
	if (bnb::atlas::getrf(tmp, pm) != 0) return false;
	if (bnb::atlas::getri(tmp, pm) != 0) return false;
	#endif

	b = tmp;

	return true;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

template <class T>
bool linear_equation(const ub::matrix<T>& a, const ub::vector<T>& b, ub::vector<T>& x) {
	ub::matrix<T> tmp(a);
	ub::permutation_matrix<> pm(tmp.size1());

	if (ub::lu_factorize(tmp, pm) != 0) return false;

	x = b;

	ub::lu_substitute(tmp, pm, x);

	return true;
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
bool linear_equation(const ub::matrix<double>& a, const ub::vector<double>& b, ub::vector<double>& x) {
	ub::matrix<double, ub::column_major> tmp(a);
	// int i;
	// int size = tmp.size1();
	ub::matrix<double, ub::column_major> tmp2(tmp.size1(), 1);

	// for (i=0; i<size; i++) tmp2(i, 0) = b(i);
	// ub::column(tmp2, 0).assign(b);
	ub::column(tmp2, 0) = b;

	#ifdef USE_LAPACK
	if (bnb::lapack::gesv(tmp, tmp2) != 0) return false;
	#endif
	#ifdef USE_ATLAS
	if (bnb::atlas::gesv(tmp, tmp2) != 0) return false;
	#endif

	// for (i=0; i<size; i++) x(i) = tmp2(i, 0);
	x = ub::column(tmp2, 0);

	return true;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

template <class T>
void mm_mult(const ub::matrix<T>& a, const ub::matrix<T>& b, ub::matrix<T>& c) {
	c = ub::prod(a, b);
}

// special version for double
#if defined(USE_LAPACK) || defined(USE_ATLAS)
template <>
void mm_mult(const ub::matrix<double>& a, const ub::matrix<double>& b, ub::matrix<double>& c) {
	ub::matrix<double, ub::column_major> ca(a);
	ub::matrix<double, ub::column_major> cb(a);
	ub::matrix<double, ub::column_major> cc(c);

	#ifdef USE_LAPACK
		bnb::blas::gemm(ca, cb, cc);
	#endif
	#ifdef USE_ATLAS
		bnb::atlas::gemm(ca, cb, cc);
	#endif

	c = cc;
}
#endif // defined(USE_LAPACK) || defined(USE_ATLAS)

} // namespace kv

#endif // MATRIX_INVERSION_HPP


#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <ipc/utils/eigen_ext.hpp>

namespace ipc {

typedef Eigen::DiagonalMatrix<double, 3> DiagonalMatrix3d;

template <typename T>
Eigen::SparseMatrix<T> SparseDiagonal(const VectorX<T>& x);

template <typename T> inline Matrix2<T> Hat(T x);
template <typename T> inline Matrix3<T> Hat(Vector3<T> x);
template <typename T> inline MatrixMax3<T> Hat(VectorMax3<T> x);

} // namespace ipc

#include "eigen_ext.tpp"


#pragma once
#include <algorithm>
#include <boost/iterator/counting_iterator.hpp>
#include <cassert>
#include <vector>

// An integer interval is a closed-open interval of integers. For example,
// IntegerInterval(3,8) = {3,4,5,6,7}.
template <class IntType = int>
class IntegerInterval
{
public:
    static_assert(std::is_integral<IntType>::value, "Template parameter IntType must be integral");
    using value_type = IntType;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    using iterator = boost::counting_iterator<IntType>;
    using const_iterator = iterator;

public:
    explicit IntegerInterval() = default;

    explicit IntegerInterval(IntType n) : last_(n)
    {
        if (last_ < 0)
            last_ = 0;
    }

    explicit IntegerInterval(IntType from, IntType to) : first_(from), last_(to)
    {
        if (last_ < first_) // empty interval
            last_ = first_;
    }

    [[nodiscard]] size_type size() const { return last_ - first_; }
    [[nodiscard]] IntType operator[](size_type i) const { return first_ + i; }

    [[nodiscard]] iterator begin() const { return iterator(first_); }
    [[nodiscard]] iterator end() const { return iterator(last_); }

private:
    IntType first_{0};
    IntType last_{0};
}; // end class IntegerInterval

using integer_interval = IntegerInterval<int>;
using big_integer_interval = IntegerInterval<std::int64_t>;

// Think of NN as the set of natural numbers.
template <class IntType>
auto NN(IntType n)
{
    return IntegerInterval<IntType>{n};
}

template <class IntTypeFrom, class IntTypeTo>
auto II(IntTypeFrom from, IntTypeTo to)
{
    using intt = std::common_type_t<IntTypeFrom, IntTypeTo>;
    return IntegerInterval<intt>{from, to};
}

template <class Container, class Index = std::ptrdiff_t>
auto indices(const Container& C)
{
    return IntegerInterval<Index>(C.size());
}


/// @file  linalg.hpp
/// @brief Linear algebra routines.

#pragma once
#ifndef ORDGEO_LINALG_LINALG_HPP
#define ORDGEO_LINALG_LINALG_HPP

#include <ordgeo/config.hpp>
#include <Eigen/Eigen>
#include <exception>
#include <memory>
#include <string>
#include <vector>

namespace ORDGEO_NAMESPACE {
namespace linalg {

/// An exception to throw in case of mathematical error in some linear algebra
/// routine.
struct LinAlgErr : public std::runtime_error {
	virtual ~LinAlgErr() = default;

	/// Build an LinAlgErr with the specified message.
	LinAlgErr(std::string message)
		: std::runtime_error("ordgeo::LinAlgErr: " + message) {
	}
};

/// The eigenvalues and eigenvectors of a matrix
struct EigResult {
	virtual ~EigResult() = default;

	/// The eigenvalues
	Eigen::ArrayXcd eig;

	/// The eigenvectors
	Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType vec;
};

/// Iterates over nonzero entries in a dense vector.
template<typename T>
struct DenseNZIterator {
	DenseNZIterator(const T& vector) : _vec(vector), _idx(-1) { ++(*this); }
	virtual ~DenseNZIterator() = default;
	void operator++() {
		++_idx;
		while (_idx < _vec.size() && _vec[_idx] == 0) {
			++_idx;
		}
	}
	operator bool() const {
		return _idx < _vec.size();
	}
	size_t index() const {
		return static_cast<size_t>(_idx);
	}

	const T& _vec;
	int _idx;
};

/// Compute the eigenvalues and eigenvectors of a matrix.
/// Throws LinAlgErr on failure.
EigResult eigendecomposition(const Eigen::MatrixXd& matrix,
	bool assumeSelfAdjoint = false);

/// Ask whether a matrix is positive semidefinite.
/// This tests whether the eigenvalues are non-negative, to within eps
/// precision.
/// Any PSD matrix is a valid n x n distance matrix for some Euclidean space
/// R^d, with d <= n - 1.
bool isPSD(const Eigen::MatrixXd& matrix, double eps = 1e-12);

/// Attempt to project a matrix onto the PSD cone.
/// Throws LinAlgErr in case of failure.
/// This will produce a PSD matrix which is as close as possible to the input
/// matrix. This is often used with methods which compute matrices to satisfy
/// some loss minimization objective without constraining those matrices to be
/// distance matrices. The output from such a method can be projected onto the
/// PSD cone to obtain the PSD solution which is as close as possible to the
/// solution with minimal loss. This is sometimes faster than constraining the
/// optimization problem to stay within the PSD cone.
Eigen::MatrixXd projectOntoPSDCone(const Eigen::MatrixXd& matrix,
	bool assumeSelfAdjoint);

/// Project a matrix onto the nearest matrix of the specified rank.
/// This works by setting the smallest eigenvalues to zero.
Eigen::MatrixXd projectOntoLowRank(const Eigen::MatrixXd& matrix, size_t rank);

/// Project a matrix onto the nuclear norm ball.
/// This has the effect of reducing its rank.
///
/// [1] Efficient Projections onto the .1-Ball for Learning in High Dimensions.
///     John Duchi, Shai Shalev-Shwartz, Yoram Singer, and Tushar Chandra.
///     International Conference on Machine Learning (ICML 2008)
///     http://www.cs.berkeley.edu/~jduchi/projects/DuchiSiShCh08.pdf
Eigen::MatrixXd projectOntoNuclearNorm(const Eigen::MatrixXd& matrix,
	double lambda);

/// Project a matrix onto the unit sphere.
/// All vectors are scaled to unit length.
Eigen::MatrixXd projectOntoUnitSphere(const Eigen::MatrixXd& matrix);

/// A position match is a rotation/reflection, translation, and scaling of a
/// position matrix so that its points are as close as possible to the positions
/// of the corresponding rows in some target matrix.
/// This is also known as a Procrustes transformation.
struct PositionMatch {
	virtual ~PositionMatch() = default;

	/// Finds a Procrustes transformation which minimizes the sum of squared
	/// distances between corresponding rows of target and testee.
	///
	/// [1] I. Borg & P. Groenen (1997): Modern multidimensional scaling: theory
	///     and applications. Springer.
	static std::shared_ptr<PositionMatch> Create(const Eigen::MatrixXd& target,
		const Eigen::MatrixXd& testee);

	/// Transforms a matrix in-place into the target space.
	virtual void transform(Eigen::MatrixXd& matrix) = 0;

	/// Return the rotation matrix.
	virtual Eigen::MatrixXd rotationMatrix() const = 0;

	/// Return the translation vector.
	virtual Eigen::VectorXd translationVector() const = 0;

	/// Return the scaling factor.
	virtual double scalingFactor() const = 0;
};

/// Find the distance scaling which minimizes the difference between two
/// distance matrices, in a least-squares sense.
double distScalingFactor(const Eigen::MatrixXd& dhat,
	const Eigen::MatrixXd& dtrue);

/// Select the specified rows of a matrix.
template<typename mat_type>
mat_type selectRows(const mat_type& X, std::vector<size_t> rows) {
	mat_type result(rows.size(), X.cols());
	for (size_t ii = 0; ii < rows.size(); ii++) {
		result.row(ii) = X.row(rows[ii]);
	}
	return result;
}
template<>
Eigen::SparseMatrix<double, Eigen::RowMajor> selectRows(
	const Eigen::SparseMatrix<double, Eigen::RowMajor>& X,
	std::vector<size_t> rows);

/// Convert a position matrix into a Euclidean distance matrix.
template<typename mat_type>
Eigen::MatrixXd posToDist(const mat_type& X) {
	Eigen::MatrixXd B = X * X.transpose();
	Eigen::VectorXd c = B.diagonal();
	Eigen::VectorXd one = Eigen::VectorXd::Ones(X.rows());
	return (c * one.transpose() + one * c.transpose() - 2 * B).cwiseSqrt();
}

/// Convert a position matrix into a Cosine distance matrix.
template<typename mat_type>
Eigen::MatrixXd posToCosineDist(const mat_type& X) {
	Eigen::MatrixXd dists(X.rows(), X.rows());
	Eigen::VectorXd norm(X.rows());
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		norm(ii) = X.row(ii).norm();
	}
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		auto a = X.row(ii);
		for (Eigen::Index jj = 0; jj < ii; jj++) {
			auto b = X.row(jj);
			dists(ii,jj) = 1.0 - (a.dot(b) / (norm(ii) + norm(jj)));
			dists(jj,ii) = dists(ii,jj);
		}
	}
	return dists;
}

/// Convert a position matrix into a Jaccard distance matrix.
template<typename M, typename V, typename IT>
Eigen::MatrixXd posToJaccardDist(const M& X) {
	Eigen::MatrixXd dists(X.rows(), X.rows());
	for (Eigen::Index ii = 0; ii < X.rows(); ii++) {
		V a = X.row(ii);
		for (Eigen::Index jj = 0; jj < ii; jj++) {
			V b = X.row(jj);

			IT ita(a), itb(b);
			double inBoth = 0, total = 0;
			while (ita && itb) {
				total++;
				if (ita.index() < itb.index()) {
					++ita;
				} else if (ita.index() > itb.index()) {
					++itb;
				} else {
					inBoth++;
					++ita;
					++itb;
				}
			}
			for (; ita; ++ita) {
				total++;
			}
			for (; itb; ++itb) {
				total++;
			}

			dists(ii,jj) = total ? (total - inBoth) / total : INFINITY;
			dists(jj,ii) = dists(ii,jj);
		}
	}
	return dists;
}

/// Center a matrix
Eigen::MatrixXd centerMatrix(const Eigen::MatrixXd& X);

/// Center a position matrix about the origin, and scale it to unit diameter
Eigen::MatrixXd normalizePos(const Eigen::MatrixXd& X);

/// Transform a vector by isotonic regression so its values are changed by the
/// smallest amount possible to appear in the specified order.
///
/// Implements the PAVA algorithm with uniform weights, as described here:
/// http://stat.wikia.com/wiki/Isotonic_regression
Eigen::VectorXd isotonicRegression(const Eigen::VectorXd& V,
	const Eigen::VectorXd& weights, const std::vector<Eigen::Index>& order);

/// Calculate the Euclidean distance between two points.
inline double dist(const Eigen::MatrixXd& X, Eigen::Index a, Eigen::Index b) {
	assert(a < X.rows());
	assert(b < X.rows());
	return (X.row(a) - X.row(b)).norm();
}

/// Calculate the squared Euclidean distance between two points.
inline double sqDist(const Eigen::MatrixXd& X, Eigen::Index a, Eigen::Index b) {
	assert(a < X.rows());
	assert(b < X.rows());
	return (X.row(a) - X.row(b)).squaredNorm();
}

/// Get pairwise squared Euclidean distances from a dissimilarity kernel.
Eigen::MatrixXd sqDistsFromKernel(const Eigen::MatrixXd& K);

/// Calculate the squared Euclidean distance between two points from a kernel.
inline double sqDistFromKernel(const Eigen::MatrixXd K, Eigen::Index a,
	Eigen::Index b) {
	assert(K.rows() == K.cols());
	assert(a < K.rows());
	assert(b < K.rows());
	return K(a,a) + K(b,b) - 2 * K(a,b);
}

/// Get an embedding from a dissimilarity kernel via SVD
Eigen::MatrixXd embeddingFromKernelSVD(const Eigen::MatrixXd& K, size_t nDim);

/// Compute a kernel matrix from a given position/feature matrix.
template<typename mat_type>
Eigen::MatrixXd kernelForFeatures(const mat_type& mat) {
	Eigen::Index nObj = mat.rows();
	Eigen::MatrixXd kernel = Eigen::MatrixXd::Zero(nObj, nObj);
	for (Eigen::Index ii = 0; ii < nObj; ii++) {
		for (Eigen::Index jj = 0; jj <= ii; jj++) {
			kernel(ii,jj) = mat.row(ii).dot(mat.row(jj));
			if (ii != jj) {
				kernel(jj,ii) = kernel(ii,jj);
			}
		}
	}
	return kernel;
}

/// Find the intersection of spheres with k-dimensional center coordinates.
/// The sphere centers are given as the columns of centers.
/// The quality variable is negative if the spheres do not intersect,
/// zero if they intersect in R^k, and positive if they intersect in R^(k+1).
/// Citation: Thm. 3.3 from
///     H.X. Huang, Z.-A. Liang, and P. M. Pardalos,
///     "Some Properties for the Euclidean Distance Matrix and Positive
///     Semidefinite Matrix Completion Problems,"
///     J Glob Optim, vol. 25, no. 1, pp. 3–21, 2003.
Eigen::VectorXd sphereIntersection(const Eigen::MatrixXd& centers,
	const Eigen::VectorXd& radii, double& quality);

} // end namespace linalg
} // end namespace ORDGEO_NAMESPACE
#endif /* ORDGEO_LINALG_LINALG_HPP */


#include <iostream>
#include <nbsimMyFunctions.h>
#include <nbsimExceptionMacro.h>
#include "nbsimParticle.h"
#include "nbsimMassiveParticle.h"
#include "nbsimSolarSystemData.ipp"
#include <Eigen/Dense>
#include <CLI/CLI.hpp>
#include <chrono>
#include <omp.h>

int main(int argc, char** argv){
    CLI::App app{"solar system simulator"};
    double step_size, total_time;
    app.add_option("-s,--timestep", step_size,  "step size, unit:year");
    app.add_option("-t,--totaltime", total_time,  "duration of simulation, unit:year");

    std::string planet_name[9];
    Eigen::Vector3d init_position, init_velocity, r_com(0,0,0), p_total(0,0,0);
    double mu,mu_total=0;
    std::shared_ptr<nbsim::MassiveParticle> planet_ptr[9];
    CLI11_PARSE(app, argc, argv);

    std::clock_t c_start = std::clock();
    auto t_start = std::chrono::high_resolution_clock::now();
    omp_set_num_threads(16);

    #pragma omp parallel for
    for (int i=0; i<9; i++) {
		planet_name[i]=nbsim::solarSystemData[i].name;
		init_position=nbsim::solarSystemData[i].position;
		init_velocity=nbsim::solarSystemData[i].velocity;
        mu=nbsim::solarSystemData[i].mu;
        mu_total+=mu;
        std::shared_ptr<nbsim::MassiveParticle> ptr_particle_i(new nbsim::MassiveParticle(init_position, init_velocity, mu/6.67408e-11));
		planet_ptr[i]=ptr_particle_i;
	}

    for (int i=0; i<9; i++){
		for (int j=0; j<9; j++){
			planet_ptr[i]->addAttractor(planet_ptr[j]);
            if (planet_ptr[i]==planet_ptr[j]){
                planet_ptr[i]->removeAttractor(planet_ptr[j]);
            }
		}
	}
    #pragma omp parallel
    for (double test_time=0; test_time<total_time; test_time+=step_size){

        #pragma omp for
		for (int i=0;i<9;i++){
			planet_ptr[i]->calculateAcceleration();
		}
        #pragma omp for nowait
		for (int i=0;i<9;i++){
			planet_ptr[i]->integrateTimestep(step_size);
		}
	}
    #pragma omp parallel for
    for (int i=0;i<9;i++){
        r_com+=nbsim::solarSystemData[i].mu*(planet_ptr[i]->getPosition());
        p_total+=nbsim::solarSystemData[i].mu*(planet_ptr[i]->getVelocity());
    }
    std::clock_t c_end = std::clock();
    auto t_end = std::chrono::high_resolution_clock::now();

    r_com=r_com/mu_total;

    for (int i=0;i<9;i++){
		std::cout<<planet_name[i]<<"\n original position:"<<nbsim::solarSystemData[i].position<<"\n current position:"<<planet_ptr[i]->getPosition()<<std::endl;

    }
    std::cout<<"\n r_com is:"<<r_com<<std::endl;
    std::cout<<"\n p_total is:"<<p_total<<std::endl;
    std::cout << std::fixed << std::setprecision(2) << "CPU time used: "
              << 1000.0 * (c_end - c_start) / CLOCKS_PER_SEC << " ms\n"
              << "Wall clock time passed: "
              << std::chrono::duration<double, std::milli>(t_end-t_start).count()
              << " ms\n";
    return 0;
}


#include "cxxmpi/cxxmpi.hpp"
#include "Support/Parsing.hpp"
#include "Support/OstreamHelpers.hpp"
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/gmp.hpp>

namespace mpi = cxxmpi;
namespace mp = boost::multiprecision;

int getSeriesSize(int Precision) {
   return 2 * Precision;
}

#ifndef LINEAR

/* SeriesSize - число членов ряда */
void calculateGroup(int SeriesSize, unsigned int Precision) {
   int Rank = mpi::commRank();
   int CommSz = mpi::commSize();

   /* the 0-th element (i.e. 1) is handled separately */
   auto WorkRange = util::WorkSplitterLinear(SeriesSize - 1, CommSz).getRange(Rank).shift(1);
   assert(WorkRange.size() > 0 && "no work was given to this process");
   auto From = WorkRange.FirstIdx;
   auto To = WorkRange.LastIdx - 1;

   mp::mpz_int Nominator = 1;
   mp::mpz_int PrevItem = 1;
   for (; To > From; --To) {
      PrevItem *= To;
      Nominator += PrevItem;
   }
   mp::mpz_int Denominator = PrevItem * To;

   mp::mpf_float PartialRes{mp::mpq_rational{Nominator, Denominator}, Precision};
   mp::mpf_float DenomRes{mp::mpq_rational{1, Denominator}, Precision};

   mpi::send(PartialRes.str(), 0, /* tag = */ 0);
   mpi::send(DenomRes.str(), 0, /* tag = */ 1);
}

int calculateExp(int Precision) {
   mp::mpf_float::default_precision(Precision);
   calculateGroup(getSeriesSize(Precision), Precision);

   if (mpi::commRank() != 0) {
      // std::cerr << mpi::whoami << ": finished" << std::endl;
      return 0;
   }

   auto CommSz = mpi::commSize();
   std::vector<mp::mpf_float> Partials(CommSz);
   std::vector<mp::mpf_float> Denoms(CommSz);

   std::string Buf;
   for (int I = 0; I < CommSz * 2; ++I) {
      auto Status = mpi::recv(Buf);
      if (Status.tag() == 0) {
         Partials[Status.source()].assign(Buf);
      } else {
         assert(Status.tag() == 1);
         Denoms[Status.source()].assign(Buf);
      }
      Buf.clear();
   }

   /* reduce */
   mp::mpf_float Sum = 1;
   mp::mpf_float CurDenom = 1;
   for (int I = 0; I < CommSz; ++I) {
      Sum += Partials[I] * CurDenom;
      CurDenom *= Denoms[I];
   }
   std::cout << Sum.str(Precision) << std::endl;
   return 0;
}

#else // LINEAR

/* calculates exp without using any parallel routines */
int calculateExp(int Precision) {
   int SeriesSize = getSeriesSize(Precision);

   mp::mpf_float::default_precision(Precision);
   mp::mpf_float Sum = 1;
   mp::mpf_float Denom = 1;

   for (int I = 1; I <= SeriesSize; ++I) {
      Denom *= I;
      Sum += 1 / Denom;
   }
   std::cout << Sum.str(Precision) << std::endl;
   return 0;
}

#endif // LINEAR

void emitUsageError() {
   std::cerr << "Usage: ./prog PRECISION" << std::endl;
   exit(EXIT_FAILURE);
}

int main(int argc, char *argv[])
{
#ifndef LINEAR
   mpi::MPIContext Ctx{&argc, &argv};
#endif

   int Precision = 0;
   if (argc != 2 || !util::parseInt(argv[1], &Precision))
      emitUsageError();
   return calculateExp(Precision);
}


#include "Tictactoe.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <stdexcept>
#include <sstream>
#include <memory>

using namespace Eigen;

TicTacToe::TicTacToe(){
	this->board = MatrixXf::Zero(this->boardSize, this->boardSize);
	this->player = 1;
}

std::unique_ptr<Game> TicTacToe::copy(){
	return std::make_unique<TicTacToe>(*this);
}

int TicTacToe::getActionSize(){
	return this->boardSize*this->boardSize;
}

std::vector<int> TicTacToe::getBoardSize(){
	return {this->boardSize, this->boardSize};
}

int TicTacToe::getInputPlanes(){
	return 3;
}

int TicTacToe::getOutputPlanes(){
	return 1;
}

void TicTacToe::printBoard(){
	std::cout<< this->board << std::endl;
}

void TicTacToe::play(int action){
	int x = action/boardSize;
	int y = action%boardSize;

	if (this->board(x,y) != 0){
		std::ostringstream  error;
		error << "Invalid action: " << action << "\n" << this->board;
		throw std::invalid_argument(error.str());
	}

	this->board(x,y) = this->player;
	this->player *= -1 ;
}

bool TicTacToe::ended(){
	for (int p : {-1, 1}){
		Matrix<bool,Dynamic,Dynamic> playerPositions = (this->board.array() == p).cast<bool>();

		if (this->findWin(playerPositions)){
			this->winner = p;
			return true;
		};
	}

	if((this->board.array() == 0).count() == 0){
		this->winner = 0;
		return true;
	}

	return false;
}

int TicTacToe::getWinner(){
	return this->winner;
}

bool TicTacToe::findWin(Matrix<bool,Dynamic,Dynamic> playerPositions){
	for (int i = 0; i < this->boardSize - this->inRow + 1; i++){
		for (int j = 0; j < this->boardSize - this->inRow + 1; j++){
			if(this->isWin(playerPositions.block(i, j, this->inRow, this->inRow))){
				return true;
			}
		}
	}

	return false;
}

bool TicTacToe::isWin(Matrix<bool,Dynamic,Dynamic> smallBoard){
	Matrix<bool, Dynamic, Dynamic> vertical = smallBoard.rowwise().all();
	Matrix<bool, Dynamic, Dynamic> horizontal = smallBoard.colwise().all();
	bool ldiagonal = smallBoard.diagonal().all();
	bool rdiagonal = smallBoard.rowwise().reverse().diagonal().all();

	return vertical.any() || horizontal.any() || ldiagonal || rdiagonal;
}

MatrixXf TicTacToe::getBoard(){
	return this->board;
}

float TicTacToe::getPlayer(){
	return this->player;
}

ArrayXf TicTacToe::getPossibleActions(){
	ArrayXf poss = ArrayXf::Zero(this->getActionSize());
	int bsize = (this->board).size();
	for(int i = 0; i < bsize; i++){
		if (this->board(i/boardSize, i%boardSize) == 0){
			poss(i) = 1;
		}
		else{
			poss(i) = 0;
		}
	}

	return poss;
}