/*  LICENSE

 _This file is Copyright 2018 by the Image Processing and Analysis Group (BioImage Suite Team). Dept. of Radiology & Biomedical Imaging, Yale School of Medicine._

 BioImage Suite Web is licensed under the Apache License, Version 2.0 (the "License");

 - you may not use this software except in compliance with the License.
 - You may obtain a copy of the License at [http://www.apache.org/licenses/LICENSE-2.0](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.__

 ENDLICENSE */

#include "bisEigenUtil.h"
#include <iostream>
#include <Eigen/Dense>
#include <string.h>

namespace bisEigenUtil {

  Eigen::MatrixXf mapToEigenMatrix(bisSimpleMatrix<float>* m)
  {
    int rows=m->getNumRows();
    int cols=m->getNumCols();
    return Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(m->getData(),rows,cols);
  }

  Eigen::VectorXf mapToEigenVector(bisSimpleVector<float>* m)
  {
    int rows=m->getLength();
    return Eigen::VectorXf::Map(m->getData(),rows);
  }

  Eigen::MatrixXf mapImageToEigenMatrix(bisSimpleImage<float>* img)
  {
    int dim[5]; img->getDimensions(dim);
    int rows=dim[3]*dim[4];
    int cols=dim[0]*dim[1]*dim[2];
    return Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(img->getData(),rows,cols);
  }

  // -----------------------------------------------------------------------------------------------------
  // deserialize And Map
  // -----------------------------------------------------------------------------------------------------

  int deserializeAndMapToEigenVector(bisSimpleVector<float>* s_vector,unsigned char* ptr,Eigen::VectorXf& output,int defaultsize,float defaultvalue,int debug)
  {
    if (ptr!=0)
     {
       if (s_vector->linkIntoPointer(ptr))
	 {
	   if (debug)
	     std::cout << "Using external vector " << std::endl;

	   output=bisEigenUtil::mapToEigenVector(s_vector);
	   return 1;
	 }

       std::unique_ptr<bisSimpleMatrix<float> > s_matrix(new bisSimpleMatrix<float>("vectormatrix"));
       if (!s_matrix->linkIntoPointer(ptr))
	 {
	   std::cerr << "Failed to deserialize vector" << std::endl;
	   return 0;
	 }

       int rows=s_matrix->getNumRows();
       int cols=s_matrix->getNumCols();
       if (cols!=1)
	 {
	   std::cerr << "Failed to deserialize vector multi-col matrix provided." << std::endl;
	   return 0;
	 }

       std::cout << "wasm- Deserializing vector from single-column matrix." << std::endl;
       output=Eigen::VectorXf::Map(s_matrix->getData(),rows);
       return 1;
     }

    if (defaultsize>0)
      {
	output=Eigen::VectorXf::Zero(defaultsize);
	for (int ia=0;ia<defaultsize;ia++)
	  output(ia)=defaultvalue;
      }
    return 2;
  }

  int deserializeAndMapToEigenMatrix(bisSimpleMatrix<float>* s_matrix,unsigned char* ptr,Eigen::MatrixXf& output,int debug)
  {
    if (ptr==0) {
      std::cerr << "Failed to deserialize matrix" << std::endl;
      return 0;
    }

    if (s_matrix->linkIntoPointer(ptr))
      {
	if (debug)
	  std::cout << "Using external matrix " << std::endl;

	output=bisEigenUtil::mapToEigenMatrix(s_matrix);
	return 1;
      }

    std::unique_ptr<bisSimpleVector<float> > s_vector(new bisSimpleVector<float>("matrixvector"));
    if (!s_vector->linkIntoPointer(ptr))
      {
	std::cerr << "Failed to deserialize matrix as vector" << std::endl;
	return 0;
      }

    int rows=s_vector->getLength();
    std::cout << "wasm- Deserializing matrix from vector." << std::endl;
    output=Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(s_vector->getData(),rows,1);
    return 1;
  }

  unsigned char* serializeAndReturn(Eigen::MatrixXf& mat,std::string name)
  {
    std::unique_ptr<bisSimpleMatrix<float> > out(bisEigenUtil::createSimpleMatrix(mat,name));
    out->releaseOwnership();
    return out->getRawArray();
  }

  // -----------------------------------------------------------------------------------------------------
  std::unique_ptr<bisSimpleMatrix< float> > createSimpleMatrix(Eigen::MatrixXf inp,std::string name)
  {
    std::unique_ptr<bisSimpleMatrix<float> > out (new bisSimpleMatrix<float>(name));
    out->allocate(inp.rows(),inp.cols());
    float* odata=out->getData();
    int index=0;

    for (int row=0;row<inp.rows();row++)
      {
	for (int col=0;col<inp.cols();col++)
	  {
	    odata[index]=inp(row,col);
	    ++index;
	  }
      }

    return std::move(out);
  }

  std::unique_ptr<bisSimpleVector< float> > createSimpleVector(Eigen::VectorXf inp,std::string name)
  {

    std::unique_ptr<bisSimpleVector<float> > out (new bisSimpleVector<float>(name));

    out->allocate(inp.rows());
    float* odata=out->getData();

    for (int row=0;row<inp.rows();row++)
      odata[row]=inp(row);

    return std::move(out);
  }

  // ---------------------------------------------------------
  // Converted from BioImageSuite::vtkpxMatrix
  Eigen::MatrixXf importFromMatlabV6(const unsigned char* bytepointer,int bytearraylength,std::string matrixname,int debug,int& out_ok)
  {
    out_ok=0;
    static unsigned char* indataptr=0;
    static int remaining=0;

    long pt=(long)bytepointer;
    indataptr=(unsigned char*)pt;
    remaining=bytearraylength;
    Eigen::MatrixXf outputMatrix=Eigen::MatrixXf::Zero(1,1);

    if (debug>1)
      std::cout << "pointer=" << (long)indataptr << " numbytes=" << bytearraylength << "\t " << remaining << std::endl;

    struct internal
    {
      static int readdata(void* outbuf,int sizeofelement,int numelements)
      {
	int sz=numelements*sizeofelement;
	if (sz<=remaining)
	  {
	    memcpy(outbuf,indataptr,sz);
	    remaining=remaining-sz;
	    indataptr+=sz;
	    //	    std::cout << "read " << sz << " elements pointer=" << (int)indataptr << "\t remaining=" << remaining << std::endl;
	    return sz;
	  }
	return 0;
      }

      static int skipahead(int sz)
      {
	if (sz<remaining)
	  {
	    indataptr+=sz;
	    remaining=remaining-sz;
	    //	    std::cout << "skipping ahead " << sz << " elements pointer=" << (int)indataptr << "\t remaining=" << remaining << std::endl;
	    return sz;
	  }
	return 0;
      }

      static int swapint(int input) {
	int a=input;
	unsigned char* bytes=(unsigned char*)&a;
	unsigned char tmp;

	tmp=*bytes;
	*bytes=*(bytes+3);
	*(bytes+3)=tmp;

	tmp=*(bytes+1);
	*(bytes+1)=*(bytes+2);
	*(bytes+2)=tmp;

	return a;
      }

      static float swapfloat(float input) {
	float a=input;
	unsigned char* bytes=(unsigned char*)&a;
	unsigned char tmp;

	tmp=*bytes;
	*bytes=*(bytes+3);
	*(bytes+3)=tmp;

	tmp=*(bytes+1);
	*(bytes+1)=*(bytes+2);
	*(bytes+2)=tmp;

	return a;
      }

      static short swapshort(short input) {
	short a=input;
	unsigned char *bytes=(unsigned char*)&a;
	unsigned char tmp;
	tmp=*bytes;
	*bytes=*(bytes+1);
	*(bytes+1)=tmp;
	return a;
      }

      static double swapdouble(double input) {
	double a=input;
	double b=input;
	unsigned char* ap=(unsigned char*)&a;
	unsigned char* bp=(unsigned char*)&b;

	ap[0]=bp[7];
	ap[1]=bp[6];
	ap[2]=bp[5];
	ap[3]=bp[4];
	ap[4]=bp[3];
	ap[5]=bp[2];
	ap[6]=bp[1];
	ap[7]=bp[0];

	return a;
      }

    };

    char buffer[2000];
    char name[2000];

    internal::readdata(buffer,1,116);
    if (debug)
      std::cout << "Description = " << buffer << std::endl;

    internal::readdata(buffer,1,8);
    int ok=1;
    for (int ia=0;ia<=7;ia++)
      {
	int v=int(buffer[ia]);
	if (!(v==0 || v==32))
	  {
	    std::cerr << "We have a problem with ia=" << ia << std::endl;
	    ok=0;
	    ia=9;
	  }
      }

    if (ok==0)
      {
	return outputMatrix;
      }
    internal::readdata(buffer,1,4);
    if (debug>1)
      std::cout << "All Offset bytes are either zero or space OK!" << std::endl;

    int done=0;
    int totalread=128;
    int count=0;

    while (done==0)
      {
	int dtype[2];
	int n=internal::readdata(dtype,sizeof(int),2);
	totalread+=8;
	if (debug>1)
	  std::cout << "n = " << n << " totalread= " << totalread << std::endl << "-----------------------------------------------------" << std::endl;
	if (n<1)
	  {
	    done=1;
	  }
	else
	  {
	    int swap=0;
	    ++count;
	    if (dtype[0]<0 || dtype[0]>1024)
	      {
		dtype[0]=internal::swapint(dtype[0]);
		dtype[1]=internal::swapint(dtype[1]);
		swap=1;
	      }
	    if (debug>1)
	      std::cout << std::endl << "dtype = " << dtype[0] << " , numbytes = " << dtype[1] << std::endl;
	    if (dtype[0]!=14)
	      {
		if (debug>1)
		  {
		    std::cout << "Not a matrix skipping ahead " << dtype[1] << " bytes" << std::endl;
		    std::cout << "Seaking ahead  " << dtype[1] << " bytes" << std::endl;
		  }
		int toread=dtype[1];

		// Add padding
		int tmp=8*int(toread/8);
		if (tmp<toread)
		  {
		    toread=8+tmp;
		    //		  std::cout  << "Adding padding from " << dtype[1] << " to " << toread << std::endl;
		  }

		while (toread>0)
		  {
		    int n=toread;
		    if (toread>256) n=256;
		    totalread+=internal::readdata(buffer,1,n);
		    toread-=n;
		  }
		if (debug>1)
		  std::cout << "Total read = " << totalread << std::endl;
	      }
	    else
	      {
		if (debug)
		  std::cout << std::endl << "Beginning to read matrix: " << std::endl;
		int bytes_read=internal::readdata(buffer,1,16);
		int cl=(int)buffer[8];
		//	      if (swap)
		//		cl=internal::swapint(cl);

		int flags[6],length[1];
		bytes_read+=internal::readdata(flags,sizeof(int),5);
		if (swap)
		  {
		    //		    length[0]=internal::swapint(length[0]);
		    for (int ic=0;ic<5;ic++)
		      flags[ic]=internal::swapint(flags[ic]);
		  }

		// Check for use of small element format
		//
		int test=int(flags[4] / 65536 );
		if (debug>1)
		  std::cout << "***************** Is Small Element " << test << std::endl;
		if (test==0)
		  {
		    length[0]=0;
		    bytes_read+=internal::readdata(length,sizeof(int),1);
		    if (swap)
		      length[0]=internal::swapint(length[0]);
		    flags[5]=length[0];
		    if (swap)
		      flags[5]=internal::swapint(flags[5]);
		    bytes_read+=internal::readdata(name,1,flags[5]);
		    name[flags[5]]=(char)0;
		    int rem=8-(flags[5]-int(flags[5]/8)*8);
		    if (debug>1)
		      std::cout << "Remainder " << rem << std::endl;
		    bytes_read+=internal::readdata(buffer,1,rem);
		  }
		else
		  {
		    const int tmp_mask=65535;
		    int nb=(flags[4] & tmp_mask);
		    if (debug>1)
		      std::cout << "Small Element nb=" << nb << " test= " << int(test/65536) << std::endl;
		    bytes_read+=internal::readdata(name,1,4);
		    name[nb]=(char)0;
		    flags[5]=0;
		  }

		int nfl[2];
		bytes_read+=internal::readdata(nfl,sizeof(int),2);
		if (swap)
		  {
		    nfl[0]=internal::swapint(nfl[0]);
		    nfl[1]=internal::swapint(nfl[1]);
		  }
		if (debug>1)
		  std::cout << "Final Flags = " << nfl[0] << "," << nfl[1] << std::endl;

		int numrows=flags[2];
		int numcols=flags[3];
		if (debug)
		  {
		    std::cout << "Dimensions =" << numrows << "x" << numcols << ", class = " << cl << ", length = " << flags[5] << std::endl;
		    std::cout << "Name = " << name << ", (bytes read= " << bytes_read << ")" << std::endl;
		  }
		int toread=dtype[1]-bytes_read;
		if (debug>1)
		  std::cout << "To read (1) = " << toread << std::endl;

		if (strcmp(name, matrixname.c_str())==0 || strlen(matrixname.c_str()) == 0)
		  {
		    if ( (cl==7 || cl==6))
		      {
			int numbytesneeded=4*numrows*numcols;
			if (cl==6)
			  numbytesneeded*=2;

			if (debug)
			  std::cout << "Numbytes needed = " << numbytesneeded << std::endl;

			if (debug>1)
			  std::cout << "Beginning to read matrix " << numbytesneeded << " < " <<  toread << std::endl;

			if (numbytesneeded<=toread)
			  {
			    outputMatrix=Eigen::MatrixXf::Zero(numrows,numcols);
			    if (cl==6)
			      {
				std::unique_ptr<double> rd(new double[numrows]);
				for (int ib=0;ib<numcols;ib++)
				  {
				    bytes_read+=internal::readdata(rd.get(),sizeof(double),numrows);
				    for (int ia=0;ia<numrows;ia++)
				      if (swap)
					outputMatrix(ia,ib)=float(internal::swapdouble(rd.get()[ia]));
				      else
					outputMatrix(ia,ib)=float(rd.get()[ia]);
				  }
			      }
			    else
			      {
				std::unique_ptr<float> rf(new float[numrows]);
				for (int ib=0;ib<numcols;ib++)
				  {
				    bytes_read+=internal::readdata(rf.get(),sizeof(float),numrows);
				    for (int ia=0;ia<numrows;ia++)
				      if (swap)
					outputMatrix(ia,ib)=(internal::swapfloat(rf.get()[ia]));
				      else
					outputMatrix(ia,ib)=(rf.get()[ia]);
				  }
			      }

			    out_ok=1;
			    return outputMatrix;
			  }
			else
			  {
			    std::cerr << "\t\t\t can't read data not enough bytes" << std::endl;
			  }
		      }
		    std::cout << "Final To read (2) = " << toread << std::endl;
		  }
		else
		  {
		    if (debug>1)
		      std::cout << "Not the matrix we are looking for" << std::endl;
		  }

		while (toread>0)
		  {
		    int n=toread;
		    if (toread>256) n=256;
		    bytes_read+=internal::readdata(buffer,1,n);
		    toread-=n;
		  }

		totalread+=bytes_read;
	      }
	  }
      }

    return outputMatrix;

  }

