#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <iterator>
#include <bits/stdc++.h>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <glob.h>
#include "../stem/stemming/english_stem.h"
#include <fstream>
#include <dirent.h>
#include <sys/types.h>

using namespace std;
using namespace boost;
using convert_t = std::codecvt_utf8<wchar_t>;
std::wstring_convert<convert_t, wchar_t> strconverter;

class avlNode{
    public:
        int page;
        int wordIndex;
        int height;
        avlNode* left;
        avlNode* right;

    avlNode(int pageN, int wordIndexN){
        page = pageN;
        wordIndex = wordIndexN;
        height = 0;
        left = NULL;
        right = NULL;
    }
};

class avlTree{
    public:
        avlNode* root = NULL;

    avlNode* getRoot(){
        return root;
    }

    int height(avlNode* node){
        if(node == NULL)
            return 0;
		return node->height;
    }

    int max(int a,int b) {
        if(a>b)
			return a;
		return b;
    }

    void insert(int page, int wordIndex){
		avlNode* newNode = new avlNode(page, wordIndex);
		if (root == NULL)
			root = newNode;
		else
			root = avlInsert(newNode, root);
	}

	avlNode* rotatewithleft(avlNode* x){
		avlNode* leftChild;
		leftChild = x->left;
		x->left = leftChild->right;
		leftChild->right = x;
		x->height = max(height(x->left),height(x->right))+1;
		leftChild->height = max(height(leftChild->left),height(leftChild->right))+1;
		return leftChild;
	}

    avlNode* rotatewithright(avlNode* x){
		avlNode* rightChild;
		rightChild = x->right;
		x->right = rightChild->left;
		rightChild->left = x;
		x->height = max(height(x->left),height(x->right))+1;
		rightChild->height = max(height(rightChild->left),height(rightChild->right))+1;
		return rightChild;
	}

	avlNode* doublerotatewithleft(avlNode* x){
		avlNode* temp;
		x->left = rotatewithright(x->left);
		temp = rotatewithleft(x);
		return temp;
	}

	avlNode* doublerotatewithright(avlNode* x){
		avlNode* temp;
		x->right = rotatewithleft(x->right);
		temp = rotatewithright(x);
		return temp;
	}

    avlNode* avlInsert(avlNode* newNode, avlNode* root){
		avlNode* newRoot = root;  // root of subtree in which i'm inserting
		// bool isBalanced = true;
		if(newNode->wordIndex < root->wordIndex){//branch left
			if (root->left == NULL)
				newRoot->left = newNode;
			else{
				newRoot->left = avlInsert(newNode, root->left);
				if((height(newRoot->left)-height(newRoot->right)) == 2){
					if(newNode->wordIndex < newRoot->wordIndex)
						newRoot = rotatewithleft(newRoot);
					else
						newRoot = doublerotatewithleft(newRoot);
				}
			}
		}
		else if(newNode->wordIndex > root->wordIndex){//branch right
			if (root->right == NULL)
				newRoot->right = newNode;
			else{
				newRoot->right = avlInsert(newNode,root->right);
				if ((height(newRoot->right)-height(newRoot->left)) == 2){
					if(newNode->wordIndex > newRoot->right->wordIndex)
						newRoot = rotatewithright(newRoot);
					else
						newRoot = doublerotatewithright(newRoot);
				}
			}
		}
		//this will be when the wordIndex is same but page is different
		//i'll put them according to pageName in that case
		else{
			if(newNode->page < root->page){//branch left
				if (root->left == NULL)
					newRoot->left = newNode;
				else{
					newRoot->left = avlInsert(newNode, root->left);
					if((height(newRoot->left)-height(newRoot->right)) == 2){
						if(newNode->wordIndex < newRoot->left->wordIndex)
							newRoot = rotatewithleft(newRoot);
						else
							newRoot=doublerotatewithleft(newRoot);
					}
				}
			}
			else if(newNode->page > root->page){//branch right
				if (root->right == NULL)
					newRoot->right = newNode;
				else{
					newRoot->right = avlInsert(newNode, root->right);
					if ((height(newRoot->right)-height(newRoot->left)) == 2){
						if(newNode->wordIndex > newRoot->left->wordIndex)
							newRoot = rotatewithright(root);
						else
							newRoot = doublerotatewithright(root);
					}
				}
			}
		}

		// Update height
		newRoot->height = max(height(newRoot->left),height(newRoot->right))+1;

		return newRoot; // return new root of this subtree
	}

    bool isPresent(int page, int wordIndex, avlNode* root){
		if(root == NULL)
			return false;
		if(root->wordIndex < wordIndex)
			return isPresent(page, wordIndex, root->right);
		if(root->wordIndex > wordIndex)
			return isPresent(page, wordIndex, root->left);
		if(root->wordIndex == wordIndex){
			if(root->page == page)
				return true;
			if(root->page > page)
				return isPresent(page, wordIndex, root->left);
			if(root->page < page)
				return isPresent(page, wordIndex, root->right);
		}
		return false;
	}

	void getWordIndex(avlNode* root, int page, vector<int> newList){
		if(root == NULL)
			return;
		getWordIndex(root->left, page, newList);
		if(root->page == page)
			newList.push_back(root->wordIndex);
		getWordIndex(root->right, page, newList);
	}

	vector<int> getWordIndex(int page){
		vector<int> newList;
		getWordIndex(root, page, newList);
		return newList;
	}

	void print(avlNode* root){
		if(root == NULL)
			return;
		print(root->left);
		cout<<root->wordIndex<<" "<<root->page<<" "<<root->height<<endl;
		print(root->right);
	}

};

string stopWords[] = {
    "ourselves", "hers", "between", "yourself", "but", "again", "there", "about", "once", "during", "out", "very", "having",
    "with", "they", "own", "an", "be", "some", "for", "do", "its", "yours", "such", "into", "of", "most", "itself", "other", "off", "is", "s",
    "am", "or", "who", "as", "from", "him", "each", "the", "themselves", "until", "below", "are", "we", "these", "your", "his", "through", "don",
    "nor", "me", "were", "her", "more", "himself", "this", "down", "should", "our", "their", "while", "above", "both", "up", "to", "ours", "had",
    "she", "all", "no", "when", "at", "any", "before", "them", "same", "and", "been", "have", "in", "will", "on", "does", "yourselves", "then",
    "that", "because", "what", "over", "why", "so", "can", "did", "not", "now", "under", "he", "you", "herself", "has", "just", "where", "too",
    "only", "myself", "which", "those", "i", "after", "few", "whom", "t", "being", "if", "theirs", "my", "against", "a", "by", "doing", "it",
    "how", "further", "was", "here", "than"
};

int sizeStop = sizeof(stopWords)/sizeof(stopWords[0]);

bool isStopWord(string s){
    for(int i=0;i<sizeStop;i++){
        if(s==stopWords[i])
            return true;
    }
    return false;
}

string my_to_string(wstring wstr){
    return strconverter.to_bytes(wstr);
}

wstring my_to_wstring(string str){
    return strconverter.from_bytes(str);
}

map<int, int> getDocs(vector<string> query, map<string,vector<pair<vector<int>,int>>> invIndex){
    vector<int> docs;
	vector<int> docsTemp;
	string term;
	map<int, int> relevance;
    bool found = false;
	vector<pair<vector<int>,int>> tempVec;
    map<string, vector<pair<vector<int>,int>>>::iterator it;

	for(int i=0;i<query.size();i++){
		it = invIndex.find(query[i]);
        if(it==invIndex.end()){
			return relevance;
		}
		if(found){
			tempVec = it->second;
			docsTemp.clear();
			for(int j=0;j<tempVec.size();j++){
				if(std::find(docs.begin(), docs.end(), tempVec[j].second) != docs.end()){
					docsTemp.push_back(tempVec[j].second);
				}
			}
			docs = docsTemp;
		}
		else{
			tempVec = it->second;
			for(int j=0;j<tempVec.size();j++){
				docs.push_back(tempVec[j].second);
			}
		}
		found = true;
	}
	if(docs.size()==0){
		return relevance;
	}

	vector<vector<int>> termPos;
	vector<vector<vector<int>>> pos;
	for(int i=0;i<docs.size();i++){
		termPos.clear();
		for(int j=0;j<query.size();j++){
			it = invIndex.find(query[j]);
			for(int k=0;k<it->second.size();k++){
				if(it->second[k].second==docs[i]){
					termPos.push_back(it->second[k].first);
					break;
				}
			}
		}
		pos.push_back(termPos);
	}

	int count;
	for(int i=0;i<pos.size();i++){
		count = 0;
		for(int j=0;j<pos[i][0].size();){
			for(int k=1;k<pos[i].size();k++){
				if(std::find(pos[i][k].begin(), pos[i][k].end(), pos[i][0][j]+k)==pos[i][k].end())
					goto nextPos;
			}
			count++;
			nextPos: j++;
		}
		if(count>0){
			relevance.insert(make_pair(docs[i], count));
		}
	}

	return relevance;
}

int main(){
    vector<string> a;
    vector<string> fileMap;
    vector<int> stdVec;
    glob_t glob_result;
    string inputDir = "../testCases/*";
    glob(inputDir.c_str(),GLOB_TILDE,NULL,&glob_result);
    for(unsigned int i=0; i<glob_result.gl_pathc; ++i){
        a.push_back(glob_result.gl_pathv[i]);
        fileMap.push_back(glob_result.gl_pathv[i]);
    }
    string outputFileName = "index";
    string stemmed;
    int m = a.size();
    const char * fileName;
    map<string, vector<pair<vector<int>,int>>> invIndex;
    map<string, vector<pair<vector<int>,int>>>::iterator it;
    stemming::english_stem<> StemEnglish;
    vector<int> tempVec1;
	tempVec1.push_back(0);
	//first is occurences, second is file number
	pair<vector<int>,int> tempPair;
	tempPair.first = tempVec1;
	tempPair.second = 0;
	vector<pair<vector<int>,int>> tempVec2;
	tempVec2.push_back(tempPair);
	long count;
	vector<string> splitVec;

	cout<<"Creating inverted index...\n";
    for(int i=0;i<m;i++){
        fileName = a[i].c_str();
        ifstream file(fileName);
        // Read file
        if(file.is_open()){
			count = 0;
            string line;
            while(getline(file, line)){
                to_lower(line);
                trim(line);
                split(splitVec, line, is_any_of(" .,-:'\"?/_()*!#@;[]{}&|~"), token_compress_on);
                for(int z=0;z<splitVec.size();z++){
                    if(splitVec[z]=="")
                        continue;
                    if(!isStopWord(splitVec[z])){
						wstring temp = my_to_wstring(splitVec[z]);
                        StemEnglish(temp);
                        stemmed = my_to_string(temp);
						if(stemmed==""||stemmed==" "||isStopWord(stemmed))
							continue;
                        count++;
                        it = invIndex.find(stemmed);
                        if(it != invIndex.end()){
                            if(it->second[it->second.size()-1].second==i){
								it->second[it->second.size()-1].first.push_back(count);
							}
							else{
								tempVec1[0] = count;
								tempPair.first = tempVec1;
								tempPair.second = i;
								it->second.push_back(tempPair);
							}
                        }
                        else{
							tempVec1[0] = count;
							tempPair.first = tempVec1;
							tempPair.second = i;
							tempVec2[0] = tempPair;
                            invIndex.insert(make_pair(stemmed, tempVec2));
                            it = invIndex.find(stemmed);
                        }
                    }
                }
            }
            file.close();
        }
    }

    // query processing
    string query, input;
    do{
        cout<<"Type query: ";
        getline(cin, query);
        to_lower(query);
        trim(query);
		pair<int,int> tempPair;
        split(splitVec, query, is_any_of(" .,-:'\"?/_()*!#@;[]{}<>&"), token_compress_on);
        vector<string> splitQuery;
        for(int i=0;i<splitVec.size();i++){
            if(splitVec[i]=="")
                continue;
			wstring temp = my_to_wstring(splitVec[i]);
			StemEnglish(temp);
			stemmed = my_to_string(temp);
            if(isStopWord(splitVec[i])||stemmed==""||stemmed==" ")
				continue;
			splitQuery.push_back(stemmed);
        }
        map<int, int> result = getDocs(splitQuery, invIndex);
        priority_queue< pair<int, int>> ans;
        int ansSize = 0;
        std::map<int, int>::iterator itResult;
        if(result.size()==0){
            cout<<"No results found!\n";
            goto nextQuery;
        }
        for(itResult = result.begin(); itResult!=result.end(); itResult++){
            ans.push(make_pair(itResult->second, itResult->first));
            ansSize++;
        }
        for(int i=0;i<min(50,ansSize);i++) {
            tempPair = ans.top();
            cout<<a[tempPair.second]<<" "<<tempPair.first<<endl;
            ans.pop();
        }
        nextQuery: cout<<"Test more queries? If yes, type yes. If no, type anything else.\n";
        getline(cin, input);
    }while(input=="yes");

}