  // ---------------------------------------------------------------------------------------------
  // Eigen Utilities
  // ---------------------------------------------------------------------------------------------
  void getMatrixDimensions(Eigen::MatrixXf& mat,int sz[2]) {
    sz[0]=mat.rows();
    sz[1]=mat.cols();
  }

  void resizeZeroVector(Eigen::VectorXf& vct,int numrows) {

    if (numrows!=vct.rows())
      {
	vct=Eigen::VectorXf::Zero(numrows);
	return;
      }

    for (int i=0;i<numrows;i++)
      vct(i)=0.0;
  }

  void resizeZeroMatrix(Eigen::MatrixXf& mat,int sz[2]) {

    int dim[2]; getMatrixDimensions(mat,dim);

    if (dim[0]!=sz[0] || dim[1]!=sz[1])
      {
	mat=Eigen::MatrixXf::Zero(sz[0],sz[1]);
	return;
      }

    for (int i=0;i<sz[0];i++)
      for (int j=0;j<sz[1];j++)
	mat(i,j)=0.0;
  }

  Eigen::MatrixXf createLSQMatrix(Eigen::MatrixXf& A) {
    Eigen::MatrixXf At=A.transpose();
    return ((At*A).inverse())*At;
  }

  int inPlaceMultiplyMV(Eigen::MatrixXf& A, Eigen::VectorXf& x, Eigen::VectorXf& b)
  {
    int dim[2]; getMatrixDimensions(A,dim);
    // R * C  * C * 1 = R *1

    //    std::cout << "Dim=" << dim[0] << "," << dim[1] << " x=" << x.rows() << " b=" <<  b.rows() << " " << std::endl;

    if (dim[1]!=x.rows() || dim[0]!=b.rows())
      return 0;

    for (int i=0;i<dim[0];i++)
      {
	float sum=0.0;
	for (int j=0;j<dim[1];j++)
	  sum+=A(i,j)*x[j];
	b(i)=sum;
      }

    //    std::cout << "b=" << b << std::endl;

    return 1;
  }

  // -----------------------------------------------------------------------------------
  int inPlaceMultiply(Eigen::MatrixXf& A, Eigen::MatrixXf& B, Eigen::MatrixXf& C)
  {
    int s1[2],s2[2],s3[2];
    getMatrixDimensions(A,s1);
    getMatrixDimensions(B,s2);

    if (s1[1]!=s2[0])
      {
	std::cerr << "Cannot multiply matrices bad size! a=" << s1[0] << "x" << s1[1] <<", b=" << s2[0] << "x" << s2[1] << std::endl;
	return 0;
      }

    s3[0]=s1[0];
    s3[1]=s2[1];

    resizeZeroMatrix(C,s3);

    for(int col = 0; col < s3[1]; col++)
      {
	for(int row = 0; row < s3[0]; row++)
	  {
	    float sum=0.0;
	    for(int i = 0; i < s1[1]; i++)
	      sum+=A(row,i)*B(i,col);
	    C(row,col)=sum;
	  }
      }
    return 1;
  }

  int inPlaceMultiply3(Eigen::MatrixXf& a,Eigen::MatrixXf& b,Eigen::MatrixXf& c,Eigen::MatrixXf& result)
  {
    int s1[2],s2[2],s3[2];
    getMatrixDimensions(a,s1);
    getMatrixDimensions(b,s2);
    getMatrixDimensions(c,s3);

    if (s1[1]!=s2[0] || s2[1]!=s3[0])
      {
	std::cerr << "Cannot multiply3 matrices bad sizes " << s1[0]<<"*" << s1[1] <<", " << s2[0] << "*" << s2[1] << ", " << s3[0] << "*" << s3[1] << std::endl;
	return 0;
      }

    int s4[2] = { s1[0],s3[1] };
    resizeZeroMatrix(result,s4);

    for(int col = 0; col < s4[1]; col++)
      {
	for(int row = 0; row < s4[0]; row++)
	  {
	    float sum=0.0;
	    for(int k = 0; k < s1[1] ; k++)
	      for (int l=0; l <  s2[1] ; l++ )
		sum+=a(row,k)*b(k,l)*c(l,col);

	    result(row,col)=sum;
	  }
      }
    return 1;
  }

}


#ifndef GUNEROMEMBERSHIPCIRCUIT_H_
#define GUNEROMEMBERSHIPCIRCUIT_H_

#include <deque>
#include <mutex>
#include <boost/optional.hpp>
#include <boost/static_assert.hpp>
#include <libff/common/utils.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/common/default_types/r1cs_gg_ppzksnark_pp.hpp>
#include <libsnark/relations/constraint_satisfaction_problems/r1cs/examples/r1cs_examples.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_gg_ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/uscs_ppzksnark/uscs_ppzksnark.hpp>
#include <libff/algebra/fields/field_utils.hpp>
#include <libff/algebra/scalar_multiplication/multiexp.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <libff/algebra/curves/edwards/edwards_pp.hpp>
#include <libff/algebra/curves/mnt/mnt4/mnt4_pp.hpp>
#include <libff/algebra/curves/mnt/mnt6/mnt6_pp.hpp>
#include <libff/common/utils.hpp>
#include <libff/algebra/curves/alt_bn128/alt_bn128_pp.hpp>

#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_read_gadget.hpp>
#include <libsnark/gadgetlib1/gadgets/merkle_tree/merkle_tree_check_update_gadget.hpp>

#include "gunero_merkle_tree.hpp"
#include "guneromembership_gadget.hpp"
#include "GuneroProof.hpp"

using namespace libsnark;

namespace gunero {

class GuneroMembershipWitness{
public:
    uint256 W;
    uint8_t N_account;
    uint256 V_account;

    GuneroMembershipWitness() {}
    GuneroMembershipWitness(
        const uint256& pW,
        const uint8_t& pN_account,
        const uint256& pV_account
    ) : W(pW),
        N_account(pN_account),
        V_account(pV_account)
    {
    }
    ~GuneroMembershipWitness() {}

    ADD_SERIALIZE_METHODS;

    template <typename Stream, typename Operation>
    inline void SerializationOp(Stream& s, Operation ser_action, int nType, int nVersion) {
        READWRITE(W);
        READWRITE(N_account);
        READWRITE(V_account);
    }

    friend std::ostream& operator<<(std::ostream &out, const GuneroMembershipWitness &witness)
    {
        ::Serialize(out, witness, 1, 1);

        return out;
    }