#include "http_server/parse_path.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

namespace http {
namespace server3 {

bool parse_path(const std::string &path, int &z, int &x, int &y)
{
  std::vector<std::string> splits;
  boost::algorithm::split(splits, path, boost::algorithm::is_any_of("/."));

  // we're expecting a leading /, then 3 numbers separated by /,
  // then ".pbf" at the end.
  if (splits.size() != 5) {
    return false;
  }

  if (splits[0] != "") {
    return false;
  }

  if (splits[4] != "pbf") {
    return false;
  }

  try {
    z = boost::lexical_cast<int>(splits[1]);
    x = boost::lexical_cast<int>(splits[2]);
    y = boost::lexical_cast<int>(splits[3]);

    return true;

  } catch (...) {
    return false;
  }
}

} }


// This file is part of PolyMPC, a lightweight C++ template library
// for real-time nonlinear optimization and optimal control.
//
// Copyright (C) 2020 Listov Petr <petr.listov@epfl.ch>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "gtest/gtest.h"
#include "solvers/admm.hpp"
#include "solvers/qp_preconditioners.hpp"
#include <Eigen/IterativeLinearSolvers>

TEST(QPSolverTest, admmSimpleQP)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2, 1, Scalar> prob;
    prob.settings().max_iter = 1000;

    prob.solve(H, h, A, al, au, xl, xu);
    Eigen::Vector2d sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(QPSolverTest, admmRuizEquilibration)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2, 1, Scalar> prob;
    prob.settings().max_iter = 1000;

    polympc::RuizEquilibration<Scalar, 2, 1> preconditioner;
    preconditioner.compute(H, h, A, al, au, xl, xu);

    prob.solve(H, h, A, al, au, xl, xu);
    Eigen::Vector2d sol      = prob.primal_solution();
    Eigen::Vector3d sol_dual = prob.dual_solution();

    preconditioner.unscale(sol, sol_dual);

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(QPSolverTest, admmSinglePrecisionFloat)
{
    using Scalar = float;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << Scalar(0.7), Scalar(0.7);
    solution << Scalar(0.3), Scalar(0.7);

    ADMM<2, 1, Scalar> prob;

    prob.solve(H,h,A,al,au,xl,xu);
    Eigen::Vector2f sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, Scalar(1e-2)));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, status_t::SOLVED);
}

TEST(QPSolverTest, admmConstraintViolation)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2, 1, Scalar> prob;

    prob.settings().eps_rel = 1e-4;
    prob.settings().eps_abs = 1e-4;

    prob.solve(H,h,A,al,au, xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    // check feasibility (with some epsilon margin)
    Eigen::Vector3d lower;
    Eigen::Vector3d upper;
    lower.segment<1>(0) = A * sol - al;
    upper.segment<1>(0) = A * sol - au;
    lower.segment<2>(1) = sol - xl;
    upper.segment<2>(1) = sol - xu;
    EXPECT_GE(lower.minCoeff(), -1e-3);
    EXPECT_LE(upper.maxCoeff(), 1e-3);
}

TEST(QPSolverTest, admmAdaptiveRho)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2, 1, Scalar> prob;

    prob.settings().adaptive_rho = false;
    prob.settings().adaptive_rho_interval = 10;

    prob.solve(H,h,A,al,au, xl, xu);
    Eigen::Vector2d sol = prob.primal_solution();

    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(QPSolverTest, admmLox)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2, 1, Scalar> prob;

    prob.settings().warm_start = false;
    prob.settings().max_iter = 1000;
    prob.settings().rho = 0.1;

    // solve whithout adaptive rho
    prob.settings().adaptive_rho = false;
    prob.solve(H,h,A,al,au, xl,xu);
    int prev_iter = prob.info().iter;

    // solve with adaptive rho
    prob.settings().adaptive_rho = true;
    prob.settings().adaptive_rho_interval = 10;
    prob.solve(H,h,A,al,au,xl,xu);

    auto info = prob.info();
    EXPECT_LT(info.iter, prob.settings().max_iter);
    EXPECT_LT(info.iter, prev_iter); // adaptive rho should improve :)
    EXPECT_EQ(info.status, SOLVED);
}

#ifdef EIGEN_NO_DEBUG
TEST(QPSolverTest, admmConjugateGradientLinearSolver)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 2,2> H;
    Eigen::Matrix<Scalar, 2,1> h;
    Eigen::Matrix<Scalar, 1,2> A;
    Eigen::Matrix<Scalar, 1,1> al;
    Eigen::Matrix<Scalar, 1,1> au;
    Eigen::Matrix<Scalar, 2,1> xl,xu;
    Eigen::Matrix<Scalar, 2,1> solution;

    H << 4, 1,
         1, 2;
    h << 1, 1;
    A << 1, 1;
    al << 1; xl << 0, 0;
    au << 1; xu << 0.7, 0.7;
    solution << 0.3, 0.7;

    ADMM<2,1,double, DENSE, Eigen::ConjugateGradient, Eigen::Lower | Eigen::Upper> prob;
    prob.solve(H,h,A,al,au,xl,xu);
    Eigen::Vector2d sol = prob.primal_solution();

    auto info = prob.info();
    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_EQ(info.status, SOLVED);
    EXPECT_LT(info.iter, prob.settings().max_iter); // convergence test
}
#endif

TEST(QPSolverTest, admmTestConstraint)
{
    using Scalar = double;

    Eigen::MatrixXd H(5,5);
    Eigen::MatrixXd h(5,1);
    Eigen::MatrixXd A(5,5);
    Eigen::MatrixXd l(5,1);
    Eigen::MatrixXd u(5,1);
    Eigen::MatrixXd solution(5,1);

    H.setIdentity();
    h.setConstant(-1);
    A.setIdentity();

    using solver_t = ADMM<5,0,Scalar>;
    solver_t prob;

    int type_expect[5];
    l(0) = -1e+17;
    u(0) =  1e+17;
    type_expect[0] = solver_t::LOOSE_BOUNDS;
    l(1) = -101;
    u(1) = 1e+17;
    type_expect[1] = solver_t::INEQUALITY_CONSTRAINT;
    l(2) = -1e+17;
    u(2) = 123;
    type_expect[2] = solver_t::INEQUALITY_CONSTRAINT;
    l(3) = -1;
    u(3) = 1;
    type_expect[3] = solver_t::INEQUALITY_CONSTRAINT;
    l(4) = 42;
    u(4) = 42;
    type_expect[4] = solver_t::EQUALITY_CONSTRAINT;

    solver_t::qp_dual_a_t al, ul;

    prob.parse_constraints_bounds(al, ul, l,u);

    for (int i = 0; i < l.rows(); i++)
        EXPECT_EQ(prob.box_constr_type[i], type_expect[i]);
}

TEST(QPSolverTest, admmSimpleLP)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 1,1> H;
    Eigen::Matrix<Scalar, 1,1> h;
    Eigen::Matrix<Scalar, 0,1> A;
    Eigen::Matrix<Scalar, 0,1> al;
    Eigen::Matrix<Scalar, 0,1> au;
    Eigen::Matrix<Scalar, 1,1> xl,xu;
    Eigen::Matrix<Scalar, 1,1> solution;

    H << 0;
    h << 1;
    xl << -1e6;
    xu <<  1e6;
    solution << -1e6;

    ADMM<1, 0, Scalar> prob;

    prob.settings().max_iter = 200;
    prob.settings().alpha = 1.0;
    prob.settings().adaptive_rho = true;
    prob.settings().check_termination = 10;

    prob.solve(H,h,A,al,au,xl,xu);
    Eigen::VectorXd sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}

TEST(QPSolverTest, admmNonConvex)
{
    using Scalar = double;

    Eigen::Matrix<Scalar, 1,1> H;
    Eigen::Matrix<Scalar, 1,1> h;
    Eigen::Matrix<Scalar, 0,1> A;
    Eigen::Matrix<Scalar, 0,1> al;
    Eigen::Matrix<Scalar, 0,1> au;
    Eigen::Matrix<Scalar, 1,1> xl,xu;
    Eigen::Matrix<Scalar, 1,1> solution, guess, dual_guess;

    H << -1;
    h << 0;
    xl << -1;
    xu <<  2;
    solution << 2;
    guess << 0.1;
    dual_guess << 0.1;

    ADMM<1, 0, Scalar> prob;

    prob.settings().max_iter = 200;
    prob.settings().alpha = 1.0;
    prob.settings().adaptive_rho = true;
    prob.settings().rho = 2;
    prob.settings().check_termination = 10;

    prob.solve(H,h,A,al,au,xl,xu, guess, dual_guess);
    Eigen::VectorXd sol = prob.primal_solution();

    EXPECT_TRUE(sol.isApprox(solution, 1e-2));
    EXPECT_LT(prob.iter, prob.settings().max_iter);
    EXPECT_EQ(prob.info().status, SOLVED);
}


/*! \file Peridigm_EnergyReleaseDamageModel.cpp */

//@HEADER
// ************************************************************************
//
//                             Peridigm
//                 Copyright (2011) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// 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 Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "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 SANDIA CORPORATION OR THE
// 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.
//
// Questions?
// David J. Littlewood   djlittl@sandia.gov
// John A. Mitchell      jamitch@sandia.gov
// Michael L. Parks      mlparks@sandia.gov
// Stewart A. Silling    sasilli@sandia.gov
//
// ************************************************************************
//////////////////////////////////////////////////////////////////////////////////////
// Routine developed for Peridigm by
// DLR Composite Structures and Adaptive Systems
//                                __/|__
//                                /_/_/_/
//            www.dlr.de/fa/en      |/ DLR
//////////////////////////////////////////////////////////////////////////////////////
// Questions?
// Christian Willberg  christian.willberg@dlr.de
// Martin Raedel       martin.raedel@dlr.de
// https://github.com/PeriDoX/
//////////////////////////////////////////////////////////////////////////////////////
//@HEADER

#include "Peridigm_EnergyReleaseDamageModel.hpp"
#include "Peridigm_Field.hpp"
#include "material_utilities.h"
#include <thread>
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::EnergyReleaseDamageModel::EnergyReleaseDamageModel(const Teuchos::ParameterList& params)
: DamageModel(params),
m_applyThermalStrains(false),
m_modelCoordinatesFieldId(-1),
m_coordinatesFieldId(-1),
m_damageFieldId(-1),
m_bondDamageFieldId(-1),
m_deltaTemperatureFieldId(-1),
m_dilatationFieldId(-1),
m_weightedVolumeFieldId(-1),
m_horizonFieldId(-1),
m_damageModelFieldId(-1),
m_planeStrain(false),
m_planeStress(false),
m_onlyTension(false),
m_rot(false),
m_Thickness(-1),
m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()) {
	m_rot = false;
    if (params.isParameter("Rot Sym")) {
		m_rot = true;
		maxRad = params.get<double>("Rot Sym");
	}
    if (params.isParameter("Critical Energy")) {
        m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        m_criticalEnergyShear = params.get<double>("Critical Energy Shear");

    } else {
        if (params.isParameter("Critical Energy Tension"))
            m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        else
            m_criticalEnergyTension = -1.0;
        if (params.isParameter("Critical Energy Compression"))
            m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        else
            m_criticalEnergyCompression = -1.0;
        if (params.isParameter("Critical Energy Shear"))
            m_criticalEnergyShear = params.get<double>("Critical Energy Shear");
        else
            m_criticalEnergyShear = -1.0;
        m_type = 1;
        if (params.isParameter("Energy Criterion")) {
            if (params.get<bool>("Energy Criterion") == true)
                m_type = 1;
            }

        if (params.isParameter("Power Law")) {
            if (params.get<bool>("Power Law") == true)
                m_type = 2;
            }

        if (params.isParameter("Separated")) {
            if (params.get<bool>("Separated") == true)
                m_type = 3;
            }

    }
    if(params.isParameter("Plane Stress")){
        m_planeStress = params.get<bool>("Plane Stress");
        m_Thickness = params.get<double>("Thickness");
    }
    if(params.isParameter("Plane Strain")){
        m_planeStrain = params.get<bool>("Plane Strain");
        m_Thickness = params.get<double>("Thickness");
    }
    m_pi = 3.14159;
    m_onlyTension = false;
    if(params.isParameter("Only Tension")){
        m_onlyTension = params.get<bool>("Only Tension");

    }

    if (params.isParameter("Thermal Expansion Coefficient")) {
        m_alpha = params.get<double>("Thermal Expansion Coefficient");
        m_applyThermalStrains = true;
    }

    PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
    m_modelCoordinatesFieldId = fieldManager.getFieldId("Model_Coordinates");
    m_coordinatesFieldId = fieldManager.getFieldId("Coordinates");
    m_volumeFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Volume");
    m_weightedVolumeFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Weighted_Volume");
    m_dilatationFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Dilatation");
    m_damageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Damage");
    m_bondDamageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::BOND, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Bond_Damage");
    m_horizonFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::CONSTANT, "Horizon");
    m_damageModelFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::NODE, PeridigmNS::PeridigmField::VECTOR, PeridigmNS::PeridigmField::TWO_STEP, "Damage_Model_Data");
    if(m_applyThermalStrains)
        m_deltaTemperatureFieldId      = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Temperature_Change");
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_modelCoordinatesFieldId);
    m_fieldIds.push_back(m_coordinatesFieldId);
    m_fieldIds.push_back(m_weightedVolumeFieldId);
    m_fieldIds.push_back(m_dilatationFieldId);
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_damageFieldId);
    m_fieldIds.push_back(m_damageModelFieldId);
    m_fieldIds.push_back(m_bondDamageFieldId);
    m_fieldIds.push_back(m_horizonFieldId);

}

PeridigmNS::EnergyReleaseDamageModel::~EnergyReleaseDamageModel() {
}

void
PeridigmNS::EnergyReleaseDamageModel::initialize(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {
        double *damage, *bondDamage;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
	dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
    // Initialize damage to zero
    int neighborhoodListIndex(0);
    int bondIndex(0);
    int nodeId, numNeighbors;
    int iID, iNID;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        damage[nodeId] = 0.0;
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        neighborhoodListIndex += numNeighbors;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {
            bondDamage[bondIndex] = 0.0;
            bondIndex += 1;
        }
    }

}