    friend std::istream& operator>>(std::istream &in, GuneroMembershipWitness &witness)
    {
        ::Unserialize(in, witness, 1, 1);

        return in;
    }
};

///// MEMBERSHIP PROOF /////
// Public Parameters:
// Authorization Root Hash (W)
// Account Status (N_account)
// Account View Hash (V_account)

// Private Parameters:
// Account Secret Key (s_account)
// alt: Account (A_account)
// Authorization Merkle Path (M_account[160])
// Account View Randomizer (r_account)

//1) Obtain A_account from s_account through EDCSA (secp256k1) operations
//1 alt) Obtain P_proof from s_account through PRF operations
//2) Validate W == calc_root(A_account, N_account, M_account[160]) (User is authorized)
//2 alt) Validate W == calc_root(A_account, keccak256(P_proof,N_account), M_account[160]) (User is authorized)
//3) Validate V_account == keccak256(A_account, keccak256(W,r_account) (View Hash is consistent)
//3) alt) Validate V_account == keccak256(P_proof, keccak256(W,r_account) (View Hash is consistent)
template<typename FieldT, typename BaseT, typename HashT, size_t tree_depth>
class GuneroMembershipCircuit
{
public:
    GuneroMembershipCircuit()
    {}
    ~GuneroMembershipCircuit() {}

    void generate(
        const std::string& r1csPath,
        const std::string& pkPath,
        const std::string& vkPath
    ) {
        protoboard<FieldT> pb;
        guneromembership_gadget<FieldT, BaseT, HashT, tree_depth> gunero(pb);

        gunero.generate_r1cs_constraints(r1csPath, pkPath, vkPath);
    }

    static void makeTestVariables(
        const uint252& s_account,
        const uint8_t& N_account,
        const uint256& r_account,
        libff::bit_vector& P_proof,
        libff::bit_vector& leaf,
        std::vector<gunero_merkle_authentication_node>& M_account,
        libff::bit_vector& A_account_padded,
        libff::bit_vector& W,
        libff::bit_vector& view_hash_1,
        libff::bit_vector& V_account
    )
    {
        /* prepare test variables */
#ifdef DEBUG
        libff::print_header("Gunero prepare test variables");
#endif
        M_account = std::vector<gunero_merkle_authentication_node>(tree_depth);

        libff::bit_vector s_account_256(uint252_to_bool_vector_256(s_account));
        assert(s_account_256.size() == HashT::get_digest_len());

        libff::bit_vector N_account_lsb(uint256_to_bool_vector(uint8_to_uint256(N_account)));
        assert(N_account_lsb.size() == HashT::get_digest_len());

        {//P_proof = Hash(0000b | (s_account&252b), 0)
            libff::bit_vector block(HashT::get_digest_len());
            block.insert(block.begin(), s_account_256.begin(), s_account_256.end());
            assert(block.at(0) == false);
            assert(block.at(1) == false);
            assert(block.at(2) == false);
            assert(block.at(3) == false);

            P_proof = HashT::get_hash(block);

            block = P_proof;
            block.insert(block.end(), N_account_lsb.begin(), N_account_lsb.end());
            leaf = HashT::get_hash(block);//hash(P_proof,N_account)
        }

        // libff::bit_vector prev_hash(HashT::get_digest_len());
        // std::generate(prev_hash.begin(), prev_hash.end(), [&]() { return std::rand() % 2; });
        // leaf = prev_hash;
        assert(leaf.size() == HashT::get_digest_len());
        libff::bit_vector prev_hash = leaf;

        // libff::bit_vector address_bits;
        libff::bit_vector A_account(tree_depth);

        size_t address = 0;
        for (long level = tree_depth-1; level >= 0; --level)
        {
            //Generate random uncle position
            const bool computed_is_right = (std::rand() % 2);
            address |= (computed_is_right ? 1ul << (tree_depth-1-level) : 0);
            // address_bits.push_back(computed_is_right);
            A_account.at(level) = computed_is_right;

            //Generate random uncle
            libff::bit_vector uncle(HashT::get_digest_len());
            std::generate(uncle.begin(), uncle.end(), [&]() { return std::rand() % 2; });

            //Create block of prev_hash + uncle
            libff::bit_vector block = prev_hash;
            block.insert(computed_is_right ? block.begin() : block.end(), uncle.begin(), uncle.end());
            //Compress block to new hash
            libff::bit_vector h = HashT::get_hash(block);

            //Add uncle to path
            M_account[level] = uncle;

            prev_hash = h;
        }

        W = prev_hash;

        A_account_padded = libff::bit_vector(HashT::get_digest_len() - A_account.size());
        A_account_padded.insert(A_account_padded.begin(), A_account.begin(), A_account.end());

        assert(A_account_padded.size() == HashT::get_digest_len());

        libff::bit_vector r_account_lsb(uint256_to_bool_vector(r_account));
        assert(r_account_lsb.size() == HashT::get_digest_len());

        {//view_hash_1 = hash(W, r_account_lsb)
            libff::bit_vector block = W;
            block.insert(block.end(), r_account_lsb.begin(), r_account_lsb.end());
            view_hash_1 = HashT::get_hash(block);//hash(W, r_account_lsb)

            //V_account = hash(P_proof, hash(W, r_account_lsb))
            block = P_proof;
            block.insert(block.end(), view_hash_1.begin(), view_hash_1.end());
            V_account = HashT::get_hash(block);//hash(P_proof, view_hash_1)
        }

#ifdef DEBUG
        printf("\n"); libff::print_indent(); libff::print_mem("after prepare test variables"); libff::print_time("after prepare test variables");
#endif
    }

    static void calculateMerkleRoot(
        const libff::bit_vector& A_account,
        const libff::bit_vector& leaf,
        const std::vector<gunero_merkle_authentication_node>& M_account,
        libff::bit_vector& W
    )
    {
        const bool UseNewFormat = false;

#ifdef DEBUG
        libff::print_header("Gunero calculateMerkleRoot()");
#endif
        assert(M_account.size() == tree_depth);

        // libff::bit_vector prev_hash(HashT::get_digest_len());
        // std::generate(prev_hash.begin(), prev_hash.end(), [&]() { return std::rand() % 2; });
        // leaf = prev_hash;
        assert(leaf.size() == HashT::get_digest_len());
        libff::bit_vector prev_hash = leaf;

        // libff::bit_vector address_bits;
        assert(A_account.size() == tree_depth);

        // size_t address = 0;
        for (long level = tree_depth-1; level >= 0; --level)
        {
            //A_account is accessed LSB but is processed as MSB (root) to LSB (leaf)
            bool parameter_is_left;
            if (UseNewFormat)
            {
                parameter_is_left = A_account.at(tree_depth-1-level);
            }
            else
            {
                parameter_is_left = !A_account.at(level);
            }

            // //Generate random uncle
            // libff::bit_vector uncle(HashT::get_digest_len());
            // std::generate(uncle.begin(), uncle.end(), [&]() { return std::rand() % 2; });
            libff::bit_vector uncle = M_account[level];

            //Create block of prev_hash + uncle
            libff::bit_vector block = prev_hash;
            block.insert((!parameter_is_left) ? block.begin() : block.end(), uncle.begin(), uncle.end());
            //Compress block to new hash
            libff::bit_vector h = HashT::get_hash(block);

            // //Add uncle to path
            // M_account[level] = uncle;

            prev_hash = h;
        }

        W = prev_hash;

#ifdef DEBUG
        printf("\n"); libff::print_indent(); libff::print_mem("after calculateMerkleRoot()"); libff::print_time("after calculateMerkleRoot()");
#endif
    }

    bool prove(
        const uint256& pW,
        const uint8_t& pN_account,
        const uint256& pV_account,
        const uint252& ps_account,
        const std::vector<gunero_merkle_authentication_node>& pM_account,
        const uint160& pA_account,
        const uint256& pr_account,
        const r1cs_ppzksnark_proving_key<BaseT>& pk,
        const r1cs_ppzksnark_verification_key<BaseT>& vk,
        GuneroProof& proof
    )
    {
#ifdef DEBUG
        libff::print_header("Gunero witness (proof)");
#endif

        {
            r1cs_primary_input<FieldT> primary_input;
            r1cs_auxiliary_input<FieldT> aux_input;
            {
                protoboard<FieldT> pb;
                {
#ifdef DEBUG
                    libff::print_header("Gunero guneromembership_gadget.load_r1cs_constraints()");
#endif

                    guneromembership_gadget<FieldT, BaseT, HashT, tree_depth> gunero(pb);

                    gunero.generate_r1cs_witness(
                        pW,
                        pN_account,
                        pV_account,
                        ps_account,
                        pM_account,
                        pA_account,
                        pr_account
                    );

#ifdef DEBUG
                    printf("\n"); libff::print_indent(); libff::print_mem("after guneromembership_gadget.load_r1cs_constraints()"); libff::print_time("after guneromembership_gadget.load_r1cs_constraints()");
#endif
                }

                // The constraint system must be satisfied or there is an unimplemented
                // or incorrect sanity check above. Or the constraint system is broken!
                assert(pb.is_satisfied());

                // TODO: These are copies, which is not strictly necessary.
                primary_input = pb.primary_input();
                aux_input = pb.auxiliary_input();

                // Swap A and B if it's beneficial (less arithmetic in G2)
                // In our circuit, we already know that it's beneficial
                // to swap, but it takes so little time to perform this
                // estimate that it doesn't matter if we check every time.
                // pb.constraint_system.swap_AB_if_beneficial();

                //Test witness_map()
                {
                    r1cs_primary_input<FieldT> primary_input_test = guneromembership_gadget<FieldT, BaseT, HashT, tree_depth>::witness_map(
                        pW,
                        pN_account,
                        pV_account
                    );
                    assert(primary_input == primary_input_test);
                }
            }

            r1cs_ppzksnark_proof<BaseT> r1cs_proof = r1cs_ppzksnark_prover<BaseT>(
                pk,
                primary_input,
                aux_input
            );

            proof = GuneroProof(r1cs_proof);

#ifdef DEBUG
            printf("\n"); libff::print_indent(); libff::print_mem("after witness (proof)"); libff::print_time("after witness (proof)");
#endif
        }

        //Verify
        {
            r1cs_primary_input<FieldT> primary_input = guneromembership_gadget<FieldT, BaseT, HashT, tree_depth>::witness_map(
                pW,
                pN_account,
                pV_account
            );

            return r1cs_ppzksnark_verifier_strong_IC<BaseT>(vk, primary_input, proof.to_libsnark_proof<r1cs_ppzksnark_proof<BaseT>>());
        }
    }

    bool verify(
        const uint256& W,
        const uint8_t& N_account,
        const uint256& V_account,
        const GuneroProof& proof,
        const r1cs_ppzksnark_verification_key<BaseT>& vk,
        const r1cs_ppzksnark_processed_verification_key<BaseT>& vk_precomp
        )
    {
        try
        {
            r1cs_primary_input<FieldT> primary_input = guneromembership_gadget<FieldT, BaseT, HashT, tree_depth>::witness_map(
                W,
                N_account,
                V_account
            );

            r1cs_ppzksnark_proof<BaseT> r1cs_proof = proof.to_libsnark_proof<r1cs_ppzksnark_proof<BaseT>>();

            ProofVerifier<BaseT> verifierEnabled = ProofVerifier<BaseT>::Strict();

            bool verified = verifierEnabled.check(
                vk,
                vk_precomp,
                primary_input,
                r1cs_proof
            );

#ifdef DEBUG
            printf("\n"); libff::print_indent(); libff::print_mem("after verify"); libff::print_time("after verify");
#endif

            if (verified)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        catch (...)
        {
            return false;
        }
    }
};

} // end namespace `gunero`

#endif /* GUNEROMEMBERSHIPCIRCUIT_H_ */


#include "ICP.h"

#include <assert.h>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>

namespace ICP {
	ANNkd_tree* create_kd_tree(
		const Eigen::MatrixXd &_points,
		ANNpointArray &_ann_points)
	{
		assert(_points.rows() == 3);
		assert(_points.cols() > 0);

		int dimension = 3;
		int num_points = static_cast<int>(_points.cols());
		ANNkd_tree* ann_kd_tree;

		_ann_points = annAllocPts(num_points, dimension);	// allocate data points

		// read data points
		for (int point_index = 0; point_index < num_points; point_index++)
		{
			for (int i = 0; i < dimension; i++)
				_ann_points[point_index][i] = _points.col(point_index)[i];
		}

		ann_kd_tree = new ANNkd_tree(		// build search structure
			_ann_points,					// the data points
			num_points,						// number of points
			dimension);						// dimension of space

		return ann_kd_tree;
	}

	template<typename T>
	void get_closest_points(
		ANNkd_tree *_data_ann_kd_tree,
		const Eigen::MatrixXd &_query_points,
		const Eigen::MatrixBase<T> &_data_values,
		Eigen::MatrixBase<T> &_closest_data_values)
	{
		assert(_data_ann_kd_tree->nPoints() > 0);
		assert(_query_points.rows() == 3);
		assert(_query_points.cols() > 0);
		assert(_data_values.rows() > 0);
		assert(_data_values.cols() == _data_ann_kd_tree->nPoints());

		int num_queries = static_cast<int>(_query_points.cols());
		int num_data = static_cast<int>(_data_values.cols());
		int dimension = static_cast<int>(_data_values.rows());
		_closest_data_values = Eigen::MatrixBase<T>::Zero(dimension, num_queries);

		ANNpoint q = annAllocPt(3);
		ANNidxArray nn_idx = new ANNidx[1];
		ANNdistArray dd = new ANNdist[1];

		for (int point_index = 0; point_index < num_queries; point_index++)
		{
			for (unsigned int i = 0; i < 3; ++i)
				q[i] = _query_points.col(point_index)(i);

			_data_ann_kd_tree->annkSearch(q, 1, nn_idx, dd);
			int closest_Y_point_index = nn_idx[0];
			assert(closest_Y_point_index < num_data);
			assert(point_index < _closest_data_values.cols());

			_closest_data_values.col(point_index) = _data_values.col(closest_Y_point_index);
		}

		// Deallocate ANN.
		annDeallocPt(q);
		delete[] nn_idx;
		delete[] dd;
	}

	void get_closest_points(
		ANNkd_tree *_data_ann_kd_tree,
		const Eigen::MatrixXd &_query_points,
		Eigen::VectorXd &_distances)
	{
		assert(_data_ann_kd_tree->nPoints() > 0);
		assert(_query_points.rows() == 3);
		assert(_query_points.cols() > 0);

		int num_queries = static_cast<int>(_query_points.cols());
		_distances = Eigen::VectorXd::Zero(num_queries);

		ANNpoint q = annAllocPt(3);
		ANNidxArray nn_idx = new ANNidx[1];
		ANNdistArray dd = new ANNdist[1];

		for (int point_index = 0; point_index < num_queries; point_index++)
		{
			for (unsigned int i = 0; i < 3; ++i)
				q[i] = _query_points.col(point_index)(i);

			_data_ann_kd_tree->annkSearch(q, 1, nn_idx, dd);
			assert(point_index < _distances.rows());
			_distances[point_index] = std::sqrt(dd[0]);
		}

		// Deallocate ANN.
		annDeallocPt(q);
		delete[] nn_idx;
		delete[] dd;
	}

	double compute_rigid_transformation(const Eigen::MatrixXd &_X, const Eigen::MatrixXd &_Y,
		Eigen::Matrix3d &_rotation_mat, Eigen::Vector3d &_translation_vec,
		const double *_distance_threshold)
	{
		// Return: error (Minus error means that the computation is failed.)

		assert(_X.rows() == 3);
		assert(_Y.rows() == 3);

		if (_X.cols() == 0 || _Y.cols() == 0)
		{
			std::cerr << "Warning: No point exists." << std::endl;
			return -1;
		}

		Eigen::MatrixXd::Index X_num_points = _X.cols();
		Eigen::MatrixXd::Index Y_num_points = _Y.cols();

		ANNpointArray X_ann_points;
		ANNkd_tree* X_ann_kd_tree = create_kd_tree(_X, X_ann_points);

		ANNpointArray Y_ann_points;
		ANNkd_tree* Y_ann_kd_tree = create_kd_tree(_Y, Y_ann_points);

		Eigen::MatrixXd closest_X;
		Eigen::MatrixXd closest_Y;

		get_closest_points(X_ann_kd_tree, _Y, _X, closest_X);
		get_closest_points(Y_ann_kd_tree, _X, _Y, closest_Y);

		annDeallocPts(X_ann_points); delete X_ann_kd_tree;
		annDeallocPts(Y_ann_points); delete Y_ann_kd_tree;

		assert(closest_X.cols() == Y_num_points);
		assert(closest_Y.cols() == X_num_points);

		Eigen::MatrixXd::Index n = X_num_points + Y_num_points;
		Eigen::MatrixXd all_X(3, n);
		Eigen::MatrixXd all_Y(3, n);

		all_X << _X, closest_X;
		all_Y << closest_Y, _Y;

		assert(all_X.cols() == n);
		assert(all_Y.cols() == n);

		Eigen::MatrixXd diff = all_X - all_Y;
		Eigen::RowVectorXd squared_dists = (diff.array() * diff.array()).colwise().sum();

		// Hausdorff distance.
		double prev_error = std::sqrt(squared_dists.maxCoeff());

		// Partial ICP.
		if (_distance_threshold)
		{
			//std::cout << "max_squared_dists = " << squared_dists.maxCoeff() << std::endl;
			//std::cout << "avg_squared_dists = " << squared_dists.sum() / n << std::endl;

			double squared_distance_threshold = (*_distance_threshold) * (*_distance_threshold);
			Eigen::MatrixXd::Index subset_n = (squared_dists.array() <= squared_distance_threshold).count();
			Eigen::MatrixXd subset_all_X(3, subset_n);
			Eigen::MatrixXd subset_all_Y(3, subset_n);
			Eigen::MatrixXd subset_diff(3, subset_n);
			Eigen::RowVectorXd subset_squared_dists(subset_n);

			int count = 0;
			for (int i = 0; i < n; ++i)
			{
				if (squared_dists[i] <= squared_distance_threshold)
				{
					assert(count < subset_n);
					subset_all_X.col(count) = all_X.col(i);
					subset_all_Y.col(count) = all_Y.col(i);
					subset_diff.col(count) = diff.col(i);
					subset_squared_dists.col(count) = squared_dists.col(i);
					++count;
				}
			}
			assert(count == subset_n);

			n = subset_n;
			all_X.swap(subset_all_X);
			all_Y.swap(subset_all_Y);
			diff.swap(subset_diff);
			squared_dists.swap(subset_squared_dists);
		}

		if (n < MIN_NUM_ICP_POINT_PAIRS)
		{
			std::cerr << "Warning: Too few point pairs." << std::endl;
			return -1;
		}

		Eigen::Vector3d all_X_mean = all_X.rowwise().mean();
		Eigen::Vector3d all_Y_mean = all_Y.rowwise().mean();

		Eigen::MatrixXd centered_all_X = all_X.colwise() - all_X_mean;
		Eigen::MatrixXd centered_all_Y = all_Y.colwise() - all_Y_mean;

		Eigen::MatrixXd S = centered_all_X * centered_all_Y.transpose();
		assert(S.rows() == 3);
		assert(S.cols() == 3);

		Eigen::JacobiSVD<Eigen::MatrixXd> svd(S, Eigen::ComputeFullU | Eigen::ComputeFullV);

		Eigen::Matrix3d det_mat = Eigen::Matrix3d::Identity();
		det_mat(2, 2) = (svd.matrixV() * svd.matrixU().transpose()).determinant();

		_rotation_mat = svd.matrixV() * det_mat * svd.matrixU().transpose();
		_translation_vec = all_Y_mean - _rotation_mat * all_X_mean;

		diff = ((_rotation_mat * all_X).colwise() + _translation_vec) - all_Y;
		squared_dists = (diff.array() * diff.array()).colwise().sum();

		// Hausdorff distance.
		double error = std::sqrt(squared_dists.maxCoeff());
		if (error > prev_error)
			return -1;

		return error;
	}

	double run_iterative_closest_points(Eigen::MatrixXd &_X, const Eigen::MatrixXd &_Y,
		Eigen::Matrix3d &_rotation_mat, Eigen::Vector3d &_translation_vec,
		const double *_distance_threshold)
	{
		_rotation_mat = Eigen::Matrix3d::Identity();
		_translation_vec = Eigen::Vector3d::Zero();
		double prev_error = std::numeric_limits<double>::max();

		const unsigned int max_num_iterations = MAX_NUM_ICP_ITERATIONS;
		const double min_angle_difference = static_cast<double>(MIN_ICP_ANGLE_DIFFERENCE) / 180.0 * M_PI;
		const double min_translation = MIN_ICP_TRANSLATION;

		for (unsigned int iteration = 0; iteration < max_num_iterations; ++iteration)
		{
			Eigen::Matrix3d new_rotation_mat;
			Eigen::Vector3d new_translation_vec;
			double error = compute_rigid_transformation(_X, _Y,
				new_rotation_mat, new_translation_vec, _distance_threshold);

			if (error < 0)
			{
				// The transformation computation is failed.
				if (iteration == 0) return -1;
				//std::cout << "[FINAL] error = " << prev_error << std::endl;
				return prev_error;
			}
			else if (error > prev_error)
			{
				// NOTE:
				// Continue only when the error value is decreasing.
				//std::cout << "[FINAL] error = " << prev_error << std::endl;
				return prev_error;
			}

			prev_error = error;
			//std::cout << "error = " << prev_error << std::endl;

			Eigen::AngleAxisd rotation_angle;
			rotation_angle.fromRotationMatrix(new_rotation_mat);
			if (rotation_angle.angle() < min_angle_difference && new_translation_vec.norm() < min_translation)
				return prev_error;

			_rotation_mat = new_rotation_mat * _rotation_mat;
			_translation_vec = new_rotation_mat * _translation_vec + new_translation_vec;

			_X = ((new_rotation_mat * _X).colwise() + new_translation_vec);
		}

		return prev_error;
	}

}


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

// This program performs betweenness centrality (BC) clustering on the
// actor collaboration graph available at
// http://www.nd.edu/~networks/database/index.html and outputs the
// result of clustering in Pajek format.
//
// This program mimics the BC clustering algorithm program implemented
// by Shashikant Penumarthy for JUNG, so that we may compare results
// and timings.
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>
#include <map>

using namespace boost;

struct Actor
{
  Actor(int id = -1) : id(id) {}