void
PeridigmNS::EnergyReleaseDamageModel::computeDamage(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {

    double *x, *y, *damage, *bondDamageNP1, *horizon;
    double *cellVolume, *weightedVolume, *damageModel;
    double criticalEnergyTension(-1.0), criticalEnergyCompression(-1.0), criticalEnergyShear(-1.0);
    // for temperature dependencies easy to extent
    double *deltaTemperature = NULL;
    if(m_applyThermalStrains)
        dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);
    double m_alpha = 0;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
    dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
    dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
    //////////////////////////////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmElastic.cpp
    //////////////////////////////////////////////////////////////
    dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
    dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
    ////////////////////////////////////
    ////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmMaterial.cpp
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamageNP1);

    ////////////////////////////////////////////////////
    double trialDamage(0.0);
    int neighborhoodListIndex(0), bondIndex(0);
    int nodeId, numNeighbors, neighborID, iID, iNID;
    double totalDamage, totalVol;
    double alphaP1 = 0.0, alphaP2 = 0.0, gammaP1 = 0.0, gammaP2 = 0.0, kappaP1 = 0.0, kappaP2 = 0.0;
    double nodeInitialX[3], nodeCurrentX[3], relativeExtension(0.0);
    double bondEnergyIsotropic(0.0), bondEnergyDeviatoric(0.0);
    double omegaP1, omegaP2;
    double critDev, critIso, critComp;
    double degradationFactor = 1.0; // Optional parameter if bond should be degradated and not fully destroyed instantaneously
    double avgHorizon, quadhorizon;
    double eiP1, eiP2, tiP1, tiP2;
    double edP1, edP2, tdP1, tdP2;
    double dilatationP1, dilatationP2;
    double BulkModP1, BulkModP2;
    double ShearModP1, ShearModP2;
    double bondEnergy;

    //double thickness;
    //---------------------------
    // INITIALIZE PROCESS STEP t
    //---------------------------

    if (m_criticalEnergyTension > 0.0)
        criticalEnergyTension = m_criticalEnergyTension;
    if (m_criticalEnergyCompression > 0.0)
        criticalEnergyCompression = m_criticalEnergyCompression;
    if (m_criticalEnergyShear > 0.0)
        criticalEnergyShear = m_criticalEnergyShear;

    // Update the bond damage
    // Break bonds if the bond energy potential is greater than the critical bond energy potential
    //---------------------------
    // DAMAGE ANALYSIS
    //---------------------------
    bondIndex = 0;
    //temperatur ist in peridigm_ElasticMaterial gegeben
    for (iID = 0; iID < numOwnedPoints; ++iID) {
        numNeighbors = neighborhoodList[neighborhoodListIndex++];

        nodeId = ownedIDs[iID];
        nodeInitialX[0] = x[nodeId*3];
        nodeInitialX[1] = x[nodeId*3+1];
        nodeInitialX[2] = x[nodeId*3+2];
        nodeCurrentX[0] = y[nodeId*3];
        nodeCurrentX[1] = y[nodeId*3+1];
        nodeCurrentX[2] = y[nodeId*3+2];

        dilatationP1 = damageModel[3*nodeId];
        BulkModP1    = damageModel[3*nodeId+1];     // weightedVolume is already included in the variable --> Perdigm_Elastic.cpp;
        ShearModP1   = damageModel[3*nodeId+2]; // weightedVolume is already included in --> Perdigm_Elastic.cpp;

        if (m_planeStress==false and m_planeStrain==false){
          //  c = 3 * K * (*theta) / weightedVol;
            // kappa = c / (*theta) / gamma / 3 (?) (above eq (7) lammiCJ
            kappaP1 = 9.0 * BulkModP1;
            gammaP1 = 1.0;
            alphaP1 = 15.0 * ShearModP1;
        }
        if (m_planeStress==true and m_planeStrain==false){
            alphaP1 = 8.0 * ShearModP1;
            kappaP1 = 3.0 * BulkModP1;
            gammaP1 = 4.0 * ShearModP1 / (3.0 * BulkModP1 + 4.0 * ShearModP1);
            alphaP1 = 8.0 * ShearModP1;
        }
        if (m_planeStrain==true and m_planeStress==false){
           // c = (12.0*K-4.0*MU) / 9.0 * (*theta) / weightedVol;
            // kappa = c / (*theta) / gamma / 3 (?) (above eq (7) lammiCJ
            //kappa = 3*(12.0*K-4.0*MU) / 18.0 * (*theta) / weightedVol;

            gammaP1 = 2.0/3.0;
            kappaP1 = 6.0*BulkModP1 - 2.0*ShearModP1;
            alphaP1 = 8.0*ShearModP1;
        }
        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            trialDamage = 0.0;
            neighborID = neighborhoodList[neighborhoodListIndex++];

            double zeta =
            distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                 x[neighborID*3], x[neighborID*3+1], x[neighborID*3+2]);

            double dY =
            distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2],
                 y[neighborID*3], y[neighborID*3+1], y[neighborID*3+2]);

            relativeExtension = (dY - zeta)/zeta;
             // the direction switches between both bonds. This results in a switch of the forces
             // as well. Therefore, all forces and bond deformations are normalized.
            double eP = dY - zeta;
            if(deltaTemperature)
              eP -= 0.5 * m_alpha*(deltaTemperature[nodeId] + deltaTemperature[neighborID])*zeta;

            // the average horizon is taken, if multiple horizons are used
            dilatationP2 = damageModel[3*neighborID];
            BulkModP2    = damageModel[3*neighborID+1]; // weightedVolume is already included in the variable --> Perdigm_Elastic.cpp;
            ShearModP2   = damageModel[3*neighborID+2]; // weightedVolume is already included in --> Perdigm_Elastic.cpp;

            if (m_planeStress){
              //  c = 4.0*K*MU/(3.0*K+4.0*MU) * (*theta) / weightedVol;
                // kappa = c / (*theta) / gamma / 3 (?) (above eq (7) lammiCJ
                kappaP2 = 3.0 * BulkModP2;
                gammaP2 = 4.0 * ShearModP2 / (3.0 * BulkModP2 + 4.0 * ShearModP2);
                alphaP2 = 8.0 * ShearModP2;
            }
            if (m_planeStrain){
               // c = (12.0*K-4.0*MU) / 9.0 * (*theta) / weightedVol;
                // kappa = c / (*theta) / gamma / 3 (?) (above eq (7) lammiCJ
                //kappa = 3*(12.0*K-4.0*MU) / 18.0 * (*theta) / weightedVol;

                gammaP2 = 2.0/3.0;
                kappaP2 =(12.0*BulkModP2 - 4.0*ShearModP2) / 3.0 / gammaP2;
                alphaP2 = 8.0 * ShearModP2;

            }
            if (m_planeStress==false and m_planeStrain==false){
              //  c = 3 * K * (*theta) / weightedVol;
                // kappa = c / (*theta) / gamma / 3 (?) (above eq (7) lammiCJ
                kappaP2 = 9.0 * BulkModP2;
                gammaP2 = 1.0;
                alphaP2 = 15.0 * ShearModP2 ;
            }

            omegaP1 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[nodeId]);
            omegaP2 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[neighborID]);
            avgHorizon = 0.5*(horizon[nodeId]+horizon[neighborID]);

            if(m_planeStrain or m_planeStress){ // m_Thickness is in the volume included (tbd to guarantee this)
            //factor = 16.0 / 5.0 / weightedVol * sqrt(M_PI / 3.0 * pow(horizon,5)); // Simplified with Mathematica
                quadhorizon =  3.0 /(m_Thickness * pow(horizon[neighborID],3) );
				if (m_rot){
					double rad = nodeInitialX[0]*nodeInitialX[0]/maxRad/maxRad;
					//if (rad == 0) rad = 1e-10;
					quadhorizon = quadhorizon*(1+rad);
				}
            }
            else {
                //yieldValue = 25.0 * yieldStress * yieldStress / 8 / M_PI / pow(horizon,5);
                //factor = sqrt(75.0 / 4 / M_PI / pow(horizon,5));        // equation (51) MitchelJA_2011; 1/2 is not needed
                //factor = 15.0 / weightedVol * sqrt(4.0 / 75.0 * M_PI * pow(horizon,5));        // equation (51) MitchelJA_2011; 1/2 is not needed

                quadhorizon =  8.0 /( m_pi * pow(avgHorizon, 4) );
            }

            eiP1 = gammaP1 * dilatationP1 * zeta / 3.0;
            eiP2 = gammaP2 * dilatationP2 * zeta / 3.0;
            tiP1 = kappaP1*eiP1;
            tiP2 = kappaP2*eiP2;

            edP1 = eP - eiP1;
            edP2 = eP - eiP2;
            tdP1 = omegaP1*alphaP1*edP1;
            tdP2 = omegaP2*alphaP2*edP2;

            bondEnergyIsotropic  = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tiP1*eiP1*tiP1*eiP1) + sqrt(tiP2*eiP2*tiP2*eiP2));
            bondEnergyDeviatoric = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tdP1*edP1*tdP1*edP1) + sqrt(tdP2*edP2*tdP2*edP2));

            critIso = 0.0;
            if (relativeExtension>0&&criticalEnergyTension != -1.0){
               critIso = (bondEnergyIsotropic/(criticalEnergyTension*quadhorizon));
            }
            critDev = 0.0;

            if (criticalEnergyShear != -1.0){
               critDev = (bondEnergyDeviatoric/(criticalEnergyShear*quadhorizon));
            }
            critComp = 0.0;
            if (relativeExtension < 0.0 && criticalEnergyCompression != -1.0){
                critComp = (bondEnergyIsotropic/(criticalEnergyCompression*quadhorizon));
                critIso = 0.0;
            }
            if (m_type == 1){ // Energy Criterion by Foster et al.(2009) Journal for Multiscale Computational Engineering;
                bondEnergy = abs(abs(tdP1+tiP1) + abs(tdP2 + tiP2))*eP;

                //bondEnergy = abs(damageModel[3*nodeId]-damageModel[3*neighborID])*zeta / *m;

                critIso = bondEnergy/(criticalEnergyTension*quadhorizon);

                if (m_criticalEnergyTension > 0.0 && critIso > 1.0 ) {
					//std::cout<<BulkModP1<< " "<< weightedVolume[nodeId]<<" "<< ShearModP1<< " "<< ShearModP2<<" "<< BulkModP2<<std::endl;
                    trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }

            if (m_type == 2){// Power Law

                if (m_criticalEnergyTension > 0.0 &&critIso*critIso + critDev*critDev + critComp*critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }
            //std::cout<< m_type<< " "<< std::endl;
            if (m_type == 3){ // Separated
                if (m_criticalEnergyTension > 0.0  &&critIso > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
                if (criticalEnergyShear > 0.0 && critDev > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;

                }
                if (m_criticalEnergyCompression > 0.0  && critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }
            if (m_onlyTension == true){
                if (relativeExtension<0)trialDamage = 0;
            }

            if (trialDamage > bondDamageNP1[bondIndex]) {
               if (trialDamage>1)trialDamage = 1;
                            bondDamageNP1[bondIndex] = trialDamage;
                    }

            bondIndex += 1;

            }

        }

    //  Update the element damage (percent of bonds broken)

    neighborhoodListIndex = 0;
    bondIndex = 0;
    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        //neighborhoodListIndex += numNeighbors;
        damageModel[3*nodeId] = 0.0;
        damageModel[3*nodeId+1] = 0.0;
        damageModel[3*nodeId+2] = 0.0;
        totalDamage = 0.0;
        totalVol = 0.0;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            neighborID = neighborhoodList[neighborhoodListIndex++];
            // must be zero to avoid synchronization errors
            damageModel[3*neighborID] = 0.0;
            damageModel[3*neighborID+1] = 0.0;
            damageModel[3*neighborID+2] = 0.0;

            totalDamage += bondDamageNP1[bondIndex]*weightedVolume[nodeId];
            totalVol += weightedVolume[nodeId];
            bondIndex += 1;
        }
        if (numNeighbors > 0)
            totalDamage /= (totalVol);
        else
            totalDamage = 0.0;
        damage[nodeId] = totalDamage;
    }
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120725    E. Dekens         File created.
 *
 *    References
 *      Vallado, D.A. Fundamentals of Astrodynamics and Applications website.
 *        URL http://www.smad.com/vallado, 2001. Last accessed: 25-07-2012.
 *      Wakker, K.F. Astrodynamics I, reader of course AE4874 I. Delft University of Technology,
 *        2007.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/clohessyWiltshirePropagator.h"