  int id;
};

typedef adjacency_list<vecS, vecS, undirectedS, Actor,
                       property<edge_centrality_t, double> > ActorGraph;
typedef graph_traits<ActorGraph>::vertex_descriptor Vertex;
typedef graph_traits<ActorGraph>::edge_descriptor Edge;

void load_actor_graph(std::istream& in, ActorGraph& g)
{
  std::map<int, Vertex> actors;

  std::string line;
  while (getline(in, line)) {
    std::vector<Vertex> actors_in_movie;

    // Map from the actor numbers on this line to the actor vertices
    typedef tokenizer<char_separator<char> > Tok;
    Tok tok(line, char_separator<char>(" "));
    for (Tok::iterator id = tok.begin(); id != tok.end(); ++id) {
      int actor_id = lexical_cast<int>(*id);
      std::map<int, Vertex>::iterator v = actors.find(actor_id);
      if (v == actors.end()) {
        Vertex new_vertex = add_vertex(Actor(actor_id), g);
        actors[actor_id] = new_vertex;
        actors_in_movie.push_back(new_vertex);
      } else {
        actors_in_movie.push_back(v->second);
      }
    }

    for (std::vector<Vertex>::iterator i = actors_in_movie.begin();
         i != actors_in_movie.end(); ++i) {
      for (std::vector<Vertex>::iterator j = i + 1;
           j != actors_in_movie.end(); ++j) {
        if (!edge(*i, *j, g).second) add_edge(*i, *j, g);
      }
    }
  }
}

template<typename Graph, typename VertexIndexMap, typename VertexNameMap>
std::ostream&
write_pajek_graph(std::ostream& out, const Graph& g,
                  VertexIndexMap vertex_index, VertexNameMap vertex_name)
{
  out << "*Vertices " << num_vertices(g) << '\n';
  typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
  for (vertex_iterator v = vertices(g).first; v != vertices(g).second; ++v) {
    out << get(vertex_index, *v)+1 << " \"" << get(vertex_name, *v) << "\"\n";
  }

  out << "*Edges\n";
  typedef typename graph_traits<Graph>::edge_iterator edge_iterator;
  for (edge_iterator e = edges(g).first; e != edges(g).second; ++e) {
    out << get(vertex_index, source(*e, g))+1 << ' '
        << get(vertex_index, target(*e, g))+1 << " 1.0\n"; // HACK!
  }
  return out;
}

class actor_clustering_threshold : public bc_clustering_threshold<double>
{
  typedef bc_clustering_threshold<double> inherited;

 public:
  actor_clustering_threshold(double threshold, const ActorGraph& g,
                             bool normalize)
    : inherited(threshold, g, normalize), iter(1) { }

  bool operator()(double max_centrality, Edge e, const ActorGraph& g)
  {
    std::cout << "Iter: " << iter << " Max Centrality: "
              << (max_centrality / dividend) << std::endl;
    ++iter;
    return inherited::operator()(max_centrality, e, g);
  }

 private:
  unsigned int iter;
};

int main(int argc, char* argv[])
{
  std::string in_file;
  std::string out_file;
  double threshold = -1.0;
  bool normalize = false;

  // Parse command-line options
  {
    int on_arg = 1;
    while (on_arg < argc) {
      std::string arg(argv[on_arg]);
      if (arg == "-in") {
        ++on_arg; assert(on_arg < argc);
        in_file = argv[on_arg];
      } else if (arg == "-out") {
        ++on_arg; assert(on_arg < argc);
        out_file = argv[on_arg];
      } else if (arg == "-threshold") {
        ++on_arg; assert(on_arg < argc);
        threshold = lexical_cast<double>(argv[on_arg]);
      } else if (arg == "-normalize") {
        normalize = true;
      } else {
        std::cerr << "Unrecognized parameter \"" << arg << "\".\n";
        return -1;
      }
      ++on_arg;
    }

    if (in_file.empty() || out_file.empty() || threshold < 0) {
      std::cerr << "error: syntax is actor_clustering [options]\n\n"
                << "options are:\n"
                << "\t-in <infile>\tInput file\n"
                << "\t-out <outfile>\tOutput file\n"
                << "\t-threshold <value>\tA threshold value\n"
                << "\t-normalize\tNormalize edge centrality scores\n";
      return -1;
    }
  }

  ActorGraph g;

  // Load the actor graph
  {
    std::cout << "Building graph." << std::endl;
    std::ifstream in(in_file.c_str());
    if (!in) {
      std::cerr << "Unable to open file \"" << in_file << "\" for input.\n";
      return -2;
    }
    load_actor_graph(in, g);
  }

  // Run the algorithm
  std::cout << "Clusting..." << std::endl;
  betweenness_centrality_clustering(g,
    actor_clustering_threshold(threshold, g, normalize),
    get(edge_centrality, g));

  // Output the graph
  {
    std::cout << "Writing graph to file: " << out_file << std::endl;
    std::ofstream out(out_file.c_str());
    if (!out) {
      std::cerr << "Unable to open file \"" << out_file << "\" for output.\n";
      return -3;
    }
    write_pajek_graph(out, g, get(vertex_index, g), get(&Actor::id, g));
  }
  return 0;
}


/**
 * @file imufilter.hpp
 * @brief Filter for obtaining roll/pitch/yaw data from the IMU on Skybotix VI-Sensor
 * @author Fernando Caballero, fcaballero@us.es
 * @author Francisco J Perez-Grau, fjperez@catec.aero
 * @date October 2016
 *
Copyright (c) 2016, fcaballero, fjperezgrau
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef __IMUFILTER_H__
#define __IMUFILTER_H__

#include <Eigen/Core>
#include <Eigen/Dense>
#include <sensor_msgs/Imu.h>
#include <vector>

// Convenient constants
#define RAD2DEG(X) ( (X) * 57.2957795131)
#define DEG2RAD(X) ( (X) * 0.01745329251)
#define LIN2GRAV(X) ( (X) * 0.10197162)

/**
 * @class ImuFilter
 * @brief Estimates IMU angles from acceleration and gyro data
 */
class ImuFilter
{
public:

    /** @brief Constructor
     * @param T Prediction period (seconds)
     * @param imuTopic Name of the IMU data topic
     * @param calibTime Initial calibration time (seconds)
     */
    ImuFilter(double T, std::string imuTopic, double calibTime = 5.0)
	{
        init_ = false;
        T_ = T;
        T2_ = T*T;
        calibTime_ = calibTime;

		// IMU EKF parameters
        accDev_ = 0.006;//0.001
        gyrDev_ = 0.005;//0.01;
        magDev_ = 0;
        magXs_ = 1.0;
        magYs_ = 1.0;
        magZs_ = 1.0;
        magXo_ = 0.0;
        magYo_ = 0.0;
        magZo_ = 0.0;
        biaDev_ = 0.00001;//0.000001;
        biaTh_ = 0.005; //0.001;

        // Setup IMU data subscriber
        sub_ = nh_.subscribe(imuTopic, 10, &ImuFilter::imuDataCallback, this);
	}

    /** @brief Initialize EKF
     * Input: vector of sensor_msgs::Imu
     * The user must continue calling this function with new gyro data until it returns true
     */
	bool initialize(void)
	{
		double gx_m, gy_m, gz_m, gx_m2, gy_m2, gz_m2, gx_d, gy_d, gz_d;

		// Compute mean value and mean square
		gx_m = gy_m = gz_m = gx_m2 = gy_m2 = gz_m2 = 0.0;
        for(int i = 0; i < (int)calibData_.size(); i++)
		{
            gx_m += calibData_[i].angular_velocity.x;
            gy_m += calibData_[i].angular_velocity.y;
            gz_m += calibData_[i].angular_velocity.z;
            gx_m2 += calibData_[i].angular_velocity.x*calibData_[i].angular_velocity.x;
            gy_m2 += calibData_[i].angular_velocity.y*calibData_[i].angular_velocity.y;
            gz_m2 += calibData_[i].angular_velocity.z*calibData_[i].angular_velocity.z;
		}
        gx_m = gx_m/(double)calibData_.size();
        gy_m = gy_m/(double)calibData_.size();
        gz_m = gz_m/(double)calibData_.size();
        gx_m2 = gx_m2/(double)calibData_.size();
        gy_m2 = gy_m2/(double)calibData_.size();
        gz_m2 = gz_m2/(double)calibData_.size();
		//std::cout << "gxM: " << gx_m << ", gyM: " << gy_m << ", gzM: " << gz_m << std::endl;

		// Compute standar deviation of gyros
		gx_d = sqrt(gx_m2-gx_m*gx_m);
		gy_d = sqrt(gy_m2-gy_m*gy_m);
		gz_d = sqrt(gz_m2-gz_m*gz_m);
		//std::cout << "gxDev: " << gx_d << ", gyDev: " << gy_d << ", gzDev: " << gz_d << std::endl;

		// Initalize compass calibration
        magCal_[0] = magXs_; magCal_[1] = magYs_; magCal_[2] = magZs_;
        magCal_[3] = magXo_; magCal_[4] = magYo_; magCal_[5] = magZo_;

		// Initialize sensor variances
        accVar_[0] = accDev_*accDev_; 	accVar_[1] = accDev_*accDev_; 	accVar_[2] = accDev_*accDev_;		// Variance in g
        gyrVar_[0] = gyrDev_*gyrDev_; gyrVar_[1] = gyrDev_*gyrDev_; gyrVar_[2] = gyrDev_*gyrDev_;	// Variance in rad/s
        magVar_[0] = magDev_*magDev_; 	magVar_[1] = magDev_*magDev_; 	magVar_[2] = magDev_*magDev_;	// Variance in mGaus wth data normalized to 1
        biaVar_[0] = biaDev_*biaDev_; biaVar_[1] = biaDev_*biaDev_; biaVar_[2] = biaDev_*biaDev_;	// Variance in rad/s

		// Initialize accelerometer threshold
        accTh_ = sqrt(accVar_[0]+accVar_[1]+accVar_[2]);

		// Initialize state vector x = [rx, ry, rz, gbx, gby, gbz]
        rx_ = ry_ = rz_ = 0.0;
        gbx_ = gx_m;
        gby_ = gy_m;
        gbz_ = gz_m;

		// Initialize covariance matrix
        P_.setIdentity(6, 6);
        P_(0,0) = M_PI_2;
        P_(1,1) = M_PI_2;
        P_(2,2) = M_PI_2;
        P_(3,3) = 0.01*0.01;
        P_(4,4) = 0.01*0.01;
        P_(5,5) = 0.01*0.01;
        /*P_(3,3) = biaVar_[0];
        P_(4,4) = biaVar_[1];
        P_(5,5) = biaVar_[2];*/

        if(gx_d < biaTh_ && gy_d < biaTh_ && gz_d < biaTh_)
		{
            init_ = true;
            ROS_INFO("IMU filter initialized");

			return true;
		}
		else
			return false;
	}

    /** EKF prediction stage based on gyro information
     * @param[in] gx Raw X gyro data (rad/s)
     * @param[in] gy Raw Y gyro data (rad/s)
     * @param[in] gz Raw Z gyro data (rad/s)
     */
	bool predict(double gx, double gy, double gz)
	{
		// Check initialization
        if(!init_)
			return false;

		// Compute matrix F
		Eigen::Matrix<double, 6, 6> F;
        F(0,0) = 1, F(0,1) = 0, F(0,2) = 0, F(0,3) = -T_, F(0,4) = 0,   F(0,5) = 0;
        F(1,0) = 0, F(1,1) = 1, F(1,2) = 0, F(1,3) = 0,  F(1,4) = -T_,  F(1,5) = 0;
        F(2,0) = 0, F(2,1) = 0, F(2,2) = 1, F(2,3) = 0,  F(2,4) = 0,   F(2,5) = -T_;
		F(3,0) = 0, F(3,1) = 0, F(3,2) = 0, F(3,3) = 1,  F(3,4) = 0,   F(3,5) = 0;
		F(4,0) = 0, F(4,1) = 0, F(4,2) = 0, F(4,3) = 0,  F(4,4) = 1,   F(4,5) = 0;
		F(5,0) = 0, F(5,1) = 0, F(5,2) = 0, F(5,3) = 0,  F(5,4) = 0,   F(5,5) = 1;

		// Update covariance matrix
        P_ = F*P_*F.transpose();
        P_(0,0) += gyrVar_[0]*T2_;
        P_(1,1) += gyrVar_[1]*T2_;
        P_(2,2) += gyrVar_[2]*T2_;
        P_(3,3) += biaVar_[0]*T_;
        P_(4,4) += biaVar_[1]*T_;
        P_(5,5) += biaVar_[2]*T_;

		// Update state vector
        rx_ += T_*(gx - gbx_);
        ry_ += T_*(gy - gby_);
        rz_ += T_*(gz - gbz_);

		return true;
	}

    /** EKF update stage based on accelerometer information
     * @param[in] ax Raw X accelerometer data (g)
     * @param[in] ay Raw Y accelerometer data (g)
     * @param[in] az Raw Z accelerometer data (g)
     */
	bool update(double ax, double ay, double az)
	{
		double crx, srx, cry, sry, mod, y[3];

		// Check initialization
        if(!init_)
			return false;

		// Pre-compute constants
        crx = cos(rx_);
        cry = cos(ry_);
        srx = sin(rx_);
        sry = sin(ry_);

		// Create measurement jacobian H
		Eigen::Matrix<double, 3, 6> H;
		H(0,0) = 0;			H(0,1) = cry;		H(0,2) = 0; H(0,3) = 0; H(0,4) = 0; H(0,5) = 0;
		H(1,0) = -crx*cry; 	H(1,1) = srx*sry; 	H(1,2) = 0; H(1,3) = 0; H(1,4) = 0; H(1,5) = 0;
		H(2,0) = cry*srx;	H(2,1) = crx*sry;	H(2,2) = 0; H(2,3) = 0; H(2,4) = 0; H(2,5) = 0;

		// Compute measurement noise jacoban R
		Eigen::Matrix<double, 3, 3> R;
		mod = fabs(sqrt(ax*ax + ay*ay + az*az)-1);
		R.setZero(3, 3);
        R(0,0) = accVar_[0];
        R(1,1) = accVar_[1];
        R(2,2) = accVar_[2];
		/*if(mod > accTh)
		{
			R(0,0) += 1.5*mod*mod;
			R(1,1) += 1.5*mod*mod;
			R(2,2) += 1.5*mod*mod;
		}*/

		// Compute innovation matrix
		Eigen::Matrix<double, 3, 3> S;
        S = H*P_*H.transpose()+R;

		// Compute kalman gain
		Eigen::Matrix<double, 6, 3> K;
        K = P_*H.transpose()*S.inverse();

		// Compute mean error
		y[0] = ax-sry;
		y[1] = ay+cry*srx;
		y[2] = az+crx*cry;

		// Compute new state vector
        rx_ += K(0,0)*y[0]+K(0,1)*y[1]+K(0,2)*y[2];
        ry_ += K(1,0)*y[0]+K(1,1)*y[1]+K(1,2)*y[2];
        rz_ += K(2,0)*y[0]+K(2,1)*y[1]+K(2,2)*y[2];
        gbx_ += K(3,0)*y[0]+K(3,1)*y[1]+K(3,2)*y[2];
        gby_ += K(4,0)*y[0]+K(4,1)*y[1]+K(4,2)*y[2];
        gbz_ += K(5,0)*y[0]+K(5,1)*y[1]+K(5,2)*y[2];

		// Compute new covariance matrix
		Eigen::Matrix<double, 6, 6> I;
		I.setIdentity(6, 6);
        P_ = (I-K*H)*P_;

		return true;
	}

    /** EKF update stage based on accelerometer information
     * @param[in] ax Raw X accelerometer data (g)
     * @param[in] ay Raw Y accelerometer data (g)
     * @param[in] az Raw Z accelerometer data (g)
     * @param[in] mx Raw X magnetometer data ()
     * @param[in] my Raw Y magnetometer data ()
     * @param[in] mz Raw Z magnetometer data ()
     */
	bool update(double ax, double ay, double az, double mx, double my, double mz)
	{
		double crx, srx, cry, sry, crz, srz, mod, y[5], hx, hy;

		// Check initialization
        if(!init_)
			return false;

		// Remove distortion and normalize magnetometer
        mx = (mx - magCal_[3])/magCal_[0];
        my = (my - magCal_[4])/magCal_[1];
        mz = (mz - magCal_[5])/magCal_[2];
		mod = sqrt(mx*mx + my*my + mz*mz);
		mx /= mod;
		my /= mod;
		mz /= mod;

		// Pre-compute constants
        crx = cos(rx_);
        cry = cos(ry_);
        crz = cos(rz_);
        srx = sin(rx_);
        sry = sin(ry_);
        srz = sin(rz_);

		// Create measurement jacobian H
		Eigen::Matrix<double, 5, 6> H;
		H(0,0) = 0;			H(0,1) = cry;		H(0,2) = 0; 	H(0,3) = 0; H(0,4) = 0; H(0,5) = 0;
		H(1,0) = -crx*cry; 	H(1,1) = srx*sry; 	H(1,2) = 0; 	H(1,3) = 0; H(1,4) = 0; H(1,5) = 0;
		H(2,0) = cry*srx;	H(2,1) = crx*sry;	H(2,2) = 0; 	H(2,3) = 0; H(2,4) = 0; H(2,5) = 0;
		H(3,0) = 0;			H(3,1) = 0;			H(3,2) = -srz; 	H(3,3) = 0; H(3,4) = 0; H(3,5) = 0;
		H(4,0) = 0;			H(4,1) = 0;			H(4,2) = -crz; 	H(4,3) = 0; H(4,4) = 0; H(4,5) = 0;

        // Compute measurement noise jacobian R
		Eigen::Matrix<double, 5, 5> R;
		mod = fabs(sqrt(ax*ax + ay*ay + az*az)-1);
		R.setZero(5, 5);
        R(0,0) = accVar_[0];
        R(1,1) = accVar_[1];
        R(2,2) = accVar_[2];
        R(3,3) = magVar_[0];
        R(4,4) = magVar_[1];
        if(mod > accTh_)
		{
			R(0,0) += 1.5*mod*mod;
			R(1,1) += 1.5*mod*mod;
			R(2,2) += 1.5*mod*mod;
		}

		// Compute innovation matrix
		Eigen::Matrix<double, 5, 5> S;
        S = H*P_*H.transpose()+R;

        // Compute Kalman gain
		Eigen::Matrix<double, 6, 5> K;
        K = P_*H.transpose()*S.inverse();

		// Compute mean error
		hx = mx*cry + mz*crx*sry + my*srx*sry;
		hy = my*crx - mz*srx;
		mod = sqrt(hx*hx+hy*hy);
		y[0] = ax-sry;
		y[1] = ay+cry*srx;
		y[2] = az+crx*cry;
		y[3] = hx/mod-crz;
		y[4] = hy/mod+srz;

		// Compute new state vector
        rx_ += K(0,0)*y[0]+K(0,1)*y[1]+K(0,2)*y[2]+K(0,3)*y[3]+K(0,4)*y[4];
        ry_ += K(1,0)*y[0]+K(1,1)*y[1]+K(1,2)*y[2]+K(1,3)*y[3]+K(1,4)*y[4];
        rz_ += K(2,0)*y[0]+K(2,1)*y[1]+K(2,2)*y[2]+K(2,3)*y[3]+K(2,4)*y[4];
        gbx_ += K(3,0)*y[0]+K(3,1)*y[1]+K(3,2)*y[2]+K(3,3)*y[3]+K(3,4)*y[4];
        gby_ += K(4,0)*y[0]+K(4,1)*y[1]+K(4,2)*y[2]+K(4,3)*y[3]+K(4,4)*y[4];
        gbz_ += K(5,0)*y[0]+K(5,1)*y[1]+K(5,2)*y[2]+K(5,3)*y[3]+K(5,4)*y[4];

		// Compute new covariance matrix
		Eigen::Matrix<double, 6, 6> I;
		I.setIdentity(6, 6);
        P_ = (I-K*H)*P_;

		return true;
	}

    /** @brief Get estimated Euler angles in radians interval [-PI,PI] rad
     * @param[out] rx Estimated roll (rad)
     * @param[out] ry Estimated pitch (rad)
     * @param[out] rz Estimated yaw (rad)
     */
    bool getAngles(double &rx, double &ry, double &rz)
	{
        rx = Pi2PiRange(rx_);
        ry = Pi2PiRange(ry_);
        rz = Pi2PiRange(rz_);
		return true;
	}