namespace tudat
{
namespace unit_tests
{

//! Test suite for Clohessy-Wiltshire propagation.
BOOST_AUTO_TEST_SUITE( test_clohessyWiltshirePropagation )

// Testcase 1: propagation of full state in low-Earth orbit.
// This test benchmarks the propagation of a full state against external data.
BOOST_AUTO_TEST_CASE( test_ClohessyWiltshirePropagation_fullState )
{
    // Set central body gravitational parameter [m^3 s^-2].
    // In this case: central body is Earth.
    const double centralBodyGravitationalParameter1 = 3.986004418e14;

    // Set propagation duration [s].
    // In this case: propagation duration is 30 minutes.
    const double propagationDuration1 = 1800.0;

    // Set reference orbit radius [m].
    // In this case: reference orbit is at 400 km altitude above Earth.
    const double referenceOrbitRadius1 = 6.778137e6;

    // Set initial state [m], [m], [m], [m/s], [m,s], [m/s].
    // In this case: arbitrary non-zero distances and velocities.
    const basic_mathematics::Vector6d initialState1 =
            ( basic_mathematics::Vector6d( ) << 45.0, 37.0, 12.0, 0.08,
              0.03, 0.01 ).finished( );

    // Calculate final state according to Tudat function.
    const basic_mathematics::Vector6d computedFinalState1
            = basic_astrodynamics::propagateClohessyWiltshire(
                initialState1,
                propagationDuration1,
                centralBodyGravitationalParameter1,
                referenceOrbitRadius1 );

    // Set final state according to the MATLAB routine "hillsr" from Vallado [2001].
    const basic_mathematics::Vector6d expectedFinalState1 =
            ( basic_mathematics::Vector6d( ) << 3.806450080201250e2,
              -5.437424675454679e2, 2.509547637285142,
              1.541620605755606e-1, -7.294751390499470e-1,
              -1.662099488431618e-2 ).finished( );

    // Check if computed final state matches expected state.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedFinalState1, expectedFinalState1, 1.0e-14 );
}

// Testcase 2: pure harmonic relative motion in low-Earth orbit.
//   According to Wakker [2007] pure harmonic relative motion occurs if two conditions are met:
//   (1) initialRadialVelocity = 0.5 * meanAngularMotion * initialAlongTrackPosition.
//   (2) initialAlongTrackVelocity = -2.0 * meanAngularMotion * initialRadialPosition.
//   Therefore the final state after one orbital revolution must be exactly equal to the initial
//   state. This testcase verifies this.
BOOST_AUTO_TEST_CASE( test_ClohessyWiltshirePropagation_harmonicMotion )
{
    // Set central body gravitational parameter [m^3 s^-2].
    // In this case: central body is Earth.
    const double centralBodyGravitationalParameter2 = 3.986004418e14;

    // Set reference orbit radius [m].
    // In this case: reference orbit is at 400 km altitude above Earth.
    const double referenceOrbitRadius2 = 6.778137e6;

    // Calculate mean angular motion of reference orbit [rad].
    const double meanAngularMotion =
            std::sqrt( centralBodyGravitationalParameter2
                       / ( referenceOrbitRadius2 * referenceOrbitRadius2
                           * referenceOrbitRadius2 ) );

    // Set propagation duration [s].
    // In this case: orbital period of the reference orbit.
    const double propagationDuration2 = 2.0 * mathematical_constants::PI
            / meanAngularMotion;

    // Set initial state [m], [m], [m], [m/s], [m,s], [m/s].
    // In this case: arbitrary values for initial positions and initialCrossTrackVelocity. The
    // initialRadialVelocity and initialAlongTrackVelocity are set to achieve harmonic motion.
    const basic_mathematics::Vector6d initialState2 =
            ( basic_mathematics::Vector6d( )
              << 34.0, 49.0 , 17.0,
              0.5 * meanAngularMotion * 49.0,
              -2.0 * meanAngularMotion * 34.0, 0.04 ).finished( );

    // Calculate final state according to Tudat function.
    const basic_mathematics::Vector6d computedFinalState2
            = basic_astrodynamics::propagateClohessyWiltshire(
                initialState2,
                propagationDuration2,
                centralBodyGravitationalParameter2,
                referenceOrbitRadius2 );

    // Check if computed final state matches the initial state.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( initialState2, computedFinalState2, 1.0e-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
 * Copyright 2015 David A. Boyuka II
 *
 * 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.
 */

/*
 * test-dilate.cpp
 *
 *  Created on: Sep 17, 2015
 *      Author: David A. Boyuka II
 */

#include <cstdlib>
#include <boost/random/mersenne_twister.hpp>

#include "pique/util/dilate.hpp"

template<typename word_t>
void test_rand() {
	boost::mt19937_64 rand; // default seed is fine

	static constexpr int N = 1000;
	for (int i = 0; i < N; ++i) {
		const word_t word = (word_t)(rand() >> (std::numeric_limits<uint64_t>::digits - std::numeric_limits<word_t>::digits/2)); // restrict the bits we use to the low half for valid dilation
		const word_t dilated_word = Dilater<word_t, 2>::dilate(word);
		const word_t word2 = Dilater<word_t, 2>::undilate(dilated_word);
		assert(word == word2);
	}
}

template<typename word_t>
void test_boundary_cases() {
	word_t dilated_word, undilated_word;

	const word_t zero_undilated = 0;
	const word_t zero_dilated = 0;
	dilated_word = Dilater<word_t, 2>::dilate(zero_undilated);
	assert(dilated_word == zero_dilated);
	undilated_word = Dilater<word_t, 2>::undilate(dilated_word);
	assert(undilated_word == zero_undilated);

	const word_t max_undilated = (word_t)~((word_t)0) >> (std::numeric_limits<word_t>::digits/2);
	word_t max_dilated = 0;
	for (int i = 0; i < std::numeric_limits<word_t>::digits; i += 2)
		max_dilated |= ((word_t)1 << i);

	dilated_word = Dilater<word_t, 2>::dilate(max_undilated);
	assert(dilated_word == max_dilated);
	undilated_word = Dilater<word_t, 2>::undilate(dilated_word);
	assert(undilated_word == max_undilated);
}

template<typename word_t>
void test_word_type() {
	test_rand<word_t>();
	test_boundary_cases<word_t>();
}

int main(int argc, char **argv) {
	test_word_type<uint8_t>();
	test_word_type<uint16_t>();
	test_word_type<uint32_t>();
	test_word_type<uint64_t>();
}


#ifndef INCLUDED_DENSITY_SMOOTH_HPP
#define INCLUDED_DENSITY_SMOOTH_HPP

#include <Eigen/Dense>

namespace density
{

	/** Gaussian blur using density-adaptive radius as kernel radius */
	Eigen::MatrixXf DensityAdaptiveSmooth(const Eigen::MatrixXf& d);

}

#endif


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

#ifndef MTL_SET_TO_0_INCLUDE
#define MTL_SET_TO_0_INCLUDE

#include <algorithm>
#include <cassert>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    // Forward declarations
    namespace matrix {
	template <typename Coll>
	typename mtl::traits::enable_if_matrix<Coll>::type
	set_to_zero(Coll& collection);
    }
    namespace vector {
	template <typename Coll>
	typename mtl::traits::enable_if_vector<Coll>::type
	set_to_zero(Coll& collection);
    }

    namespace impl {

	template <typename Coll>
	void set_to_zero(Coll& collection, tag::vector_ref, ashape::scal)
	{
	    using math::zero;
	    typename Collection<Coll>::value_type  ref, my_zero(zero(ref));
	    for (typename Collection<Coll>::size_type i= 0; i < size(collection); ++i)
		collection[i]= my_zero;
	}

	template <typename Coll>
	void set_to_zero(Coll& collection, tag::contiguous_dense, ashape::scal)
	{
	    using math::zero;
	    typename Collection<Coll>::value_type  ref, my_zero(zero(ref));

	    std::fill(collection.elements(), collection.elements()+collection.used_memory(), my_zero);
	}

	template <typename Coll>
	void set_to_zero(Coll& collection, tag::std_vector, ashape::scal)
	{
	    using math::zero;
	    typename Collection<Coll>::value_type  ref, my_zero(zero(ref));

	    std::fill(collection.begin(), collection.end(), my_zero);
	}

	template <typename Matrix>
	void set_to_zero(Matrix& matrix, tag::morton_dense, ashape::scal)
	{
	    using math::zero;
	    typename Collection<Matrix>::value_type  ref, my_zero(zero(ref));
	    // maybe faster to do it straight
	    // if performance problems we'll take care of the holes
	    // std::cout << "set_to_zero: used_memory = " << matrix.used_memory() << "\n";
	    std::fill(matrix.elements(), matrix.elements() + matrix.used_memory(), my_zero);

#if 0
	    for (int i= 0; i < matrix.num_rows(); i++)
	      for (int j= 0; j < matrix.num_cols(); j++)
		matrix[i][j]= my_zero;
#endif
	}

	// For nested collection, we must consider the dimensions of the elements
	// (Morton-order is included in contiguous_dense)
	template <typename Coll>
	void set_to_zero(Coll& collection, tag::contiguous_dense, ashape::nonscal)
	{
	    for (typename Collection<Coll>::size_type i= 0; i < collection.used_memory(); ++i)
		set_to_zero(collection.value_n(i));
	}

	// Is approbriate for all sparse matrices and vectors (including collections as value_type)
	template <typename Coll>
	void set_to_zero(Coll& collection, tag::sparse, ashape::universe)
	{
	    collection.make_empty();
	}

	// Special treatment for multi_vector
	template <typename Coll>
	void set_to_zero(Coll& collection, tag::multi_vector, ashape::universe)
	{
	    using mtl::vector::set_to_zero;
	    for (typename Collection<Coll>::size_type i= 0; i < num_cols(collection); ++i)
		set_to_zero(collection.vector(i));
	}

	template <typename Coll>
	bool has_strided_data(const Coll&)
	{ return false; }

	template <typename Value, typename Parameter>
	bool has_strided_data(const matrix::dense2D<Value, Parameter>& A)
	{ return A.has_strided_data(); }

	template <typename Matrix>
	void naive_set_to_zero(Matrix& A, tag::matrix, tag::dense)
	{
	    using math::zero;
	    typename Collection<Matrix>::value_type  ref, my_zero(zero(ref));

	    for (unsigned i= 0; i < num_rows(A); i++)
		for (unsigned j= 0; j < num_cols(A); j++)
		    A[i][j]= my_zero;
	}

	template <typename Matrix>
	void naive_set_to_zero(Matrix&, tag::matrix, tag::sparse)
	{
	    assert(true); // must not be called
	}

    }

namespace matrix {

    /// Sets all values of a collection to 0
    /// More spefically the defined multiplicative identity element
    template <typename Coll>
    typename mtl::traits::enable_if_matrix<Coll>::type
    set_to_zero(Coll& collection)
    {
	using mtl::traits::category;
	vampir_trace<3031> tracer;
	typedef typename Collection<Coll>::value_type value_type;
	if (mtl::impl::has_strided_data(collection))
	    mtl::impl::naive_set_to_zero(collection, typename category<Coll>::type(), typename category<Coll>::type());
	else
	    mtl::impl::set_to_zero(collection, typename category<Coll>::type(),typename ashape::ashape<value_type>::type()); // 2. ashape ???
    }
}

namespace vector {

    /// Sets all values of a collection to 0
    /// More spefically the defined multiplicative identity element
    template <typename Coll>
    typename mtl::traits::enable_if_vector<Coll>::type
    set_to_zero(Coll& collection)
    {
	using mtl::traits::category;
	vampir_trace<2029> tracer;
	typedef typename Collection<Coll>::value_type value_type;
	mtl::impl::set_to_zero(collection, typename category<Coll>::type(),typename ashape::ashape<value_type>::type());
    }

}

} // namespace mtl

#endif // MTL_SET_TO_0_INCLUDE


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ZK_BLUEPRINT_ELEMENT_FP2_COMPONENT_TEST_HPP
#define CRYPTO3_ZK_BLUEPRINT_ELEMENT_FP2_COMPONENT_TEST_HPP

#include <boost/test/unit_test.hpp>

using namespace nil::crypto3::zk;

template <typename FieldType, template<class> class Fpk_variableT,
          template<class> class Fpk_mul_componentT>
components::blueprint<typename FieldType::base_field_type> test_field_element_mul(typename FieldType::value_type a_value,
                                                      typename FieldType::value_type b_value){
    using field_type = FieldType;
    using element_component = Fpk_variableT<field_type>;
    using element_mul_component = Fpk_mul_componentT<field_type>;
    using base_field_type = typename field_type::base_field_type;

    components::blueprint<base_field_type> bp;

    element_component A(bp, a_value);
    element_component B(bp, b_value);
    element_component result(bp);

    element_mul_component el_mul_instance(bp, A, B, result);
    el_mul_instance.generate_r1cs_constraints();
    el_mul_instance.generate_r1cs_witness();

    const typename field_type::value_type res = result.get_element();

    BOOST_CHECK(bp.is_satisfied());
    BOOST_CHECK(res == (a_value * b_value));

    return bp;
}

template <typename FieldType, template<class> class Fpk_variableT,
          template<class> class Fpk_squared_componentT>
components::blueprint<typename FieldType::base_field_type> test_field_element_squared(typename FieldType::value_type a_value){
    using field_type = FieldType;
    using element_component = Fpk_variableT<field_type>;
    using element_squared_component = Fpk_squared_componentT<field_type>;
    using base_field_type = typename field_type::base_field_type;

    components::blueprint<base_field_type> bp;

    element_component A(bp, a_value);
    element_component result(bp);

    element_squared_component el_squared_instance(bp, A, result);
    el_squared_instance.generate_r1cs_constraints();
    el_squared_instance.generate_r1cs_witness();

    const typename field_type::value_type res = result.get_element();

    BOOST_CHECK(bp.is_satisfied());
    BOOST_CHECK(res == (a_value.squared()));

    return bp;
}

#endif    // CRYPTO3_ZK_BLUEPRINT_ELEMENT_FP2_COMPONENT_TEST_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_MINIMUM_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_REDUCTION_FUNCTIONS_SIMD_SSE_SSE2_MINIMUM_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/toolbox/reduction/functions/minimum.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/simd/include/functions/simd/min.hpp>

namespace boost { namespace simd { namespace ext
{
  //============================================================================
  // Implementation when type A0 is int16_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0 > ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 min1 = _mm_shufflehi_epi16(a0  , _MM_SHUFFLE(1, 0, 3, 2));
         min1 = _mm_shufflelo_epi16(min1, _MM_SHUFFLE(1, 0, 3, 2));
         min1 = min(a0, min1);
      A0 min2 = _mm_shuffle_epi32  (min1, _MM_SHUFFLE(1, 0, 3, 2));
         min2 = _mm_shufflelo_epi16(min2, _MM_SHUFFLE(1, 0, 3, 2));
         min2 = min(min1, min2);
      A0 min3 = _mm_shuffle_epi32(min2, _MM_SHUFFLE(3, 2, 1, 0));
      A0 min4 = _mm_shufflelo_epi16(min3, _MM_SHUFFLE(0, 1, 2, 3));
      A0 that = boost::simd::min(min3, min4);
      return that[0];
    }
  };