    /** @brief Get estimated integrated Euler angles since last reset
     * @param[out] rx Estimated integrated roll (rad)
     * @param[out] ry Estimated integrated pitch (rad)
     * @param[out] rz Estimated integrated yaw (rad)
     */
    bool getAngleIntegration(double &irx, double &iry, double &irz)
    {
        irx = irx_;
        iry = iry_;
        irz = irz_;
        return true;
    }

    /** @brief Reset integrated angles
     */
    bool resetAngleIntegration(void)
    {
        irx_ = iry_ = irz_ = 0.0;
        return true;
    }

    /** @brief Get estimated gyro biases in rad/s
     * @param[out] gbx Estimated X gyro bias (rad/s)
     * @param[out] gby Estimated Y gyro bias (rad/s)
     * @param[out] gbz Estimated Z gyro bias (rad/s)
     */
    bool getBIAS(double &gbx, double &gby, double &gbz)
	{
        gbx = gbx_;
        gby = gby_;
        gbz = gbz_;

		return true;
	}

    /** @brief IMU initialization return function
     * @return Returns true if IMU initialized
     */
	bool isInit(void)
	{
        return init_;
	}

    /** IMU sensor data callback
     * @param[in] msg IMU data message
     */
	void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
	{
		// Check for IMU initialization
        if(!init_)
		{
            calibData_.push_back(*msg);
            if(calibData_.size() > calibTime_/T_)
				initialize();

			return;
		}

		// Process sensor data
		predict(msg->angular_velocity.z, msg->angular_velocity.x, msg->angular_velocity.y);
		update(LIN2GRAV(msg->linear_acceleration.z), LIN2GRAV(msg->linear_acceleration.x), LIN2GRAV(msg->linear_acceleration.y));

        // Integrate angle rates
        irx_ += (msg->angular_velocity.z-gbx_)*T_;
        iry_ += (msg->angular_velocity.x-gby_)*T_;
        irz_ += (msg->angular_velocity.y-gbz_)*T_;
    }

protected:

    /** @brief Round down absolute function
     * @param[in] value Input value
     * @return Input value floored
     */
    double floorAbsolute( double value )
	{
	  if (value < 0.0)
		return ceil( value );
	  else
		return floor( value );
	}

    /** @brief Convert angles into interval [-PI,PI] rad
     * @param[in] contAngle Input angle (rad)
     * @return Input angle in interval [-PI,PI] rad
     */
    double Pi2PiRange(double contAngle)
	{
        double boundAngle = 0.0;
        if(fabs(contAngle)<=M_PI)
            boundAngle= contAngle;
		else
		{
            if(contAngle > M_PI)
                boundAngle = (contAngle-2*M_PI) - 2*M_PI*floorAbsolute((contAngle-M_PI)/(2*M_PI));

            if(contAngle < - M_PI)
                boundAngle = (contAngle+2*M_PI) - 2*M_PI*floorAbsolute((contAngle+M_PI)/(2*M_PI));
		}

        return boundAngle;
	}

    double calibTime_;      /**< IMU calibration time*/

    double T_;                                  /**< IMU KF prediction period*/
    double T2_;                                 /**< Squared IMU KF prediction period*/
    double rx_, ry_, rz_, gbx_, gby_, gbz_;     /**< IMU KF state vector x = [rx, ry, rz, gbx, gby, gbz]*/
    Eigen::MatrixXd P_;                         /**< IMU KF matrix*/

    double magCal_[6];      /**< Sensor calib info [gainX, gainY, gainZ, offsetX, offsetY, offsetZ]*/

    double accVar_[3];      /**< Accelerometers variances [varX, varY, varZ]*/
    double magVar_[3];      /**< Magnetometers variances [varX, varY, varZ]*/
    double gyrVar_[3];      /**< Gyroscopes variances [varX, varY, varZ]*/
    double biaVar_[3];      /**< Bias variances [varX, varY, varZ]*/

    double accTh_;          /**< Accelerometer threshold for filter updating*/

    bool init_;             /**< Flag indicating if IMU has been initialized*/

	// EKF Parameters
    double accDev_;
    double gyrDev_;
    double magDev_;
    double biaDev_;
    double biaTh_;
    double magXs_;
    double magYs_;
    double magZs_;
    double magXo_;
    double magYo_;
    double magZo_;

    std::vector<sensor_msgs::Imu> calibData_;   /**< IMU data for initialization*/

    ros::NodeHandle nh_;        /**< ROS node handler*/
    ros::Subscriber sub_;       /**< IMU data subscriber*/