  //============================================================================
  // Implementation when type A0 is double
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0> ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 that = _mm_min_sd(a0, _mm_unpackhi_pd(a0,a0));
      return that[0];
    }
  };

  //============================================================================
  // Implementation when type A0 is int64_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints64_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0 > ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return boost::simd::min(a0[0], a0[1]);
    }
  };

  //============================================================================
  // Implementation when type A0 is float
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    ,(A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0 > ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 min  = _mm_min_ps(a0, _mm_movehl_ps(a0,a0));
      A0 that = _mm_min_ss(min, _mm_shuffle_ps(min,min,0x01));
      return that[0];
    }
  };

  //============================================================================
  // Implementation when type A0 is int8_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0>::type              result_type;
    typedef typename meta::make_dependent<double,A0>::type  double_t;
    typedef simd::native<double_t,boost::simd::tag::sse_>   rtype;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      rtype v0 = simd::bitwise_cast<rtype>(a0);
      A0 pack = simd::bitwise_cast<A0>(_mm_unpackhi_pd(v0,v0));
      A0 min1 = boost::simd::min(a0,pack);
      A0 min2 = min(min1, simd::bitwise_cast<A0>(_mm_shufflelo_epi16(min1, _MM_SHUFFLE(0, 1, 2, 3))));
         min2 = min(min2, simd::bitwise_cast<A0>(_mm_shuffle_epi32  (min2, _MM_SHUFFLE(2, 3, 0, 1))));
      return boost::simd::min(min2[0],min2[1]);
    }
  };

  //============================================================================
  // Implementation when type A0 is int32_
  //============================================================================
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::minimum_
                                    , boost::simd::tag::sse2_
                                    ,(A0)
                                    ,((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                                    )
  {
    typedef typename meta::scalar_of<A0 > ::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 min1 = min(a0,simd::bitwise_cast<A0>(_mm_shuffle_epi32(a0, _MM_SHUFFLE(1, 0, 3, 2))));
      A0 that = min(min1, simd::bitwise_cast<A0>(_mm_shuffle_epi32(min1, _MM_SHUFFLE(2, 3, 0, 1))));
      return that[0];
    }
  };
} } }

#endif
#endif


/********************************************************************
	created:	2012/11/16
	created:	16:11:2012   16:59
	filename: 	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\test\EnvelopFinderTest.cpp
	file path:	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\test
	file base:	EnvelopFinderTest
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/

#include <GAGPL/SPECTRUM/EnvelopFinder.h>
#include <GAGPL/SPECTRUM/PeakList.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <utility>
#include <boost/algorithm/string.hpp>

using namespace gag;
using namespace std;

RichList readSpectrum()
{
	std::cout << "Load spectrum..." << std::endl;

	std::ifstream infile("../data/spectrum.txt");

	std::string line;

	RichList spec;
	if(infile.is_open())
	{
		while(std::getline(infile, line))
		{
			std::istringstream is;
			is.str(line);
			double k1, k2, k3, k4;
			is >> k1 >> k2 >> k3 >> k4;
			//gag::Peak pk(k1, k2);
			RichPeakPtr pk = createRichPeak(k1, k2, k3, k4);
			spec.addPeak(pk);
		}
	}
	infile.close();
	infile.clear();
	return spec;

}

int main(int argc, char *argv[])
{
	// Test 1
	cout << "Test 1: Manual interpretation" << endl;
	RichList spec1;
	RichPeakPtr pk0 = createRichPeak(376.49227,2125405,5184,3.7);
	RichPeakPtr pk1 = createRichPeak(376.99506,51087228,311536,130);
	RichPeakPtr pk2 = createRichPeak(377.32956,12896527,68383,31.5);
	RichPeakPtr pk3 = createRichPeak(377.65959,3833504,4942,8.1);
	RichPeakPtr pk4 = createRichPeak(377.66405,4087202,5864,8.7);
	RichPeakPtr pk5 = createRichPeak(377.99828,1626549,5004,2.4);
	spec1.addPeak(pk0);
	spec1.addPeak(pk1);
	spec1.addPeak(pk2);
	spec1.addPeak(pk3);
	spec1.addPeak(pk4);
	spec1.addPeak(pk5);

	EnvelopFinder env_finder1(spec1);

	std::vector<EnvelopPtr> env_pool1 = env_finder1.getEnvelops();
	BOOST_FOREACH(EnvelopPtr env, env_pool1)
	{
		env_finder1.printEnvelop(env);
	}
	// Test 2
	//cout << endl;
	//cout << "Test 2: Real spectrum" << endl;
	//// 1. TBD: Read the data and examine the format of the data.
	//
	//RichList spectrum = readSpectrum();
	//spectrum.printPeakList<peak_mz>();

	//EnvelopFinder env_finder2(spectrum);

	//// 2. print the identified envelops.
	//std::vector<EnvelopPtr> env_pool = env_finder2.getEnvelops();

	//BOOST_FOREACH(EnvelopPtr env, env_pool)
	//{
	//	env_finder2.printEnvelop(env);
	//}

}


#ifndef ALM_TASK_MAIN_H
#define ALM_TASK_MAIN_H

#include <Eigen/Dense>

#include <avatar_locomanipulation/models/robot_model.hpp>
#include <avatar_locomanipulation/helpers/orientation_utils.hpp>

#include <string>
#include <iostream>

#define TASK_DIM_X 0
#define TASK_DIM_Y 1
#define TASK_DIM_Z 2
#define TASK_DIM_RX 3
#define TASK_DIM_RY 4
#define TASK_DIM_RZ 5

class Task{
public:
	Task();
	Task(std::shared_ptr<RobotModel> & input_model);
	virtual ~Task();

	virtual void getTaskJacobian(Eigen::MatrixXd & J_task);
	virtual void getTaskJacobianDot(Eigen::MatrixXd & Jdot_task);

	// Sets a reference for the task
	// vec_ref can be a position or a custom
	virtual void setReference(const Eigen::VectorXd & vec_ref_in);
	virtual void setReference(const Eigen::Quaterniond & quat_ref_in);
	virtual void setReference(const Eigen::VectorXd & vec_ref_in, const Eigen::Quaterniond & quat_ref_in);

	// Gets the currently set References
	virtual void getReference(Eigen::VectorXd & vec_ref_out);
	virtual void getReference(Eigen::VectorXd & vec_ref_out, Eigen::Quaterniond & quat_ref_out);
	virtual void getReference(Eigen::Quaterniond & quat_ref_out);

	// Computes the error for a given reference
	virtual void computeError();
	// Gets the current error
	virtual void getError(Eigen::VectorXd & error_out, bool compute=true);
	// Sets the task error manually
	virtual void setError(const Eigen::VectorXd & error_in);
	// Set the Task's kp gain value. Default = 1.0;
	virtual void setTaskGain(const double & kp_task_gain_in);

	int task_dim = 0;
	std::shared_ptr<RobotModel> robot_model;
	std::string task_name = "empty task";
	std::string frame_name = "no frame";

protected:
	double kp_task_gain_ = 1.0;
	Eigen::VectorXd vec_ref_;
	Eigen::Quaterniond quat_ref_;
	Eigen::VectorXd error_;

};

#endif


/*******************************************************************************
 * This file is part of KaHyPar.
 *
 * Copyright (C) 2019 Sebastian Schlag <sebastian.schlag@kit.edu>
 *
 * KaHyPar is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * KaHyPar is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with KaHyPar.  If not, see <http://www.gnu.org/licenses/>.
 *
******************************************************************************/

#include <boost/algorithm/string/predicate.hpp>

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>

#include "kahypar/definitions.h"
#include "kahypar/io/hypergraph_io.h"
#include "kahypar/io/partitioning_output.h"
#include "kahypar/macros.h"
#include "kahypar/partition/context.h"
#include "kahypar/partition/metrics.h"
#include "tools/mtx_to_hgr_conversion.h"

using namespace kahypar;
using mtxconversion::Matrix;

static inline double imb(const Hypergraph& hypergraph, const PartitionID k) {
  HypernodeWeight max_weight = hypergraph.partWeight(0);
  for (PartitionID i = 1; i != k; ++i) {
    max_weight = std::max(max_weight, hypergraph.partWeight(i));
  }
  return static_cast<double>(max_weight) /
         ceil(static_cast<double>(hypergraph.totalWeight()) / k) - 1.0;
}

static inline Hypergraph createHypergraphFromMtx(const std::string& filename,
                                                  const PartitionID num_parts) {
  // we use the row-net model
  Matrix matrix = mtxconversion::readMatrix(filename);
  HypernodeID num_hypernodes = matrix.info.num_columns;
  HyperedgeID num_hyperedges = matrix.info.num_rows;
  HyperedgeIndexVector index_vector;
  HyperedgeVector edge_vector;

  index_vector.push_back(edge_vector.size());
  for (const auto& hyperedge : matrix.data.entries) {
    if (hyperedge.size() != 0) {
      for (const auto& pin : hyperedge) {
        edge_vector.push_back(pin);
      }
      index_vector.push_back(edge_vector.size());
    }
  }
  ALWAYS_ASSERT(matrix.info.object == mtxconversion::MatrixObjectType::WEIGHTED_MATRIX
                || matrix.data.weights.empty(), "Weights not allowed");
  return Hypergraph(num_hypernodes, num_hyperedges, index_vector, edge_vector,
                    num_parts, HyperedgeWeightVector{ }, matrix.data.weights);
}

int main(int argc, char* argv[]) {
  if (argc != 2 && argc != 3) {
    std::cout << "No .hgr file specified" << std::endl;
    std::cout << "Usage: EvaluateMondriaanPartiton <.hgr/.mtx> <partition file>" << std::endl;
    exit(0);
  }
  const std::string hypergraph_filename(argv[1]);
  Hypergraph hypergraph;
  if (boost::algorithm::ends_with(hypergraph_filename, ".mtx")) {
    hypergraph = createHypergraphFromMtx(hypergraph_filename, 2);
  } else {
    hypergraph = io::createHypergraphFromFile(hypergraph_filename, 2);
  }

  PartitionID max_part = 1;
  std::vector<PartitionID> partition(hypergraph.initialNumNodes(), -1);
  if (argc == 3) {
    const std::string partition_filename(argv[2]);
    std::cout << "Reading partition file: " << partition_filename << std::endl;
    std::ifstream file(partition_filename);
    if (file) {
      HypernodeID vertex = -1;
      PartitionID part = -1;
      // header
      file >> vertex >> max_part;
      ALWAYS_ASSERT(vertex == hypergraph.initialNumNodes());
      while (file >> vertex >> part) {
        // one-based
        partition[vertex - 1] = part - 1;
      }
      file.close();
    } else {
      std::cerr << "Error: File not found: " << std::endl;
    }
    ALWAYS_ASSERT(std::none_of(std::begin(partition), std::end(partition),
                               [](PartitionID i) {
      return i == -1;
    }));
  }

  if (partition.size() != 0 && partition.size() != hypergraph.initialNumNodes()) {
    std::cout << "partition file has incorrect size. Exiting." << std::endl;
    exit(-1);
  }

  LOG << V(max_part);
  hypergraph.changeK(max_part);

  for (size_t index = 0; index < partition.size(); ++index) {
    hypergraph.setNodePart(index, partition[index]);
  }

  Context context;
  context.partition.k = max_part;

  for (PartitionID i = 0; i < context.partition.k; ++i) {
    LOG << i << V(hypergraph.partSize(i)) << V(hypergraph.partWeight(i));
  }

  std::cout << "***********************" << hypergraph.k()
            << "-way Partition Result************************" << std::endl;
  std::cout << "cut=" << metrics::hyperedgeCut(hypergraph) << std::endl;
  std::cout << "soed=" << metrics::soed(hypergraph) << std::endl;
  std::cout << "km1= " << metrics::km1(hypergraph) << std::endl;
  std::cout << "absorption= " << metrics::absorption(hypergraph) << std::endl;
  std::cout << "imbalance= " << imb(hypergraph, context.partition.k)
            << std::endl;

  std::cout << "RESULT"
            << " graph=" << hypergraph_filename.substr(hypergraph_filename.find_last_of('/') + 1)
            << " k=" << context.partition.k
            << " imbalance=" << imb(hypergraph, context.partition.k)
            << " cut=" << metrics::hyperedgeCut(hypergraph)
            << " soed=" << metrics::soed(hypergraph)
            << " km1=" << metrics::km1(hypergraph) << std::endl;

  return 0;
}


#pragma once

#include <iostream>

#include <Eigen/Geometry>

#include <ipc/utils/eigen_ext.hpp>
#include <ipc/utils/logger.hpp>

namespace ipc {

template <typename DerivedP, typename DerivedE0, typename DerivedE1>
PointEdgeDistanceType point_edge_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedE0>& e0,
    const Eigen::MatrixBase<DerivedE1>& e1)
{
    assert(p.size() == 2 || p.size() == 3);
    assert(e0.size() == 2 || e0.size() == 3);
    assert(e1.size() == 2 || e1.size() == 3);

    const auto e = e1 - e0;
    const auto e_length_sqr = e.squaredNorm();
    if (e_length_sqr == 0) {
        IPC_LOG(warn("Degenerate edge in point_edge_distance_type!"));
        return PointEdgeDistanceType::P_E0; // WARNING: use arbitrary end-point
    }
    auto ratio = e.dot(p - e0) / e_length_sqr;
    if (ratio < 0) {
        return PointEdgeDistanceType::P_E0; // PP (p-e0)
    } else if (ratio > 1) {
        return PointEdgeDistanceType::P_E1; // PP (p-e1)
    } else {
        return PointEdgeDistanceType::P_E; // PE
    }
}

template <
    typename DerivedP,
    typename DerivedT0,
    typename DerivedT1,
    typename DerivedT2>
PointTriangleDistanceType point_triangle_distance_type(
    const Eigen::MatrixBase<DerivedP>& p,
    const Eigen::MatrixBase<DerivedT0>& t0,
    const Eigen::MatrixBase<DerivedT1>& t1,
    const Eigen::MatrixBase<DerivedT2>& t2)
{
    typedef typename DerivedP::Scalar T;

    assert(p.size() == 3);
    assert(t0.size() == 3);
    assert(t1.size() == 3);
    assert(t2.size() == 3);

    Eigen::Matrix<T, 2, 3> basis;
    basis.row(0) = t1 - t0;
    basis.row(1) = t2 - t0;

    auto normal = cross(basis.row(0), basis.row(1));

    Eigen::Matrix<T, 2, 3> param;

    basis.row(1) = cross(basis.row(0), normal);
    param.col(0) =
        (basis * basis.transpose()).ldlt().solve(basis * Vector3<T>(p - t0));
    if (param(0, 0) > 0.0 && param(0, 0) < 1.0 && param(1, 0) >= 0.0) {
        return PointTriangleDistanceType::P_E0; // edge 0 is the closest
    } else {
        basis.row(0) = t2 - t1;
        basis.row(1) = cross(basis.row(0), normal);
        param.col(1) = (basis * basis.transpose())
                           .ldlt()
                           .solve(basis * Vector3<T>(p - t1));
        if (param(0, 1) > 0.0 && param(0, 1) < 1.0 && param(1, 1) >= 0.0) {
            return PointTriangleDistanceType::P_E1; // edge 1 is the closest
        } else {
            basis.row(0) = t0 - t2;

            basis.row(1) = cross(basis.row(0), normal);
            param.col(2) = (basis * basis.transpose())
                               .ldlt()
                               .solve(basis * Vector3<T>(p - t2));
            if (param(0, 2) > 0.0 && param(0, 2) < 1.0 && param(1, 2) >= 0.0) {
                return PointTriangleDistanceType::P_E2; // edge 2 is the closest
            } else {
                if (param(0, 0) <= 0.0 && param(0, 2) >= 1.0) {
                    // vertex 0 is the closest
                    return PointTriangleDistanceType::P_T0;
                } else if (param(0, 1) <= 0.0 && param(0, 0) >= 1.0) {
                    // vertex 1 is the closest
                    return PointTriangleDistanceType::P_T1;
                } else if (param(0, 2) <= 0.0 && param(0, 1) >= 1.0) {
                    // vertex 2 is the closest
                    return PointTriangleDistanceType::P_T2;
                } else {
                    return PointTriangleDistanceType::P_T;
                }
            }
        }
    }
}

// A more robust implementation of http://geomalgorithms.com/a07-_distance.html
template <
    typename DerivedEA0,
    typename DerivedEA1,
    typename DerivedEB0,
    typename DerivedEB1>
EdgeEdgeDistanceType edge_edge_distance_type(
    const Eigen::MatrixBase<DerivedEA0>& ea0,
    const Eigen::MatrixBase<DerivedEA1>& ea1,
    const Eigen::MatrixBase<DerivedEB0>& eb0,
    const Eigen::MatrixBase<DerivedEB1>& eb1)
{
    assert(ea0.size() == 3);
    assert(ea1.size() == 3);
    assert(eb0.size() == 3);
    assert(eb1.size() == 3);

    auto u = ea1 - ea0;
    auto v = eb1 - eb0;
    auto w = ea0 - eb0;

    auto a = u.squaredNorm(); // always >= 0
    auto b = u.dot(v);
    auto c = v.squaredNorm(); // always >= 0
    auto d = u.dot(w);
    auto e = v.dot(w);
    auto D = a * c - b * b; // always >= 0
    auto tD = D;            // tc = tN / tD, default tD = D >= 0

    EdgeEdgeDistanceType defaultCase = EdgeEdgeDistanceType::EA_EB;

    // compute the line parameters of the two closest points
    auto sN = (b * e - c * d);
    decltype(sN) tN;
    if (sN <= 0.0) { // sc < 0 => the s=0 edge is visible
        tN = e;
        tD = c;
        defaultCase = EdgeEdgeDistanceType::EA0_EB;
    } else if (sN >= D) { // sc > 1  => the s=1 edge is visible
        tN = e + b;
        tD = c;
        defaultCase = EdgeEdgeDistanceType::EA1_EB;
    } else {
        tN = (a * e - b * d);
        if (tN > 0.0 && tN < tD
            && (cross(u, v).squaredNorm() < 1.0e-20 * a * c)) {
            // avoid nearly parallel EE
            if (sN < D / 2) {
                tN = e;
                tD = c;
                defaultCase = EdgeEdgeDistanceType::EA0_EB;
            } else {
                tN = e + b;
                tD = c;
                defaultCase = EdgeEdgeDistanceType::EA1_EB;
            }
        }
        // else defaultCase stays EdgeEdgeDistanceType::EA_EB
    }

    if (tN <= 0.0) { // tc < 0 => the t=0 edge is visible
        // recompute sc for this edge
        if (-d <= 0.0) {
            return EdgeEdgeDistanceType::EA0_EB0;
        } else if (-d >= a) {
            return EdgeEdgeDistanceType::EA1_EB0;
        } else {
            return EdgeEdgeDistanceType::EA_EB0;
        }
    } else if (tN >= tD) { // tc > 1  => the t=1 edge is visible
        // recompute sc for this edge
        if ((-d + b) <= 0.0) {
            return EdgeEdgeDistanceType::EA0_EB1;
        } else if ((-d + b) >= a) {
            return EdgeEdgeDistanceType::EA1_EB1;
        } else {
            return EdgeEdgeDistanceType::EA_EB1;
        }
    }

    return defaultCase;
}

} // namespace ipc


//把kitti的groundtru格式类型的pose(时间戳 4X3的转移矩阵[去掉了0 0 0 1]) 转化为 Tum数据集类型的数据( 'timestamp tx ty tz qx qy qz qw' )
//在转化的过程中经过一个对pose进行的一个变换transpose*pose*transposeInv 保存 , 详情见 int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose);

#include <iostream>
#include <iomanip> //要用到格式控制符
#include <fstream>
#include <boost/program_options.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <boost/format.hpp>  // for formating strings

#include <vector>
#include <string>

#include <sstream>
#include <boost/filesystem.hpp>
#include <algorithm>
#include <iterator>
#include <math.h>

using namespace std;

int loadKittiPoses(const string & file_name, vector<Eigen::Matrix4d> &poses) ;
int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose);
int loadKittiTimes( const string & file_name,  vector<double> & timestamps);
int convert(  vector<Eigen::Matrix4d> & poses,  vector<double> & timestamps,  const string & filename);

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

    if (argc != 4)
    {
        //example: ./project    kitti类型的pose.txt   kitti时间戳文件夹   要输出的文件名
        cerr << endl << "Usage: ./project    kittiPoseXX.txt     kittiTimeXX.txt    tumOutxx.txt " << endl;
        return 1;
    }

    string kittipath =  std::string(argv[1]);
    string kittTimepath =  std::string(argv[2]);
    string tumFileName = std::string(argv[3]);
    cout << kittipath << endl;
    cout << kittTimepath << endl;
    cout << tumFileName << endl;

    vector<Eigen::Matrix4d> poses;         // 相机位姿
    vector<double>  timestamps;// 时间戳

     Eigen::Matrix4d T_rectC2_to_rectC0;
     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0595,
             0.0000,    1.0000,    0.0000,    0.0015,
             -0.0000,   -0.0000,    1.0000,   -0.0038,
             0,         0,         0,    1.0000; //00-02

//     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0596,
//             0.0000,    1.0000,    0.0000,    0.0006,
//             -0.0000,   -0.0000,    1.0000,   -0.0027,
//             0,         0,         0,    1.0000; //03