    double irx_, iry_, irz_;    /**< Integrated angles since last reset*/
};

#endif


/*
  Copyright (c) 2012,2013,2014 Matthew H. Reilly (kb1vc)
  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.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "HilbertTransformer.hxx"

#include <iostream>
#include <string.h>
#include <fftw3.h>
#include <boost/format.hpp>

int dbgctr = 0;

static unsigned int ipow(unsigned int x, unsigned int y) __attribute__ ((unused));
static unsigned int ipow(unsigned int x, unsigned int y)
{
  unsigned int ret;
  ret = 1;
  unsigned int i;

  for(i = 0; i < y; i++) {
    ret *= x;
  }

  return ret;
}

SoDa::HilbertTransformer::HilbertTransformer(unsigned int inout_buffer_length,
					     unsigned int filter_length) :
  SoDa::Base("HilbertTransformer")
{
  // these are the salient dimensions for this Overlap/Save
  // widget (for terminology, see Lyons pages 719ff
  M = inout_buffer_length;

  // now find N.
  N = 4 * filter_length;
  while(N < (M + filter_length)) {
    N = N * 2;
  }
  // now that we have N, we can back-calculate Q.
  Q = (N - M) + 1;

  //  std::cerr << "\n\nHILBERT picked N = " << N << " Q = " << Q << " M = " << M << std::endl;

  std::complex<float> htu[N], htl[N];

  // create the impulse response images
  // There is probably a simpler way, but the obvious real/imag swap
  // scheme doesn't work at all well.
  HTu_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  HTl_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  Pass_U_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  Pass_L_filter = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * N);
  fftwf_plan HTu_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htu, (fftwf_complex *) HTu_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  if(HTu_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating HT upper plan...\n");
  }
  fftwf_plan HTl_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htl, (fftwf_complex *) HTl_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  if(HTl_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating HT lower plan...\n");
  }

  // now build the time domain image of the filter.
  unsigned int i, j;
  for(i = 0; i < N; i++) {
    htu[i] = htl[i] = std::complex<float>(0.0, 0.0);
  }
  // this is actually a scaled ht -- removing the Fs * 2 / pi scaling factor.
  for(i = 0, j = (Q / 2); i < (Q / 2); i++) {
    if((i & 1) != 0) {
      htu[j + i] = std::complex<float>(1.0 / ((float) i), 0.0);
      htu[j - i] = std::complex<float>(-1.0 / ((float) i), 0.0);
      htl[j + i] = std::complex<float>(-1.0 / ((float) i), 0.0);
      htl[j - i] = std::complex<float>(1.0 / ((float) i), 0.0);
    }
  }
  fftwf_execute(HTu_plan);
  fftwf_execute(HTl_plan);
  // now we have the HT filter image
  fftwf_destroy_plan(HTu_plan);
  fftwf_destroy_plan(HTl_plan);

  // now do the delay filter
  fftwf_plan dly_plan = fftwf_plan_dft_1d(N,
					 (fftwf_complex *) htu, (fftwf_complex *) Pass_U_filter,
					 FFTW_FORWARD, FFTW_ESTIMATE);
  // load up a new impulse response
  for(i = 0; i < N; i++) htu[i] = std::complex<float>(0.0, 0.0);
  htu[Q/2] = std::complex<float>(1.0, 0.0);
  // now create the passthrough filter
  fftwf_execute(dly_plan);
  fftwf_destroy_plan(dly_plan);

  // Do some equalization on the pass filter to fix the low frequency response
  // to match the response of the HT filter.
  for(i = 0; i < N; i++) {
    float tumag = abs(HTu_filter[i]);
    float tlmag = abs(HTl_filter[i]);
    float pmag = abs(Pass_U_filter[i]);
    float uadj = 1.0;
    float ladj = 1.0;
    if(pmag > 0.001) {
      uadj = tumag / pmag;
      ladj = tlmag / pmag;
      if (uadj > 2) uadj = 1.0;
      if (ladj > 2) ladj = 1.0;
    }
    Pass_L_filter[i] = ladj * Pass_U_filter[i];
    Pass_U_filter[i] = uadj * Pass_U_filter[i];
  }

  // calculate the magnitudes of the two filters.
  float hmag, pmag;
  hmag = 0.0;
  pmag = 0.0;
  for(i = 0; i < N; i++) {
    hmag += HTu_filter[i].real() * HTu_filter[i].real()
      + HTu_filter[i].imag() * HTu_filter[i].imag();
    pmag += Pass_U_filter[i].real() * Pass_U_filter[i].real()
      + Pass_U_filter[i].imag() * Pass_U_filter[i].imag();
  }

  // now allocate all the storage vectors
  fft_I_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_Q_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_I_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  fft_Q_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_I_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_Q_input = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_I_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));
  ifft_Q_output = (std::complex<float> *) fftwf_malloc(sizeof(std::complex<float>) * (N + 128));

  // and create the plans
  forward_I_plan = fftwf_plan_dft_1d(N,
				     (fftwf_complex *) fft_I_input, (fftwf_complex *) fft_I_output,
				     FFTW_FORWARD, FFTW_ESTIMATE);
  if(forward_I_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating forward I plan...\n");
  }

  forward_Q_plan = fftwf_plan_dft_1d(N,
				     (fftwf_complex *) fft_Q_input,
				     (fftwf_complex *) fft_Q_output,
				     FFTW_FORWARD, FFTW_ESTIMATE);
  if(forward_Q_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating forward Q plan...\n");
  }

  backward_I_plan = fftwf_plan_dft_1d(N, (fftwf_complex *) ifft_I_input,
				      (fftwf_complex *) ifft_I_output,
				      FFTW_BACKWARD, FFTW_ESTIMATE);
  if(backward_I_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating backward I plan...\n");
  }
  backward_Q_plan = fftwf_plan_dft_1d(N, (fftwf_complex *) ifft_Q_input,
				      (fftwf_complex *) ifft_Q_output,
				      FFTW_BACKWARD, FFTW_ESTIMATE);
  if(backward_Q_plan == NULL) {
    throw SoDa::Exception("Hilbert had trouble creating backward Q plan...\n");
  }
  // zero out the start of the fft_input buffer for the first iteration.
  for(i = 0; i <= Q-1; i++) {
    fft_I_input[i] = 0.0;
    fft_Q_input[i] = 0.0;
  }

  // finally, set the transform gain (1/N)
  //   passthrough_gain = 1.0 / ((float) N);
  //  H_transform_gain = 2.0 / (M_PI * 0.966 * ((float) N)); // 0.966 is a fudge factor
  H_transform_gain = 2.0 / (M_PI * ((float) N));
  passthrough_gain = H_transform_gain;
}

unsigned int SoDa::HilbertTransformer::apply(std::complex<float> * inbuf,
					     std::complex<float> * outbuf,
					     bool pos_sided, float gain)
{
  unsigned int i, j;

  std::complex<float> *HT_F;
  std::complex<float> *PA_F;

  if(pos_sided) {
    HT_F = HTl_filter;
    PA_F = Pass_L_filter;
  }
  else {
    HT_F = HTu_filter;
    PA_F = Pass_U_filter;
  }
  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff
  // copy the I channel to the tail of the input buffer.
  memcpy(&(fft_I_input[Q-1]), inbuf, sizeof(std::complex<float>) * M);

  // do a pass through
  fftwf_execute(forward_I_plan);

  // now save the tail of the input to the save buffer
  memcpy(fft_I_input, &(inbuf[1 + (M - Q)]), sizeof(std::complex<float>) * (Q - 1));

  // now apply the delay filter and the hilbert transform
  for(i = 0; i < N; i++) {
    ifft_Q_input[i] = fft_I_output[i] * HT_F[i];
    ifft_I_input[i] = fft_I_output[i] * PA_F[i];
  }

  // do the inverse fft for the I and Q channels
  fftwf_execute(backward_I_plan);
  fftwf_execute(backward_Q_plan);

  // now put the two channels together
  // Note that we're shifting the normal sampling window.  This is because the
  // quadrature sampler is just not quite right for
  for(i = 0, j = Q-1; i < M; i++, j++) {
    // seems like it worked once... but apparent shift is same for either sideband
    outbuf[i] = std::complex<float>(ifft_I_output[j].real() * passthrough_gain * gain,
				    ifft_Q_output[j].real() * H_transform_gain * gain);
  }

  dbgctr++;

  return M;
}

unsigned int SoDa::HilbertTransformer::apply(float * inbuf,
					     std::complex<float> * outbuf,
					     bool pos_sided, float gain)
{
  unsigned int i;
  // This creates an analytic signal from a single input buffer.

  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff
  std::complex<float> cinbuf[M];

  // copy the I channel to the tail of the input buffer.
  for(i = 0; i < M; i++) {
    cinbuf[i] = std::complex<float>(inbuf[i], 0.0);
  }

  // call the complex HT
  return apply(cinbuf, outbuf, pos_sided, gain);

  return M;
}

unsigned int SoDa::HilbertTransformer::applyIQ(std::complex<float> * inbuf,
					       std::complex<float> * outbuf,
					       float gain)
{
  unsigned int i, j;
  // This creates an analytic signal from a single input buffer.

  // Note we're using overlap-and-save  see the OSFilter implementation
  // or Lyons pages 719ff

  // copy the I channel to the tail of the I input buffer.
  // copy the I channel to the tail of the Q input buffer.
  for(i = 0; i < M; i++) {
    fft_I_input[i + (Q-1)] = std::complex<float>(inbuf[i].real(), 0.0);
    fft_Q_input[i + (Q-1)] = std::complex<float>(inbuf[i].imag(), 0.0);
  }

  // do a the I (passthrough) and Q channel FFTs
  fftwf_execute(forward_I_plan);
  fftwf_execute(forward_Q_plan);

  // now save the tail of the input to the save buffer
  for(i = 0; i < (Q - 1); i++) {
    fft_I_input[i] = std::complex<float>(inbuf[i + 1 + (M - Q)].real(), 0.0);
    fft_Q_input[i] = std::complex<float>(inbuf[i + 1 + (M - Q)].imag(), 0.0);
  }

  // now apply the delay filter (to I) and the hilbert transform (to Q)
  for(i = 0; i < N; i++) {
    ifft_Q_input[i] = fft_Q_output[i] * HTu_filter[i];
    ifft_I_input[i] = fft_I_output[i] * Pass_U_filter[i];
  }

  // do the inverse fft for the I and Q channels
  fftwf_execute(backward_I_plan);
  fftwf_execute(backward_Q_plan);

  // now put the two channels together
  // Note that we're shifting the normal sampling window.  This is because the
  // quadrature sampler is just not quite right for
  for(i = 0, j = Q-1; i < M; i++, j++) {
    outbuf[i] = std::complex<float>(ifft_I_output[j].real() * passthrough_gain * gain,
				    ifft_Q_output[j].real() * H_transform_gain * gain);
  }

  dbgctr++;

  return M;
}

std::ostream & SoDa::HilbertTransformer::dump(std::ostream & os)
{
  unsigned int i, j;
  for(i = 0; i < N; i++) {
    j = i;
    float mag = std::abs(HTl_filter[i]);
    float ang = std::arg(HTl_filter[i]);
    float pmag = std::abs(Pass_L_filter[i]);
    float pang = std::arg(Pass_L_filter[i]);
    os << boost::format("%d %f %f %f %f %f %f %f %f\n")
      % j % HTu_filter[i].real() % HTu_filter[i].imag() % mag % ang %
      Pass_U_filter[i].real() % Pass_U_filter[i].imag() % pmag % pang;
  }
  return os;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to 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)
//
// Example combining Boost.Geometry with Boost.Units

#include <iostream>

#include <boost/geometry/geometry.hpp>

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/io.hpp>

// TEMPORARY this will go to somewhere within Boost.Geometry
namespace boost { namespace geometry
{

namespace cs
{

template <typename Unit>
struct units_cartesian {};

}

namespace traits
{
template<typename U>
struct cs_tag<cs::units_cartesian<U> >
{
    typedef cartesian_tag type;
};

}

namespace model
{

// Define a point type to interoperate with Boost.Units, having
// 1. a constructor taking quantities
// 2. defining a quantified coordinate system
// Note that all values are still stored in "normal" types as double
template <typename U, std::size_t D = 2, typename T = double, typename CS = cs::units_cartesian<U> >
class quantity_point : public model::point<T, D, CS>
{
    typedef boost::units::quantity<U, T> qtype;

public :

    // Templated constructor to allow constructing with other units then qtype,
    // e.g. to convert from centimeters to meters
    template <typename Q>
    inline quantity_point(Q const& x, Q const& y)
        : model::point<T, D, CS>(
            qtype(x).value(),
            qtype(y).value())
    {}
};

}

// Adapt quantity_point to the Point Concept
#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct tag<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef point_tag type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_type<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateType type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct coordinate_system<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
{
    typedef CoordinateSystem type;
};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem>
struct dimension<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> >
    : boost::mpl::int_<DimensionCount>
{};

template<typename Units, std::size_t DimensionCount, typename CoordinateType, typename CoordinateSystem, std::size_t Dimension>
struct access<model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>, Dimension >
{
    static inline CoordinateType get(
        model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem> const& p)
    {
        return p.template get<Dimension>();
    }

    static inline void set(model::quantity_point<Units, DimensionCount, CoordinateType, CoordinateSystem>& p,
        CoordinateType const& value)
    {
        p.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

// For extra support for functions as distance,area,get,set
namespace units
{
    namespace detail
    {
        // Define an extra meta-function to get the units of a coordinate system
        template <typename CS>
        struct unit_dimension
        {
            // define it as dimensionless
            // or MPL ASSERT
        };

        template <typename U>
        struct unit_dimension<cs::units_cartesian<U> >
        {
            typedef U type;
        };
    }

    // Define an extra metafunction to define the quantity of a Geometry type
    template <typename Geometry, typename CT = typename coordinate_type<Geometry>::type>
    struct quantity
    {
        typedef boost::units::quantity
            <
                typename detail::unit_dimension
                    <
                        typename coordinate_system<Geometry>::type
                    >::type,
                CT
            > type;
    };

    template <typename Geometry1, typename Geometry2>
    inline typename quantity<Geometry1, typename default_distance_result<Geometry1, Geometry2>::type>::type
        distance(Geometry1 const& g1, Geometry2 const& g2)
    {
        typedef typename quantity<Geometry1, typename default_distance_result<Geometry1, Geometry2>::type>::type q;
        return q::from_value(geometry::distance(g1, g2));
    }

    template <std::size_t Index, typename Point>
    inline typename quantity<Point>::type get(Point const& p)
    {
        typedef typename quantity<Point>::type q;
        return q::from_value(geometry::get<Index>(p));
    }
}

}}
// END TEMPORARY

int main(void)
{
    using namespace boost::geometry;
    using namespace boost::units;

    // 1: using it directly
    {
        typedef model::quantity_point<si::length, 2> point;
        point p1(1 * si::meter, 2 * si::meter);
        point p2(3 * si::meter, 4 * si::meter);

        std::cout << get<0>(p2) << std::endl;

        // This is a little inconvenient:
        quantity<si::length> d = distance(p1, p2) * si::meter;

        std::cout << d << std::endl;
    }

    // 2: same but now using centimeters, and using boost::geometry::units::
    {
        typedef model::quantity_point<cgs::length, 2> point;
        point p1(1 * si::meter, 2 * si::meter);
        point p2(3 * si::meter, 4 * si::meter);

        std::cout << boost::geometry::units::get<0>(p2) << std::endl;
        quantity<cgs::length> d = boost::geometry::units::distance(p1, p2);
        std::cout << d << std::endl;
    }

    return 0;
}


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// 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 "lipton-tarjan.h"
#include "typedefs.h"
#include "strutil.h"
#include "graphutil.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/bimap.hpp>
#include <iostream>
#include <fstream>
#include <csignal>
#include <utility>
using namespace std;
using namespace boost;

int main(int argc, char* argv[])
{
        vector<string> fname;
        if( argc < 2 ){
                cout << "Usage: lt [filename]\n";
                return 0;
        }
        for( uint i = 1; i < argc; ++i ) fname.push_back(argv[i]);

        for( string& f : fname ){
                cout << "loading graph\n";

                Graph g = load_graph(f);

#ifdef GRAPH_TYPE_LIST
                init_vert_propmap(g);
#endif

                uint n = num_vertices(g);

                cout << "n: " << n << '\n';
                uint e = num_edges(g);

                cout << "starting lipton tarjan...\n";
                print_graph(g);

		try {
			Partition p = lipton_tarjan_separator(g);
			uint num_verts_finished = p.total_num_verts();
			cout << "Finished!\n";

                        p.print(&g);

                        BOOST_ASSERT(p.verify_edges(g));
			BOOST_ASSERT(p.verify_sizes(g));
			//p.print(vmap2);

			cout << "finished num verts: " << num_verts_finished << '\n';
		} catch (NotPlanarException e) {
			cout << "cannot finish lipton-tarjan because graph is not planar\n";
		} catch (NoNontreeEdgeException e) {
			cout << "cannot finish lipton-tarjan because I could not find a nontree edge out of " << e.num_edges << " edges\n";
		}
        }
}