//     T_rectC2_to_rectC0 << 1.0000, 0.0000,   -0.0000,   -0.0602,
//             0.0000,    1.0000,    0.0000,    0.0021,
//             -0.0000,   -0.0000,    1.0000,   -0.0062,
//             0,         0,         0,    1.0000;//04-10

    //load  poses
    if (loadKittiPoses(kittipath,  poses, T_rectC2_to_rectC0) == 0 )
    {
        cerr << "cannot find pose file" << endl;
        return 0;
    }
    //load  poses
    if (loadKittiTimes(kittTimepath,  timestamps) == 0 )
    {
        cerr << "cannot find time file" << endl;
        return 0;
    }

    if ( convert( poses, timestamps, tumFileName) == 0)
    {
        cerr << "convert failed!!!" << endl;
        return 0;
    }

    cout << "It is done!------------------------------------------------" << endl;

    return 1;
}

int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses)
{
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(T);
        }
    }
    fclose(fp);
    return 1;

}

//保存transpose*T*transposeInv到poses
int loadKittiPoses(const string &  file_name, vector<Eigen::Matrix4d> & poses, Eigen::Matrix4d &transpose)
{
 Eigen::Matrix4d transposeInv=transpose.inverse();
	cout<<"transpose: "<<transpose<<endl;
cout<<"transposeInv: "<<transposeInv<<endl;
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(transpose*T*transposeInv);
        }
    }
    fclose(fp);
    return 1;

}

int loadKittiTimes( const string & file_name,  vector<double> & timestamps)
{
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double timestamp;
        if (fscanf(fp, "%lf ", &timestamp ) == 1) {
            timestamps.push_back(timestamp);
        }
    }
    fclose(fp);
    return 1;

}

int convert(  vector<Eigen::Matrix4d> & poses,  vector<double> & timestamps,  const string & filename)
{
    cout << endl << "Saving camera pose to " << filename << " ..." << endl;
    ofstream fileout;
    fileout.open(filename.c_str(),std::ios::out);
    fileout << "#Format: timestamp tx ty tz qx qy qz qw" << endl ;

    vector<double>::iterator itTimestamp = timestamps.begin();
    if (poses.size() != timestamps.size())
    {
        cerr << "!!!-----------poses.size() != timestamps.size()----------!!!" << endl;
        //return 0;
    }

//NOTE 这里以pose的size()为准
    for (vector<Eigen::Matrix4d>::iterator it = poses.begin(); it != poses.end(); it++, itTimestamp++)
    {
        Eigen::Matrix4d tmpPose = *it;
        Eigen::Matrix3d tmpRotationMatrix = tmpPose.block(0, 0, 3, 3);
        Eigen::Quaterniond q = Eigen::Quaterniond ( tmpRotationMatrix ); //// 请注意四元数 的顺序是(x,y,z,w), w 为实部，前三者为虚部
        //cout<< q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;
        fileout << setprecision(6) << *itTimestamp << " " <<  setprecision(9) <<  tmpPose(0, 3) << " " << tmpPose(1, 3) << " " << tmpPose(2, 3) << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl;

    }

    fileout.close();
    cout << endl << "trajectory saved!" << endl;
    return 1;

}


/*
Compute probability & bayesian evidence that a Pfam or TIGR protein domain is near
their respective average tri-nucleotide or amino acid distribution.

Conversely, the same quantities an be computed for domains consistently far from the mean.
*/
#include <iostream>
#include <string>
#include <algorithm>
#include <boost/program_options.hpp>
#include "./task/domain_probability_task.cpp"
#include "./task/context.cpp"

using namespace std;
namespace po = boost::program_options;

int main(int ac, char* av[]) {
	try {
		po::options_description desc(
			"Compute probability & bayesian evidence that a Pfam or TIGR "
            "protein domain is near (or far) from their respective "
            "average tri-nucleotide or amino acid distribution"
		);
		desc.add_options()
            (
            	"help,h",
            	"Print help message"
            )
            (
                "kind,k",
                po::value<string>()->default_value(""),
                "One of \"tri-nucleotide\" or \"amino-acid\""
            )
            (
                "query,q",
                po::value<string>()->default_value(""),
                "Protein domain source = Pfam or TIGR (case insensitive)"
            )
            (
                "tail,d",
                po::value<string>()->default_value("left"),
                "Tail of the distribution of distances to tri-nucleotide mean on which to focus: "
                "left (closest to min distance 0) or right (closest to max distance 1)"
            )
            (
            	"n_threads,t",
            	po::value<int>()->default_value(4),
            	"Number of threads to use."
            )
            (
                "config_file,c",
                po::value<string>()->default_value(""),
                "Use JSON config file instead of command line parameters"
            )
        ;

        po::variables_map vm;
        po::store(po::parse_command_line(ac, av, desc), vm);
        po::notify(vm);

        if (vm.count("help")) {
            cerr << desc << endl;
            return 0;
        }

        string config_file_path = vm["config_file"].as<string>();
        if (!config_file_path.empty()) {
            cerr << "Using config file: " << config_file_path << endl;
            DomainProbabilityContext ctx = parse_domain_probability_context(config_file_path);
            compute_domain_probabilities(ctx);

        } else {
            string kind = vm["kind"].as<string>();
            if (kind == "tri-nucleotide" || kind == "amino-acid") {
                cerr << "Kind: " << kind << endl;
            } else if (kind.empty()) {
                cerr << "Error: parameter \"kind\" not set.";
                cerr << " See --help for usage." << endl;
                return 1;
            } else {
                cerr << "Error: unknown value for parameter \"kind\": \"" << kind;
                cerr << "\". See --help for usage." << endl;
                return 1;
            }

            auto query = vm["query"].as<string>();
            transform(query.begin(), query.end(), query.begin(), ::tolower);

            cerr << "Query: " << query << endl;
            if (query != "pfam" && query != "tigr") {
                cerr << "Error: --query must be one of pfam, tigr" << endl;
                cerr << desc << endl;
                return 1;
            }

            auto tail = vm["tail"].as<string>();
            transform(tail.begin(), tail.end(), tail.begin(), ::tolower);
            cerr << "Tail: " << tail << endl;
            if (tail != "left" && tail != "right") {
                cerr << "Error: --tail must be one of left, right" << endl;
                cerr << desc << endl;
                return 1;
            }

            const int n_threads = vm["n_threads"].as<int>();
            cerr << "Threads: " << n_threads << endl;

            DomainProbabilityContext ctx = get_context_from_parameters(
                kind,
                query,
                tail,
                n_threads
            );
            compute_domain_probabilities(ctx);
        }
	}
	catch (exception& e) {
		cerr << "Exception raised: " << e.what() << endl;
		return 1;
	}
	catch (...) {
		cerr << "Unknown exception raised" << endl;
		return 1;
	}
	return 0;
}


//
// Copyright (c) 2019-2020 Kris Jusiak (kris at jusiak dot net)
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <array>
#include <boost/ut.hpp>
#include <memory>
#include <string>
#include <string_view>
#include <vector>

constexpr auto sum = [](auto... args) { return (0 + ... + args); };

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

  "operators"_test = [] {
    expect(0_i == sum());
    expect(2_i != sum(1, 2));
    expect(sum(1) >= 0_i);
    expect(sum(1) <= 1_i);
  };

  "message"_test = [] { expect(3_i == sum(1, 2)) << "wrong sum"; };

  "expressions"_test = [] {
    expect(0_i == sum() and 42_i == sum(40, 2));
    expect(1_i == sum() or 0_i == sum());
    expect(1_i == sum() or (sum() != 0_i or sum(1) > 0_i)) << "compound";
  };

  "that"_test = [] {
    expect(that % 0 == sum());
    expect(that % 42 == sum(40, 2) and that % (1 + 2) == sum(1, 2));
    expect(that % 1 != 2 or 2_i > 3);
  };

  "eq/neq/gt/ge/lt/le"_test = [] {
    expect(eq(42, sum(40, 2)));
    expect(neq(1, 2));
    expect(eq(sum(1), 1) and neq(sum(1, 2), 2));
    expect(eq(1, 1) and that % 1 == 1 and 1_i == 1);
  };

  "floating points"_test = [] {
    expect(42.1_d == 42.101) << "epsilon=0.1";
    expect(42.10_d == 42.101) << "epsilon=0.01";
    expect(42.10000001 == 42.1_d) << "epsilon=0.1";
  };

  "strings"_test = [] {
    using namespace std::literals::string_view_literals;
    using namespace std::literals::string_literals;

    expect("str"s == "str"s);
    expect("str1"s != "str2"s);

    expect("str"sv == "str"sv);
    expect("str1"sv != "str2"sv);

    expect("str"sv == "str"s);
    expect("str1"sv != "str2"s);
    expect("str"s == "str"sv);
    expect("str1"s != "str2"sv);
  };

  "types"_test = [] {
    expect(type<int> == type<int>);
    expect(type<float> != type<double>);

    const auto i = 0;
    expect(type<const int> == type<decltype(i)>);
    expect(type<decltype(i)> != type<int>);
  };

  "containers"_test = [] {
    std::vector v1{1, 2, 3};
    std::vector v2{1, 2, 3};
    expect(v1 == v2);
    expect(std::vector{"a", "b"} != std::vector{"c"});
    expect(std::array{true, false} == std::array{true, false});
  };

  "constant"_test = [] {
    constexpr auto compile_time_v = 42;
    auto run_time_v = 99;
    expect(constant<42_i == compile_time_v> and run_time_v == 99_i);
  };

  "convertible"_test = [] {
    expect(bool(std::make_unique<int>()));
    expect(not bool(std::unique_ptr<int>{}));
  };

  "boolean"_test = [] {
    expect("true"_b);
    expect("true"_b or not "true"_b);
    expect(not "true"_b != "true"_b);
  };
}


// Copyright 2017, 2018 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hash2/fnv1a.hpp>
#include <boost/hash2/siphash.hpp>
#include <boost/hash2/xxhash.hpp>
#include <boost/hash2/spooky2.hpp>
#include <boost/hash2/murmur3.hpp>
#include <boost/hash2/md5.hpp>
#include <boost/hash2/sha1.hpp>
#include <boost/hash2/get_integral_result.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/cstdint.hpp>
#include <algorithm>
#include <cstddef>

using boost::hash2::get_integral_result;

template<class H> void test()
{
    H h;

    get_integral_result<int>( h.result() );
    get_integral_result<boost::uint32_t>( h.result() );
    get_integral_result<boost::uint64_t>( h.result() );
}

int main()
{
    test<boost::hash2::fnv1a_32>();
    test<boost::hash2::fnv1a_64>();
    test<boost::hash2::murmur3_32>();
    test<boost::hash2::murmur3_128>();
    test<boost::hash2::xxhash_32>();
    test<boost::hash2::xxhash_64>();
    test<boost::hash2::spooky2_128>();
    test<boost::hash2::siphash_32>();
    test<boost::hash2::siphash_64>();
    test<boost::hash2::md5_128>();
    test<boost::hash2::sha1_160>();
    test<boost::hash2::hmac_md5_128>();
    test<boost::hash2::hmac_sha1_160>();

    return boost::report_errors();
}