#include <chrono>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <boost/format.hpp>
#include "bloom_filter.h"
#include "/home/vagrant/nfllib/include/nfl.hpp"
#include "/home/vagrant/nfllib/include/nfl/params.hpp"

//#define memset_s(W,WL,V,OL) memset(W,V,OL)
//#include "/Users/mkilliji/Code/keccak-tiny/keccak-tiny.c"

#define CONFIG 512, 62, uint64_t

template <class P>
__attribute__((noinline)) static void encrypt(P& a, P& b, P const & message, uint64_t pt_modulus, P const & pka, P const & pkb, P const & pkaprime, P const & pkbprime, FastGaussianNoise<uint8_t, typename P::value_type, 2> *g_prng)
{
    // u
    P e1 = nfl::gaussian<typename P::value_type>(g_prng);
    e1.ntt_pow_phi();

    // p*e_1
    P e2 = nfl::gaussian<typename P::value_type>(g_prng, pt_modulus);
    e2.ntt_pow_phi();

    // p*e_2
    P e3 = nfl::gaussian<typename P::value_type>(g_prng, pt_modulus);
    e3 = e3 + message;
    e3.ntt_pow_phi();

    // encCset_a = pka * u + p*e_2
    a = e1 * pka + e2 ;

    // encCset_b = pkb * u + p*e_3 + message
    b = e1 * pkb + e3;
}

template <class P>
__attribute__((noinline)) static void decrypt(P& tmp, P const & a, P const& b, P const& s, P const& sprime, typename P::value_type const modulus, uint64_t const pt_modulus)
{
    using fuck = typename P::signed_value_type;
    tmp = b -  nfl::shoup(a * s, sprime);

    tmp.invntt_pow_invphi();

    uint64_t const adder=(uint64_t)(((1ULL<<63)/pt_modulus)*pt_modulus-modulus);

    for(auto & v : tmp)
    {
        v= (v<modulus/2) ? v%pt_modulus : (v + adder)%pt_modulus;
    }

}

template <class P, class Q>
static void plonge(P& out, Q& in) {
    for(int j=0;j<out.nmoduli;j++) {
        for(int i=0;i<out.degree;i++) {
            out(j,i)=in(j,i);
        }
    }
}

template <class P>
static void encode(P& res, unsigned char const bits[],  int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    for(int j=0;j<tmp.nmoduli;j++) {
        for(int i=0;i<tmp.degree&&i<size;i++) {
            tmp(j,i)=bits[i];
        }
    }
    tmp.invntt_pow_invphi();
    plonge(res,tmp);
}

template <class P>
static void decode(unsigned char *& clear, P const res, int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    plonge(tmp,res);
    tmp.ntt_pow_phi();
    clear=(unsigned char *)malloc(size*sizeof(int));
    for(int i=0;i<res.degree&&i<size;i++) {
        clear[i]=tmp(0,i);
    }
}

template <class P>
static void decode_noalloc( char  * clear, P const res, int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    plonge(tmp,res);
    tmp.ntt_pow_phi();
    for(int i=0;i<res.degree&&i<size;i++) {
        clear[i]=tmp(0,i);
    }
}

template <class P>
static void encode_nontt(P& res, uint8_t* const bits,  int const size) {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    poly_cleartext tmp=0;
    for(int j=0;j<tmp.nmoduli;j++) {
        for(int i=0;i<tmp.degree&&i<size;i++) {
            tmp(j,i)=bits[i];
        }
    }
    plonge(res,tmp);
}

uint64_t cleartext_modulus() {
    using poly_cleartext = nfl::poly_from_modulus<uint16_t, 512, 14>;
    return poly_cleartext::get_modulus(0);
}

template <class T>
double get_time_us(T const& start, T const& end, uint32_t N)
{
    auto diff = end-start;
    return (long double)(std::chrono::duration_cast<std::chrono::microseconds>(diff).count())/N;
}

template <class T, size_t Align, class... Args>
T* alloc_aligned(size_t n, Args&& ... args)
{
    T* ret;
    if (posix_memalign((void**) &ret, Align, sizeof(T)*n) != 0) {
        throw std::bad_alloc();
    }
    for (size_t i = 0; i < n; i++) {
        new (&ret[i]) T{std::forward<Args>(args)...};
    }
    return ret;
}

template<size_t degree, size_t modulus, class T>
int testencode_decode () {
    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    uint8_t in[degree];
    fastrandombytes(in,degree);
    for(int i=0;i<degree;i++) {
        in[i]=(in[i])%2;
    }

    uint8_t  *out;
    poly_cyphertext c;
    encode(c,in,degree);
    decode(out, c,degree);
    for(int i=0;i<degree;i++) {
        assert(in[i] == out[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "Encode/Decode test passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testencrypt_decrypt () {
    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;
    poly_cyphertext &pka = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkaprime = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkb = *alloc_aligned<poly_cyphertext, 32>(1),
    &pkbprime = *alloc_aligned<poly_cyphertext, 32>(1),
    &a = *alloc_aligned<poly_cyphertext, 32>(1),
    &b = *alloc_aligned<poly_cyphertext, 32>(1),
    &d = *alloc_aligned<poly_cyphertext, 32>(1);

    // This step generates a secret key
    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);

    poly_cyphertext &s = *alloc_aligned<poly_cyphertext, 32>(1, nfl::gaussian<T>(&g_prng));
    poly_cyphertext &sprime = *alloc_aligned<poly_cyphertext, 32>(1);
    s.ntt_pow_phi();
    sprime = nfl::compute_shoup(s);

    // This step generates a public key
    pka = nfl::uniform();
    pkb = nfl::gaussian<T>(&g_prng, cleartext_modulus());
    pkb.ntt_pow_phi();

    // pkb = pkb + pka * s;
    pkb = pkb + nfl::shoup(pka * s, sprime);
    pkaprime = nfl::compute_shoup(pka);
    pkbprime = nfl::compute_shoup(pkb);

    uint8_t in[degree];
    fastrandombytes(in,degree);
    for(int i=0;i<degree;i++) {
        in[i]=(in[i])%2;
    }

    uint8_t  *out;
    poly_cyphertext c;
    encode(c,in,degree);

    encrypt(a, b, c, cleartext_modulus(), pka, pkb, pkaprime, pkbprime, &g_prng);
    decrypt(d, a, b, s, sprime, d.get_modulus(0),cleartext_modulus());

    decode(out, d,degree);
    for(int i=0;i<degree;i++) {
        assert(in[i] == out[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "Encrypt/Decrypt test passed" << std::endl;

}

// The PSI protocol is simple
// Remember it is an asymetric protocol where C learns the intersection of its set with S's set
// The protocol isn't secure against malicious participants, i.e. if C's set = Universe then Intersection(Universe,S's set)=S's set
// --> C's can learn S's set
// The protocol is secure with honest but curious (semi-honest) participants
// C calls initiateC with its set
// sends the resulting char buffer to S (which includes C's public key)
// S calls initiateS with its set and the received buffer
// sends back the resulting char buffer to C
// C calls terminateC with the received buffer
// and computes the intersection
// As classical with PSI, it is an asymetric protocol, i.e. C learns the intersection but not S
// if you want it to be symetrical, just do it also the other way around, i.e. C becomes S and vice versa (see testSymetricPSIAsLib as an example)

// initiateC is called by C to launch the protocol
template<size_t degree, size_t modulus, class T>
void initiateC(char*& outbuf,char* const inbuf,uint64_t  &outlen, uint64_t const inlen, nfl::poly_from_modulus<T, degree, modulus> *&SecretKey){

    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    // If size doesn't divide degree, we have to increase the inbuf size
    // and then recursively call initiateC (next time the buffer size is going to divide degree and then no recursion)
    if((inlen%degree!=0)) {
        char *new_inbuf=(char*)malloc(nbP*degree);
        memcpy(new_inbuf,inbuf,inlen);
        initiateC<degree,modulus,T>(outbuf,new_inbuf,outlen,nbP*degree,SecretKey);
    } else {

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // the input is in inbuf, has length inlen

    // Allocate a large enough buffer for :
    // the public keys (and their shoupified versions) and the two (times nbP) ciphers a and b
    outlen = (nbP*2+4)*degree*sizeof(T);
    outbuf=(char*)malloc(outlen);

    poly_cyphertext *pka = (poly_cyphertext*)outbuf,
                    *pkaprime = (poly_cyphertext*)(outbuf+degree*sizeof(T)),
                    *pkb = (poly_cyphertext*)(outbuf+2*degree*sizeof(T)),
                    *pkbprime = (poly_cyphertext*)(outbuf+3*degree*sizeof(T));

    // This step generates a secret key
    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);
    SecretKey = alloc_aligned<poly_cyphertext, 32>(1, nfl::gaussian<T>(&g_prng));
    SecretKey->ntt_pow_phi();

    // This step generates a public key
    // pka is just uniform
    *pka = nfl::uniform();
    *pkb = nfl::gaussian<T>(&g_prng, cleartext_modulus());
    pkb->ntt_pow_phi();
    // pkb = pkb + pka * s;
    *pkb = *pkb + *pka * *SecretKey;
    *pkaprime = nfl::compute_shoup(*pka);
    *pkbprime = nfl::compute_shoup(*pkb);

    // Then alloc a single polynom for encoding and encrypting in the loop
    poly_cyphertext p_in=*alloc_aligned<poly_cyphertext, 32>(1);

    // Then do the encoding and encrypting in the loop
    for(int i=0;i<nbP;i++) {
        encode(p_in,(unsigned char*)(inbuf+i*degree),degree);
        // encrypt (a[i]),b[i],p_in,...) but we are doing this directly in the output buffer
        encrypt(*((poly_cyphertext*)(outbuf+(4+i)*degree*sizeof(T))), *((poly_cyphertext*)(outbuf+(nbP+4+i)*degree*sizeof(T))), p_in, cleartext_modulus(), *pka, *pkb, *pkaprime, *pkbprime, &g_prng);
    }}
}

// initiateS is called by S upon receiving a request by C, that's the second step of the protocol
template<size_t degree, size_t modulus, class T>
void initiateS(char*& outbuf,char* const inbuf, char* Sinput, uint64_t &outlen, uint64_t const inlen){
    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // C sent its public keys in the input buffer (pka,pkb,pkaprime,pkbprime)
    poly_cyphertext *pka = (poly_cyphertext*)inbuf,
                    *pkaprime = (poly_cyphertext*)(inbuf+degree*sizeof(T)),
                    *pkb = (poly_cyphertext*)(inbuf+2*degree*sizeof(T)),
                    *pkbprime = (poly_cyphertext*)(inbuf+3*degree*sizeof(T)),
                    // and we are going to produce a cipher of a zero polynom
                    &p_zero = *alloc_aligned<poly_cyphertext, 32>(1),
                    &zeroa = *alloc_aligned<poly_cyphertext, 32>(1),
                    &zerob = *alloc_aligned<poly_cyphertext, 32>(1);

    FastGaussianNoise<uint8_t, T, 2> g_prng(8, 128, 1<<10);

    // We are going to encode S input set into p_inS
    poly_cyphertext p_inS=*alloc_aligned<poly_cyphertext, 32>(1);

    // The output buffer and its length
    outlen = (nbP*2)*degree*sizeof(T);
    outbuf=(char*)malloc(outlen);

    // A zero polynom that is going to be ciphered in the loop.
    // Please note that it could be sent by C and S would not need C's public key
    // But it is more secure to produce a new zero polynom for each addition
    // Henceforth by enabling S to produce them (by giving it S public key)
    // We save on communication, but not on computation
    char zero[degree];
    for(int i=0;i<degree;i++) {
        zero[i]=0;
    }
    encode(p_zero,(unsigned char*)zero,degree);

    for(int i=0;i<nbP;i++) {
        // S's set is encoded into p_inS and NTTified before mult
        encode(p_inS,(unsigned char*)(Sinput+i*degree),degree);
        p_inS.ntt_pow_phi();
        // We produce a new cipher of zero
        encrypt(zeroa, zerob, p_zero, cleartext_modulus(),*pka, *pkb, *pkaprime, *pkbprime, &g_prng);
        // and compute the intersection and add zero
        // a = a *p_ins + zero and b =b * p_ins +zero
        *((poly_cyphertext*)(outbuf+i*degree*sizeof(T))) = *((poly_cyphertext*)(inbuf+(4+i)*degree*sizeof(T))) * p_inS + zeroa;
        *((poly_cyphertext*)(outbuf+(i+nbP)*degree*sizeof(T))) = *((poly_cyphertext*)(inbuf+(4+i+nbP)*degree*sizeof(T))) * p_inS + zerob;
    }
}

// terminateC is called by C upon receiving a reply by S, that's the third and final step of the protocol
template<size_t degree, size_t modulus, class T>
void terminateC(char* const outbuf,char* const inbuf,uint64_t outlen, uint64_t const inlen, nfl::poly_from_modulus<T, degree, modulus> *SecretKey){
    // nbP is the number of polynoms we are going to create
    uint64_t nbP=(inlen/degree)+ ( (inlen%degree==0) ? 0 : 1 );

    using poly_cyphertext = nfl::poly_from_modulus<T, degree, modulus>;

    // Sprime is the shoupified secret key
    poly_cyphertext &sprime = *alloc_aligned<poly_cyphertext, 32>(1);
    sprime = nfl::compute_shoup(*SecretKey);

    // d is a polynom into which we are going to decrypt in the loop
    poly_cyphertext &d=*alloc_aligned<poly_cyphertext, 32>(1);

    // TODO: Do we really need to compute this apart from stats ?
    outlen=nbP*degree*sizeof(T);

    for(int i=0;i<nbP;i++) {
        // We decrypt S's reply into d (decrypt(d,a,b,...))
        decrypt(d, *(poly_cyphertext*)(inbuf+i*degree*sizeof(T)), *(poly_cyphertext*)(inbuf+(nbP+i)*degree*sizeof(T)), *SecretKey, sprime, d.get_modulus(0),cleartext_modulus());
        // And decode d into outbuf
        //  the length of the decode depends on whether or not the size of the universe fits into degree's polynoms
        if(i==nbP-1&&inlen%degree!=0)
            decode_noalloc((outbuf+i*degree*sizeof(char)), d ,inlen%degree);
        else
            decode_noalloc((outbuf+i*degree*sizeof(char)), d ,degree);
    }

}

static char* getRandomBinaryBytes(uint64_t size) {
    unsigned char *buff=(unsigned char*)malloc(size);
    fastrandombytes(buff,size);
    for(int i=0;i<size;i++) {
        buff[i]=(buff[i])%2;
    }
    return (char*)buff;
}

template<size_t degree, size_t modulus, class T>
int testPSI1(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    uint8_t Sset[size];
    for(int i=0;i<size;i++) {
        Sset[i]=1;
    }

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==Cset[i]);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI test1 passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSI0(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);

    uint8_t Sset[size];
    for(int i=0;i<size;i++) {
        Sset[i]=0;
    }

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==0);
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI test0 passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSIAND(uint64_t size)
{
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);

    char *Sout;
    uint64_t Soutlen;
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);

    char Intersection[size];
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==(Cset[i]&&Sset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI testAND passed" << std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size];

    start = std::chrono::steady_clock::now();

    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(Intersection[i]==(Cset[i]&&Sset[i]));
    }
    auto etime=get_time_us(start, end, 1);
    uint64_t bps=1000000*size/etime;
    uint64_t xfact=(Coutlen+Soutlen);
    xfact=(uint64_t)((float)xfact/(float)size);
    std::cout << "PSI : n=" << size << " \tin " << etime << " us \t\t" <<
                  bps << "bit/sec "<<
                  " \tand " << Coutlen + Soutlen << " bytes communication"<<
                  " \t expension factor = "<< xfact <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int testSymetricPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyAlice;
    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyBob;

    char *Aliceset=getRandomBinaryBytes(size);
    char *Bobset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char IntersectionAlice[size];
    char IntersectionBob[size];

    start = std::chrono::steady_clock::now();
    // Alice -> Bob
    initiateC<degree,modulus,T>(Cout,(char*)Aliceset,Coutlen,size,SecretKeyAlice);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Bobset,Soutlen,size);
    terminateC<degree,modulus,T>(IntersectionAlice,Sout,Soutlen,size,SecretKeyAlice);
    // Bob -> Alice
    initiateC<degree,modulus,T>(Cout,(char*)Bobset,Coutlen,size,SecretKeyBob);
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Aliceset,Soutlen,size);
    terminateC<degree,modulus,T>(IntersectionBob,Sout,Soutlen,size,SecretKeyBob);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(IntersectionBob[i]==(Aliceset[i]&&Bobset[i]));
        assert(IntersectionAlice[i]==(Aliceset[i]&&Bobset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "SPSI : n=" << size << " \tin " << get_time_us(start, end, 1) << " us \t\t" <<
                  1000000*size/get_time_us(start, end, 1) << "bit/sec "<<
                  " \tand " << (Coutlen + Soutlen)*2 << " bytes communication"<<
                  " \t expension factor = "<< (Coutlen+Soutlen)*2/size <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int test3PartiesPSIAsLib(uint64_t size)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    nfl::poly_from_modulus<T, degree, modulus> *SecretKeyAlice;

    char *Aliceset=getRandomBinaryBytes(size);
    char *Bobset=getRandomBinaryBytes(size);
    char *Charlieset=getRandomBinaryBytes(size);

    char *Aout;
    uint64_t Aoutlen;
    char *Bout;
    uint64_t Boutlen;
    char *Cout;
    uint64_t Coutlen;
    char IntersectionAlice[size];

    start = std::chrono::steady_clock::now();
    initiateC<degree,modulus,T>(Aout,(char*)Aliceset,Aoutlen,size,SecretKeyAlice);
    initiateS<degree,modulus,T>(Bout,Aout,(char*)Bobset,Boutlen,size);
    // terminateC<degree,modulus,T>(IntersectionAlice,Bout,Boutlen,size,SecretKeyAlice);
    // initiateC<degree,modulus,T>(Aout,IntersectionAlice,Aoutlen,size,SecretKeyAlice);
    //initiateS<degree,modulus,T>(Cout,Aout,(char*)Charlieset,Aoutlen,size);
    initiateS<degree,modulus,T>(Cout,Bout,(char*)Charlieset,Coutlen,size);
    terminateC<degree,modulus,T>(IntersectionAlice,Cout,Coutlen,size,SecretKeyAlice);
    end = std::chrono::steady_clock::now();

    for(int i=0;i<size;i++) {
        assert(IntersectionAlice[i]==(Aliceset[i]&&Bobset[i]&&Charlieset[i]));
    }

    std::cout << "======================================================================" << std::endl;
    std::cout << "3PSI : n=" << size << " \tin " << get_time_us(start, end, 1) << " us \t\t" <<
                  1000000*size/get_time_us(start, end, 1) << "bit/sec "<<
                  " \tand " << (Aoutlen + Boutlen + Coutlen) << " bytes communication"<<
                  " \t expension factor = "<< (Aoutlen + Boutlen + Coutlen)/size <<std::endl;

}

template<size_t degree, size_t modulus, class T>
int testPerfPSI(uint64_t size,uint64_t reps)
{

    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();
    uint64_t t1 = 0;
    uint64_t t2 = 0;
    uint64_t t3 = 0;

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;

    char *Cset=getRandomBinaryBytes(size);
    char *Sset=getRandomBinaryBytes(size);

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size];
    for(int i=0;i<reps;i++) {
    start = std::chrono::steady_clock::now();
    initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();
    t1=t1+(get_time_us(start, end, 1));
    start = std::chrono::steady_clock::now();
    initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
    end = std::chrono::steady_clock::now();
    t2=t2+(get_time_us(start, end, 1));
    start = std::chrono::steady_clock::now();
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
    end = std::chrono::steady_clock::now();
    t3=t3+(get_time_us(start, end, 1));
}

    std::cout << "======================================================================" << std::endl;
    std::cout << "PSI : n=" << size << " in " << (t1+t2+t3)/reps << " us " << (t1+t2+t3)/reps/size << "us/bit"<< std::endl;
    std::cout << "PSI : initC " << size << " in " << (t1)/reps << " us " << (100*t1/(t1+t2+t3)) <<"%"<< std::endl;
    std::cout << "PSI : initS " << size << " in " << (t2)/reps << " us " << (100*t2/(t1+t2+t3)) <<"%"<< std::endl;
    std::cout << "PSI : finaC " << size << " in " << (t3)/reps << " us " << (100*t3/(t1+t2+t3)) <<"%"<< std::endl;

}

void testBBF(size_t size) {
    unsigned char a[]="marco";size_t asize=5;
    unsigned char b[]="carlos";size_t bsize=6;
    unsigned char c[]="joris";size_t csize=5;
    BloomFilter BF(512,4);
    BF.insert(a,asize);
    BF.insert(b,bsize);
    bool res=BF.test(a,asize);
    assert(res==true);
    res=BF.test(b,bsize);
    assert(res==true);
    res=BF.test(c,csize);
    assert(res==false);
}

template<size_t degree, size_t modulus, class T>
void testPSIBF(size_t size) {
    /* L'idée est de :
      C crée un BF et y met son set
      C encode ce BF de 512 bytes vers un bitset de 512*8 bits
      puis intiateC et envoie à S
      S encode son BF contenant son set vers un bitset de 512*8 bits
      puis initiateS et envoie à C
      C terminateC dans un newBF
      pour chacun de ses éléments fait un newBF.test
    */

    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
    unsigned char a[]="marco";size_t asize=5;
    unsigned char b[]="carlos";size_t bsize=6;
    unsigned char c[]="joris";size_t csize=5;

    BloomFilter BF(size,4);
    BF.insert(a,asize);
    BF.insert(b,bsize);
    BF.insert(c,bsize);
    char* serC=BF.serialize();

    BloomFilter BFS(size,4);
    BFS.insert(a,asize);
    BFS.insert(b,bsize);
    char* serS=BFS.serialize();

    char *Cout;
    uint64_t Coutlen;
    char *Sout;
    uint64_t Soutlen;
    char Intersection[size*8];

    initiateC<degree,modulus,T>(Cout,serC,Coutlen,size*8,SecretKey);
    initiateS<degree,modulus,T>(Sout,Cout,serS,Soutlen,size*8);
    terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size*8,SecretKey);

    BloomFilter BFInter(size,4);

    BFInter.deserialize(Intersection);

    bool res=BFInter.test(a,asize);
    assert(res==true);
    res=BFInter.test(b,bsize);
    assert(res==true);
    res=BFInter.test(c,csize);
    assert(res==false);
}

// template<size_t degree, size_t modulus, class T>
// int testPerfPSIBloom(uint64_t size,uint64_t reps)
// {
//
//     auto start = std::chrono::steady_clock::now();
//     auto end = std::chrono::steady_clock::now();
//  uint64_t t1 = 0;
//  uint64_t t2 = 0;
//  uint64_t t3 = 0;
//
//  nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
//
//  BloomFilter Cset(512,4);
//  char *Sset=getRandomBinaryBytes(size);
//
//  char *Cout;
//  uint64_t Coutlen;
//  char *Sout;
//  uint64_t Soutlen;
//  char Intersection[size];
//  for(int i=0;i<reps;i++) {
//     start = std::chrono::steady_clock::now();
//  initiateC<degree,modulus,T>(Cout,(char*)Cset,Coutlen,size,SecretKey);
//     end = std::chrono::steady_clock::now();
//  t1=t1+(get_time_us(start, end, 1));
//     start = std::chrono::steady_clock::now();
//  initiateS<degree,modulus,T>(Sout,Cout,(char*)Sset,Soutlen,size);
//     end = std::chrono::steady_clock::now();
//  t2=t2+(get_time_us(start, end, 1));
//     start = std::chrono::steady_clock::now();
//  terminateC<degree,modulus,T>(Intersection,Sout,Soutlen,size,SecretKey);
//     end = std::chrono::steady_clock::now();
//  t3=t3+(get_time_us(start, end, 1));
// }
//
//  std::cout << "======================================================================" << std::endl;
//  std::cout << "PSI : n=" << size << " in " << (t1+t2+t3)/reps << " us " << std::endl;
//  std::cout << "PSI : initC " << size << " in " << (t1)/reps << " us " << (100*t1/(t1+t2+t3)) <<"%"<< std::endl;
//  std::cout << "PSI : initS " << size << " in " << (t2)/reps << " us " << (100*t2/(t1+t2+t3)) <<"%"<< std::endl;
//  std::cout << "PSI : finaC " << size << " in " << (t3)/reps << " us " << (100*t3/(t1+t2+t3)) <<"%"<< std::endl;
//
//
// }

template<size_t degree, size_t modulus, class T>
int run()
{

    // testencode_decode<degree,modulus,T>();
    // testencrypt_decrypt<degree,modulus,T>();
    // testPSI1<degree,modulus,T>(1024);
    // testPSI0<degree,modulus,T>(1024);
    // testPSIAND<degree,modulus,T>(1024);
    // for(int i=512;i<2*512;i+=1)
    //   testPSIAsLib<degree,modulus,T>(i);
    // testPSIAsLib<degree,modulus,T>(2*512);
    // testPSIAsLib<degree,modulus,T>(10*512);
    // testPSIAsLib<degree,modulus,T>(100*512);
    // testPSIAsLib<degree,modulus,T>(1000*512);
    // testPSIAsLib<degree,modulus,T>(2000*512);
    // testPSIAsLib<degree,modulus,T>(2*2000*512);
    //testPSIAsLib<degree,modulus,T>(3*2000*512);
    //testPSIAsLib<degree,modulus,T>(512*512);
    //testSymetricPSIAsLib<degree,modulus,T>(2000*512);
    testPerfPSI<degree,modulus,T>(100000,10);
    testPerfPSI<degree,modulus,T>(10000,10);
    testPerfPSI<degree,modulus,T>(1000,100);
    testPerfPSI<degree,modulus,T>(100,100);
    //test3PartiesPSIAsLib<degree,modulus,T>(512*512);

    // testHash<degree,modulus,T>(512,512);
    // testHash<degree,modulus,T>(1024,512);
    // testHash<degree,modulus,T>(2048,512);
    // testHash<degree,modulus,T>(4096,512);

    testBBF(512);
    std::cout<<"---"<<std::endl;
    testPSIBF<degree,modulus,T>(512);

    return 0;
}

int main() {
    return run<CONFIG>();
}


#include <gtest/gtest.h>
#include <boost/math/quaternion.hpp>

#include "test_util.h"

#include "AlignToCube.hpp"
#include "SwarmieSensors.hpp"
#include "SwarmieInterface.hpp"
#include "Tag.hpp"

class AlignToCubeTest : public testing::Test
{
protected:
   SwarmieSensors sensors;
   AlignToCube align;
   boost::math::quaternion<double> defaultOrientation;

   AlignToCubeTest() : defaultOrientation(1.2, 1.2, 1.2, 2.1) {
      align.Update(sensors, SwarmieAction());
   }
};

TEST_F(AlignToCubeTest, noCubeNoMovement) {
   SwarmieAction a = align.GetAction();
   EXPECT_FALSE(is_moving(a));
}

TEST_F(AlignToCubeTest, nonCubeTagNoMovement) {
   Tag t = tag_top_left(Tag::NEST_TAG_ID);
   sensors.DetectedTag(t);
   align.Update(sensors, SwarmieAction());
   SwarmieAction a = align.GetAction();
   EXPECT_FALSE(is_moving(a));
}

TEST_F(AlignToCubeTest, cubeOnLeftTurnLeft) {
   sensors.DetectedTag(tag_top_left(Tag::CUBE_TAG_ID));
   align.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_turning_left(align.GetAction())) << "Yaw: " << align.GetAction().GetVelocity().GetYaw();

   sensors.ClearDetections();
   align.Update(sensors, SwarmieAction());
   EXPECT_FALSE(is_moving(align.GetAction()));

   sensors.DetectedTag(tag_bottom_left(Tag::CUBE_TAG_ID));
   align.Update(sensors, SwarmieAction());
   for(int i = 0; i < 50 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_turning_left(align.GetAction())) << "Yaw: " << align.GetAction().GetVelocity().GetYaw();
}

TEST_F(AlignToCubeTest, cubeOnRightTurnRight) {
   sensors.DetectedTag(tag_top_right(Tag::CUBE_TAG_ID));
   align.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_turning_right(align.GetAction()));

   sensors.ClearDetections();
   align.Update(sensors, SwarmieAction());
   EXPECT_FALSE(is_moving(align.GetAction()));

   sensors.DetectedTag(tag_bottom_right(Tag::CUBE_TAG_ID));
   align.Update(sensors, SwarmieAction());
   // accumulate integral term in the PI controller
   for(int i = 0; i < 30 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_turning_right(align.GetAction()));
}

TEST_F(AlignToCubeTest, nestAndCubeVisible) {
   sensors.DetectedTag(tag_top_left(Tag::CUBE_TAG_ID));
   sensors.DetectedTag(tag_bottom_left(Tag::NEST_TAG_ID));
   align.Update(sensors, SwarmieAction());
   EXPECT_FALSE(is_moving(align.GetAction()));
}

TEST_F(AlignToCubeTest, movementStopsWhenTagVanishes)
{
   sensors.DetectedTag(tag_top_left(Tag::CUBE_TAG_ID));
   align.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_TRUE(is_moving(align.GetAction()));
   sensors.ClearDetections();
   // Update many times to test that the integral term is no longer
   // having an effect.
   for(int i = 0; i < 1000; i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(align.GetAction()));
}

TEST_F(AlignToCubeTest, noMovementWhenAligned)
{
   sensors.DetectedTag(Tag(Tag::CUBE_TAG_ID, -0.023, -0.12, 0.5, defaultOrientation));
   align.Update(sensors, SwarmieAction());
   for(int i = 0; i < 30 && !is_moving(align.GetAction()); i++) {
      align.Update(sensors, SwarmieAction());
   }
   EXPECT_FALSE(is_moving(align.GetAction()));
}


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::record::random_generator_v0.hpp                   //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_GENERATE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_GENERATE_HPP_ER_2009
#include <algorithm>
#include <iterator>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>
#include <boost/statistics/detail/distribution/survival/record/random/generator_v0.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace record{

 	// sample records given (x,model)
	template<typename It,typename N,typename U,typename M,typename X>
    It generate_n(It it_ft,const N& n,U& urng,const M& model,const X& x)
    {
    	typedef typename distribution::meta::value<M>::type val_;
        typedef record::meta::random_generator<0,val_> meta_gen_;
        typedef typename meta_gen_::type gen_;
        gen_ gen = meta_gen_::call(x, model, urng);
        return std::generate_n( it_ft, n, gen );
    }

}// record
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Detail/Cartesian.h"
#include "Molassembler/Temple/Random.h"
#include "Molassembler/Options.h"

#include <Eigen/Geometry>

#include <cmath>

BOOST_AUTO_TEST_CASE(DihedralTests, *boost::unit_test::label("Molassembler")) {
  using namespace Scine;
  using namespace Molassembler;

  Scine::Utils::PositionCollection positions(4, 3);
  positions.row(0) = Scine::Utils::Position {1, 0, 0};
  positions.row(1) = Scine::Utils::Position {0, 0, 0};
  positions.row(2) = Scine::Utils::Position {0, 1, 0};

  const Eigen::Vector3d lastPosition {1, 1, 0};

  positions.row(3) = lastPosition;

  for(
    const double randomAngle :
    Temple::Random::getN<double>(
      -M_PI + 0.01,
      M_PI - 0.01,
      100,
      Scine::Molassembler::randomnessEngine()
    )
  ) {

    positions.row(3) = Eigen::AngleAxisd(
      randomAngle,
      Eigen::Vector3d::UnitY()
    ) * lastPosition;

    const double forwardDihedral = Cartesian::dihedral(
      positions.row(0),
      positions.row(1),
      positions.row(2),
      positions.row(3)
    );

    BOOST_CHECK_MESSAGE(
       std::fabs(forwardDihedral - randomAngle) < 1e-10,
      "Twist angle: " << randomAngle << ", reported angle: " << forwardDihedral
    );

    const double reverseDihedral = Cartesian::dihedral(
      positions.row(3),
      positions.row(2),
      positions.row(1),
      positions.row(0)
    );

    BOOST_CHECK_MESSAGE(
      std::fabs(forwardDihedral - reverseDihedral) < 1e-10,
      "Dihedral of reverse sequence is not identical: " << forwardDihedral
        << ", " << reverseDihedral
    );
  }
}


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

/**
 * @file
 */

#include <nuklei/RegionOfInterest.h>
#include <nuklei/KernelCollection.h>
#include <sstream>
#include <boost/assign/std/vector.hpp>

#ifdef NUKLEI_USE_CGAL

#define CGAL_EIGEN3_ENABLED

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangle_primitive.h>
#include <CGAL/AABB_polyhedron_triangle_primitive.h>
#include <CGAL/property_map.h>
#include <CGAL/IO/read_off_points.h>
#include <CGAL/IO/read_xyz_points.h>
#include <CGAL/IO/write_xyz_points.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/squared_distance_3.h>
#include <CGAL/Point_with_normal_3.h>

#endif

namespace nuklei
{
#ifdef NUKLEI_USE_CGAL
  typedef CGAL::Simple_cartesian<double> K;
  typedef K::FT FT;
  typedef K::Ray_3 Ray;
  typedef K::Line_3 Line;
  typedef K::Point_3 Point;
  typedef K::Plane_3 Plane;
  typedef K::Vector_3 Vector;
  typedef K::Segment_3 Segment;
  typedef K::Triangle_3 Triangle;
  typedef std::list<Triangle>::iterator Iterator;
  typedef CGAL::AABB_triangle_primitive<K, Iterator> Primitive;
  typedef CGAL::AABB_traits<K, Primitive> AABB_triangle_traits;
  typedef CGAL::AABB_tree<AABB_triangle_traits> Tree;
#if CGAL_VERSION_NR >= 1040300000
  typedef boost::optional< Tree::Intersection_and_primitive_id<Plane>::Type > Plane_intersection;
#else
  typedef boost::optional< Tree::Object_and_primitive_id > Plane_intersection;
#endif
#endif

  SphereROI::SphereROI(const std::string &centerAndRadius)
  {
    std::istringstream is(centerAndRadius);
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> center_[i]);
    NUKLEI_ASSERT(is >> radius_);
    NUKLEI_ASSERT(radius_ >= 0);
  }

  BoxROI::BoxROI(const std::string &s)
  {
    setCenterOriSize(s);
  }

  void BoxROI::setCenterAxesSize(const std::string &centerSize)
  {
    std::istringstream is(centerSize);
    Vector3 center, axis1, axis2;
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> center[i]);
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> axis1[i]);
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> axis2[i]);
    NUKLEI_ASSERT(is >> edgeLengths_[2]);

    axis1 -= center; axis2 -= center;
    edgeLengths_[0] = axis1.Length()*2*1.2;
    edgeLengths_[1] = axis2.Length()*2*1.2;
    edgeLengths_[2] *= 2*1.0;
    axis1.Normalize(); axis2.Normalize();

    Matrix3 ori;
    ori.SetColumn(0, axis1);
    ori.SetColumn(1, axis2);
    ori.SetColumn(2, la::normalized(axis1.Cross(axis2)));
    ori.Orthonormalize();

    centerLoc_ = center;
    la::copyRotation(centerOri_, ori);
  }

  void BoxROI::setCenterOriSize(const std::string &centerSize)
  {
    std::istringstream is(centerSize);
    Quaternion ori;
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> centerLoc_[i]);
    for (unsigned int i = 0; i < 4; ++i)
      NUKLEI_ASSERT(is >> ori[i]);
    for (unsigned int i = 0; i < 3; ++i)
      NUKLEI_ASSERT(is >> edgeLengths_[i]);
    la::copyRotation(centerOri_, ori);
  }

  bool SphereROI::contains_(const Vector3 &v) const
  {
    return dist<groupS::r3>::d(v, center_) <= radius_;
  }

  bool BoxROI::contains_(const Vector3 &v) const
  {
    Vector3 projection = la::project(centerLoc_, centerOri_, v);

    return std::fabs(projection[0]) <= edgeLengths_[0]/2. &&
      std::fabs(projection[1]) <= edgeLengths_[1]/2. &&
      std::fabs(projection[2]) <= edgeLengths_[2]/2.;
  }

  struct BoxFace
  {
  public:
    BoxFace(const int a,
            const int b,
            const int c,
            const int d) :
    a(a), b(b), c(c), d(d)
    {

    }

    int a, b, c, d;
  };

  BoxFace make_boxface(const int a,
                       const int b,
                       const int c,
                       const int d)
  {
    return BoxFace(a, b, c, d);
  }

  void BoxROI::pushTriangles_(std::list<boost::any>& triangles) const
  {
#ifdef NUKLEI_USE_CGAL
    KernelCollection corners;
    {
      kernel::r3 k;
      k.loc_ = Vector3(edgeLengths_.X()/2 , + edgeLengths_.Y()/2 , + edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(edgeLengths_.X()/2 , + edgeLengths_.Y()/2 , - edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(edgeLengths_.X()/2 , - edgeLengths_.Y()/2 , + edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(edgeLengths_.X()/2 , - edgeLengths_.Y()/2 , - edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(- edgeLengths_.X()/2 , + edgeLengths_.Y()/2 , + edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(- edgeLengths_.X()/2 , + edgeLengths_.Y()/2 , - edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(- edgeLengths_.X()/2 , - edgeLengths_.Y()/2 , + edgeLengths_.Z()/2);
      corners.add(k);
    }
    {
      kernel::r3 k;
      k.loc_ = Vector3(- edgeLengths_.X()/2 , - edgeLengths_.Y()/2 , - edgeLengths_.Z()/2);
      corners.add(k);
    }

    corners.transformWith(centerLoc_, centerOri_);

    std::vector<BoxFace> faces;
    {
      using namespace boost::assign;
      faces += make_boxface(0, 1, 3, 2),
      make_boxface(2, 3, 7, 6),
      make_boxface(6, 7, 5, 4),
      make_boxface(4, 5, 1, 0),
      make_boxface(0, 1, 3, 2),
      make_boxface(0, 2, 6, 4),
      make_boxface(1, 3, 7, 5);
    }

    for (std::vector<BoxFace>::const_iterator i = faces.begin();
         i != faces.end(); ++i)
    {
      const Vector3& aa = corners.at(i->a).getLoc();
      const Vector3& bb = corners.at(i->b).getLoc();
      const Vector3& cc = corners.at(i->c).getLoc();
      const Vector3& dd = corners.at(i->d).getLoc();

      Point a(aa.X(), aa.Y(), aa.Z());
      Point b(bb.X(), bb.Y(), bb.Z());
      Point c(cc.X(), cc.Y(), cc.Z());
      Point d(dd.X(), dd.Y(), dd.Z());

      triangles.push_back(Triangle(a,b,c));
      triangles.push_back(Triangle(a,d,c));
    }
#else
    NUKLEI_THROW("This function requires CGAL. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
  }

  void BoxROI::buildAABBTree_()
  {
#ifdef NUKLEI_USE_CGAL
    std::list<Triangle> triangles;
    std::list<boost::any> t;
    pushTriangles(t);
    for (std::list<boost::any>::const_iterator i = t.begin(); i != t.end(); ++i)
    {
      triangles.push_back(boost::any_cast<Triangle>(*i));
    }

    // constructs AABB tree
    boost::shared_ptr<Tree> tree(new Tree(triangles.begin(),triangles.end()));
    tree_ = tree;
#else
    NUKLEI_THROW("This function requires CGAL. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
  }

#ifdef NUKLEI_USE_CGAL
  bool intersects(Plane p, const boost::any& t)
  {
    NUKLEI_ASSERT(!p.is_degenerate());
    NUKLEI_ASSERT(!t.empty());
    boost::shared_ptr<Tree> tree = boost::any_cast< boost::shared_ptr<Tree> >(t);
    Plane_intersection plane_intersection = tree->any_intersection(p);
    return plane_intersection;
  }
#endif

  bool BoxROI::intersectsPlane(const Vector3& p, const Vector3& q, const Vector3& r) const
  {
#ifdef NUKLEI_USE_CGAL
    Point pp(p.X(), p.Y(), p.Z());
    Point qq(q.X(), q.Y(), q.Z());
    Point rr(r.X(), r.Y(), r.Z());
    Plane plane_query(pp, qq, rr);
    return intersects(plane_query, tree_);
#else
    NUKLEI_THROW("This function requires CGAL. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
  }

  bool BoxROI::intersectsPlane(const Vector3& p, const Vector3& v) const
  {
#ifdef NUKLEI_USE_CGAL
    Vector vec(v.X(), v.Y(), v.Z());
    Point a(p.X(), p.Y(), p.Z());
    Plane plane_query(a,vec);
    return intersects(plane_query, tree_);
#else
    NUKLEI_THROW("This function requires CGAL. See http://nuklei.sourceforge.net/doxygen/group__install.html");
#endif
  }

}


#include "SPlisHSPlasH/Common.h"
#include "GL/glew.h"
#include "Visualization/MiniGL.h"
#include "GL/glut.h"
#include "SPlisHSPlasH/TimeManager.h"
#include <Eigen/Dense>
#include <iostream>
#include "SPlisHSPlasH/Utilities/Timing.h"
#include "Utilities/PartioReaderWriter.h"
#include "PositionBasedDynamicsWrapper/PBDRigidBody.h"
#include "Utilities/OBJLoader.h"
#include "SPlisHSPlasH/Utilities/PoissonDiskSampling.h"
#include "PositionBasedDynamicsWrapper/PBDWrapper.h"
#include "Demos/Common/DemoBase.h"
#include "Utilities/FileSystem.h"

// Enable memory leak detection
#ifdef _DEBUG
#ifndef EIGEN_ALIGN
#define new DEBUG_NEW
#endif
#endif

using namespace SPH;
using namespace Eigen;
using namespace std;

void timeStep();
void initBoundaryData();
void render();
void renderBoundary();
void reset();
void updateBoundaryParticles(const bool forceUpdate);
void updateBoundaryForces();
void simulationMethodChanged();

DemoBase   base;
PBDWrapper pbdWrapper;

// main
int main(int argc, char** argv)
{
    REPORT_MEMORY_LEAKS;

    base.init(argc, argv, "DynamicBoundaryDemo");

    //////////////////////////////////////////////////////////////////////////
    // PBD
    //////////////////////////////////////////////////////////////////////////
    pbdWrapper.initShader();
    pbdWrapper.readScene(base.getSceneFile());

    initBoundaryData();
    base.buildModel();
    base.setSimulationMethodChangedFct(simulationMethodChanged);
    pbdWrapper.initGUI();

    pbdWrapper.initModel(TimeManager::getCurrent()->getTimeStepSize());

    MiniGL::setClientIdleFunc(50, timeStep);
    MiniGL::setKeyFunc(0, 'r', reset);
    MiniGL::setClientSceneFunc(render);

    glutMainLoop();

    base.cleanup();

    Timing::printAverageTimes();
    Timing::printTimeSums();

    return 0;
}

void reset()
{
    Timing::printAverageTimes();
    Timing::reset();

    //////////////////////////////////////////////////////////////////////////
    // PBD
    //////////////////////////////////////////////////////////////////////////
    pbdWrapper.reset();

    updateBoundaryParticles(true);

    base.getSimulationMethod().simulation->reset();
    TimeManager::getCurrent()->setTime(0.0);
}

void timeStep()
{
    if((base.getPauseAt() > 0.0) && (base.getPauseAt() < TimeManager::getCurrent()->getTime()))
        base.setPause(true);

    if(base.getPause())
        return;

    // Simulation code
    for(unsigned int i = 0; i < base.getNumberOfStepsPerRenderUpdate(); i++)
    {
        START_TIMING("SimStep");
        base.getSimulationMethod().simulation->step();
        STOP_TIMING_AVG;

        // if fluid is stabilizing, then don't update solid
        if(TimeManager::getCurrent()->getTime() < base.getScene().stabilizingTime)
            continue;

        updateBoundaryForces();

        //////////////////////////////////////////////////////////////////////////
        // PBD
        //////////////////////////////////////////////////////////////////////////
        START_TIMING("SimStep - PBD");
        pbdWrapper.timeStep();
        STOP_TIMING_AVG;

        updateBoundaryParticles(false);

        int savedFluidFrame = base.getSimulationMethod().model.writeFrameFluidData(TimeManager::getCurrent()->getTime());

        static std::vector < Vector3r > vertices;
        static std::vector < Vector3r > normals;

        if(savedFluidFrame > 0)
        {
            auto scene = base.getScene();
            PBD::SimulationModel&                  model = pbdWrapper.getSimulationModel();
            PBD::SimulationModel::RigidBodyVector& rb    = model.getRigidBodies();
            base.m_MeshWriter->reset_buffer();
            base.m_MeshWriter->getBuffer().push_back(static_cast < unsigned int > (rb.size() - 1));

            for(size_t i = 0; i < rb.size(); i++)
            {
                if(!scene.boundaryModels[i]->isWall)
                {
                    auto vertexData = rb[i]->getGeometry().getVertexData();
                    auto mesh       = rb[i]->getGeometry().getMesh();

                    auto nFaces            = mesh.numFaces();
                    auto faces             = mesh.getFaces();
                    auto faceVertices      = vertexData.getVertices();
                    auto faceVertexNormals = mesh.getVertexNormals();

                    vertices.resize(0);
                    vertices.reserve(nFaces * 3);
                    normals.resize(0);
                    normals.reserve(nFaces * 3);

                    for(unsigned int i = 0; i < nFaces; ++i)
                    {
                        for(unsigned int j = 0; j < 3; ++j)
                        {
                            unsigned int v_index = faces[i * 3 + j];
                            vertices.push_back((*faceVertices)[v_index]);
                            normals.push_back(faceVertexNormals[v_index]);
                        }
                    }

                    base.m_MeshWriter->getBuffer().push_back(static_cast < unsigned int > (vertices.size()));
                    base.m_MeshWriter->getBuffer().push_back_to_float_array(vertices, false);
                    base.m_MeshWriter->getBuffer().push_back_to_float_array(normals, false);
                }
            }

            base.m_MeshWriter->flush_buffer_async(savedFluidFrame);
        }
    }
}

void simulationMethodChanged()
{
    pbdWrapper.initGUI();
}

void renderBoundary()
{
    DemoBase::SimulationMethod& simulationMethod = base.getSimulationMethod();
    Shader&                     shader           = base.getShader();
    Shader&                     meshShader       = base.getMeshShader();
    SceneLoader::Scene&         scene            = base.getScene();
    const int                   renderWalls      = base.getRenderWalls();

    float                       wallColor[4] = { 0.1f, 0.6f, 0.6f, 1.0f };
    if((renderWalls == 1) || (renderWalls == 2))
    {
        if(MiniGL::checkOpenGLVersion(3, 3))
        {
            shader.begin();
            glUniform3fv(shader.getUniform("color"), 1, &wallColor[0]);
            glEnableVertexAttribArray(0);
            for(int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
            {
                if((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
                {
                    FluidModel::RigidBodyParticleObject* rb = simulationMethod.model.getRigidBodyParticleObject(body);
                    glVertexAttribPointer(0, 3, GL_DOUBLE, GL_FALSE, 0, &simulationMethod.model.getPosition(body + 1, 0));
                    glDrawArrays(GL_POINTS, 0, rb->numberOfParticles());
                }
            }
            glDisableVertexAttribArray(0);
            shader.end();
        }
        else
        {
            glDisable(GL_LIGHTING);
            glPointSize(4.0f);

            glBegin(GL_POINTS);
            for(int body = simulationMethod.model.numberOfRigidBodyParticleObjects() - 1; body >= 0; body--)
            {
                if((renderWalls == 1) || (!scene.boundaryModels[body]->isWall))
                {
                    FluidModel::RigidBodyParticleObject* rb = simulationMethod.model.getRigidBodyParticleObject(body);
                    for(unsigned int i = 0; i < rb->numberOfParticles(); i++)
                    {
                        glColor3fv(wallColor);
                        glVertex3v(&simulationMethod.model.getPosition(body + 1, i)[0]);
                    }
                }
            }
            glEnd();
            glEnable(GL_LIGHTING);
        }
    }
}

void render()
{
    MiniGL::coordinateSystem();

    base.renderFluid();
    renderBoundary();

    //////////////////////////////////////////////////////////////////////////
    // PBD
    //////////////////////////////////////////////////////////////////////////

    PBD::SimulationModel&                  model = pbdWrapper.getSimulationModel();
    PBD::SimulationModel::RigidBodyVector& rb    = model.getRigidBodies();

    const int                              renderWalls = base.getRenderWalls();
    SceneLoader::Scene&                    scene       = base.getScene();
    if((renderWalls == 3) || (renderWalls == 4))
    {
        for(size_t i = 0; i < rb.size(); i++)
        {
            const PBD::VertexData&      vd   = rb[i]->getGeometry().getVertexData();
            const PBD::IndexedFaceMesh& mesh = rb[i]->getGeometry().getMesh();
            if((renderWalls == 3) || (!scene.boundaryModels[i]->isWall))
            {
                float* col = &scene.boundaryModels[i]->color[0];
                if(!scene.boundaryModels[i]->isWall)
                {
                    base.meshShaderBegin(col);
                    pbdWrapper.drawMesh(vd, mesh, 0, col);
                    base.meshShaderEnd();
                }
                else
                {
                    base.meshShaderBegin(col);
                    pbdWrapper.drawMesh(vd, mesh, 0, col);
                    base.meshShaderEnd();
                }
            }
        }
    }

    pbdWrapper.renderTriangleModels();
    pbdWrapper.renderTetModels();
    pbdWrapper.renderConstraints();
    pbdWrapper.renderBVH();
}

void initBoundaryData()
{
    std::string         base_path = FileSystem::getFilePath(base.getSceneFile());
    SceneLoader::Scene& scene     = base.getScene();
    const bool          useCache  = base.getUseParticleCaching();

    for(unsigned int i = 0; i < scene.boundaryModels.size(); i++)
    {
        std::vector < Vector3r > boundaryParticles;
        if(scene.boundaryModels[i]->samplesFile != "")
        {
            string particleFileName = base_path + "/" + scene.boundaryModels[i]->samplesFile;
            PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), scene.boundaryModels[i]->scale[0], boundaryParticles);
        }

        // Cache sampling
        std::string                            mesh_base_path   = FileSystem::getFilePath(scene.boundaryModels[i]->meshFile);
        std::string                            mesh_file_name   = FileSystem::getFileName(scene.boundaryModels[i]->meshFile);
        std::string                            scene_path       = FileSystem::getFilePath(base.getSceneFile());
        std::string                            scene_file_name  = FileSystem::getFileName(base.getSceneFile());
        string                                 cachePath        = scene_path + "/" + mesh_base_path + "/Cache";
        string                                 particleFileName = FileSystem::normalizePath(cachePath + "/" + scene_file_name + "_" + mesh_file_name + "_" + std::to_string(i) + ".bgeo");

        PBD::SimulationModel&                  model       = pbdWrapper.getSimulationModel();
        PBD::SimulationModel::RigidBodyVector& rigidBodies = model.getRigidBodies();
        PBDRigidBody*                          rb          = new PBDRigidBody(rigidBodies[i]);
        PBD::RigidBodyGeometry&                geo         = rigidBodies[i]->getGeometry();
        PBD::IndexedFaceMesh&                  mesh        = geo.getMesh();
        PBD::VertexData&                       vd          = geo.getVertexData();

        if(scene.boundaryModels[i]->samplesFile == "")
        {
            bool foundCacheFile = false;
            if(useCache)
            {
                foundCacheFile = PartioReaderWriter::readParticles(particleFileName, Vector3r::Zero(), Matrix3r::Identity(), 1.0, boundaryParticles);
                if(foundCacheFile)
                    std::cout << "Loaded cached boundary sampling: " << particleFileName << "\n";
            }

            if(!useCache || !foundCacheFile)
            {
                std::cout << "Surface sampling of " << scene.boundaryModels[i]->meshFile << "\n";
                START_TIMING("Poisson disk sampling");
                PoissonDiskSampling sampling;
                sampling.sampleMesh(mesh.numVertices(), &vd.getPosition(0), mesh.numFaces(), mesh.getFaces().data(), scene.particleRadius, 10, 1, boundaryParticles);
                STOP_TIMING_AVG;

                // Cache sampling
                if(useCache && (FileSystem::makeDir(cachePath) == 0))
                {
                    std::cout << "Save particle sampling: " << particleFileName << "\n";
                    PartioReaderWriter::writeParticles(particleFileName, (unsigned int)boundaryParticles.size(), boundaryParticles.data(), NULL, scene.particleRadius);
                }
            }
            // transform back to local coordinates
            for(unsigned int j = 0; j < boundaryParticles.size(); j++)
                boundaryParticles[j] = rb->getRotation().transpose() * (boundaryParticles[j] - rb->getPosition());
        }
        base.getSimulationMethod().model.addRigidBodyObject(rb, static_cast < unsigned int > (boundaryParticles.size()), &boundaryParticles[0]);
    }
    updateBoundaryParticles(true);
}

void updateBoundaryParticles(const bool forceUpdate = false)
{
    SceneLoader::Scene& scene    = base.getScene();
    const unsigned int  nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
    for(unsigned int i = 0; i < nObjects; i++)
    {
        FluidModel::RigidBodyParticleObject* rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
        RigidBodyObject*                     rbo  = rbpo->m_rigidBody;
        if(rbo->isDynamic() || forceUpdate)
        {
#pragma omp parallel default(shared)
            {
#pragma omp for schedule(static)
                for(int j = 0; j < (int)rbpo->numberOfParticles(); j++)
                {
                    rbpo->m_x[j] = rbo->getRotation() * rbpo->m_x0[j] + rbo->getPosition();
                    rbpo->m_v[j] = rbo->getAngularVelocity().cross(rbpo->m_x[j] - rbo->getPosition()) + rbo->getVelocity();
                }
            }
        }
    }
}

void updateBoundaryForces()
{
    Real                h        = TimeManager::getCurrent()->getTimeStepSize();
    SceneLoader::Scene& scene    = base.getScene();
    const unsigned int  nObjects = base.getSimulationMethod().model.numberOfRigidBodyParticleObjects();
    for(unsigned int i = 0; i < nObjects; i++)
    {
        FluidModel::RigidBodyParticleObject* rbpo = base.getSimulationMethod().model.getRigidBodyParticleObject(i);
        RigidBodyObject*                     rbo  = rbpo->m_rigidBody;
        if(rbo->isDynamic())
        {
            ((PBDRigidBody*)rbo)->updateTimeStepSize();
            Vector3r force, torque;
            force.setZero();
            torque.setZero();

            for(int j = 0; j < (int)rbpo->numberOfParticles(); j++)
            {
                force  += rbpo->m_f[j];
                torque += (rbpo->m_x[j] - rbo->getPosition()).cross(rbpo->m_f[j]);
                rbpo->m_f[j].setZero();
            }
            rbo->addForce(force);
            rbo->addTorque(torque);
        }
    }
}


#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
#include "exodusII.h"
#include "share_header.hpp"
#include "mesh_Generated_multi_faults.hpp"
#include "Meshrectangular.hpp"
using namespace Eigen;
using namespace std;

int main()
{
    // Domain Size
    double x_min = -20e3;
    double x_max = 20e3;
    double y_min = -4.0e3;
    double y_max = 4.0e3;
    int dim = 2.0;
    double dx = 100;
    double dy = 100;
    int nx = (x_max-x_min)/dx;
    int ny = (y_max-y_min)/dy;
    MatrixXd Node = MatrixXd::Zero((nx+1)*(ny+1),2);
    MatrixXi_rm Element(nx*ny,4); Element.setZero();
    ArrayXi BIE_top_surf_nodes = ArrayXi::Zero((nx+1),1);
    ArrayXi BIE_bot_surf_nodes = ArrayXi::Zero((nx+1),1);
    int num_faults = 3;
    // Mesh
    VectorXd fault_pos_y(num_faults);
    fault_pos_y << 0.0 ,1.0e3, -1.0e3;
    std::vector<std::vector<int>> fault_nodes(num_faults*2);
  //  Meshrectangular(x_min, x_max, y_min, y_max, dx, dy, nx, ny, Node, Element);
    mesh_Generated_multi_faults(x_min,x_max,y_min,y_max,dx,dy,nx,ny,Node, Element,BIE_top_surf_nodes, BIE_bot_surf_nodes,fault_pos_y, fault_nodes);

    int exoid, num_dim, num_nodes, num_elem, num_elem_blk;
    int num_elem_in_block[10], num_nodes_per_elem[10];
    int num_node_sets, num_side_sets, error;
    int ebids[10];
    int CPU_word_size,IO_word_size;
   // float x[100], y[100], z[100], *dummy;
    double *dummy;
    dummy = 0; /* assign this so the Cray compiler doesn’t complain */
    /* Specify compute and i/o word size */
    CPU_word_size = 8;/* float or double */
    IO_word_size = 8;/* use system default (4 bytes) */
    /* create EXODUS II file */
    exoid = ex_create ("test.exo",/* filename path */
                       EX_CLOBBER,/* create mode */
                       &CPU_word_size,/* CPU float word size in bytes */
                       &IO_word_size);/* I/O float word size in bytes */
    /* ncopts = NC_VERBOSE; */
    /* initialize file with parameters */
    num_dim = 2;
    num_nodes = Node.rows();
    num_elem = Element.rows();
    num_elem_blk = 1;
    num_node_sets = 0;
    num_side_sets = 0;
    error = ex_put_init (exoid, "This is a test", num_dim, num_nodes, num_elem,
                         num_elem_blk, num_node_sets, num_side_sets);
    /* write nodal coordinates values and names to database */
    double *z ;
    error = ex_put_coord (exoid, Node.col(0).data(), Node.col(1).data(), z);
    /* write element block parameters */
    num_elem_in_block[0] = num_elem;
    num_nodes_per_elem[0] = 4; /* elements in block #1 are 4-node quads  */
    ebids[0] = 1;
    error = ex_put_elem_block (exoid, ebids[0], "QUAD", num_elem_in_block[0],
                               num_nodes_per_elem[0], 1);
    Element.array() += 1;
//    /* write element connectivity */
    error = ex_put_elem_conn (exoid, ebids[0], Element.data());
    /* for each time step, write the analysis results;
     * the code below fills the arrays glob_var_vals,
     * nodal_var_vals, and elem_var_vals with values for debugging purposes;
     * obviously the analysis code will populate these arrays
     */
    /* write results variables parameters and names */
    char *var_names[3];
    var_names[0] = "nod_var0";
    int num_nod_vars;
    num_nod_vars = 1;
    error = ex_put_var_param (exoid, "n", num_nod_vars);
    error = ex_put_var_names (exoid, "n", num_nod_vars, var_names);
    /* for each time step, write the analysis results;
     * the code below fills the arrays glob_var_vals,
     * nodal_var_vals, and elem_var_vals with values for debugging purposes;
     * obviously the analysis code will populate these arrays
     */
    int whole_time_step, num_time_steps;
    double time_value;
    double *nodal_var_vals;
    whole_time_step = 1;
    num_time_steps = 10;
    nodal_var_vals = (double *) calloc (num_nodes, CPU_word_size);
    for (int i=0; i<num_time_steps; i++)
    {
        time_value = (double)(i+1)/100.;
        /* write time value */
        error = ex_put_time (exoid, whole_time_step, &time_value);
        /* write nodal variables */
        for (int k=1; k<=num_nod_vars; k++)
        {
            for (int j=0; j<num_nodes; j++)
            {
                nodal_var_vals[j] = (double)k + ((double)(j+1) * time_value*100);
            }
            error = ex_put_nodal_var (exoid, whole_time_step, k, num_nodes,
                                      nodal_var_vals);
        }
        whole_time_step++;
        /* update the data file; this should be done at the end of every time step
         * to ensure that no data is lost if the analysis dies
         */
        error = ex_update (exoid);
    }
    /* close the EXODUS files */
    free(nodal_var_vals);
    error = ex_close (exoid);
}


/// @file  shell.hpp
/// @brief Declarations for methods to find spherical shell intersections.

#pragma once
#ifndef OGT_EMBED_SHELL_HPP
#define OGT_EMBED_SHELL_HPP

#include <ogt/config.hpp>
#include <vector>
#include <Eigen/Dense>

namespace OGT_NAMESPACE {
namespace embed {

/// Find a point within the intersection of n or more spherical shells in n
/// dimensions. If the shells do not intersect, a point is found which is as
/// close as possible to the shells using a least-squares objective.
///
/// Throws std::invalid_argument if the matrix dimensions are incorrect.
/// Throws ogt::EmbedErr if the embedding fails.
///
/// @see sphere_intersection_opt()
Eigen::VectorXd shell_intersection(const Eigen::MatrixXd &A,
	const Eigen::VectorXd& d, const Eigen::VectorXd& t, double eps,
	double min_delta, size_t max_iter, bool verbose);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_SHELL_HPP */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008, 2011 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <ql/models/shortrate/onefactormodels/hullwhite.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmesher.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/fdmhestonhullwhiteop.hpp>
#include <ql/methods/finitedifferences/operators/secondordermixedderivativeop.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    FdmHestonHullWhiteEquityPart::FdmHestonHullWhiteEquityPart(
        const boost::shared_ptr<FdmMesher>& mesher,
        const boost::shared_ptr<HullWhite>& hwModel,
        const boost::shared_ptr<YieldTermStructure>& qTS)
    : x_(mesher->locations(2)),
      varianceValues_(0.5*mesher->locations(1)),
      dxMap_ (FirstDerivativeOp(0, mesher)),
      dxxMap_(SecondDerivativeOp(0, mesher).mult(0.5*mesher->locations(1))),
      mapT_   (0, mesher),
      hwModel_(hwModel),
      mesher_ (mesher),
      qTS_(qTS) {

        // on the boundary s_min and s_max the second derivative
        // d²V/dS² is zero and due to Ito's Lemma the variance term
        // in the drift should vanish.
        const boost::shared_ptr<FdmLinearOpLayout> layout = mesher_->layout();
        const FdmLinearOpIterator endIter = layout->end();
        for (FdmLinearOpIterator iter = layout->begin(); iter != endIter;
            ++iter) {
            if (   iter.coordinates()[0] == 0
                || iter.coordinates()[0] == layout->dim()[0]-1) {
                varianceValues_[iter.index()] = 0.0;
            }
        }
        volatilityValues_ = Sqrt(2*varianceValues_);
    }

    void FdmHestonHullWhiteEquityPart::setTime(Time t1, Time t2) {
        const boost::shared_ptr<OneFactorModel::ShortRateDynamics> dynamics =
            hwModel_->dynamics();

        const Real phi = 0.5*(  dynamics->shortRate(t1, 0.0)
                              + dynamics->shortRate(t2, 0.0));

        const Rate q = qTS_->forwardRate(t1, t2, Continuous).rate();

        mapT_.axpyb(x_+phi-varianceValues_-q, dxMap_, dxxMap_, Array());
    }

    const TripleBandLinearOp& FdmHestonHullWhiteEquityPart::getMap() const {
        return mapT_;
    }

    FdmHestonHullWhiteOp::FdmHestonHullWhiteOp(
                    const boost::shared_ptr<FdmMesher>& mesher,
                    const boost::shared_ptr<HestonProcess>& hestonProcess,
                    const boost::shared_ptr<HullWhiteProcess>& hwProcess,
                    Real equityShortRateCorrelation)
    : v0_(hestonProcess->v0()),
      kappa_(hestonProcess->kappa()),
      theta_(hestonProcess->theta()),
      sigma_(hestonProcess->sigma()),
      rho_(hestonProcess->rho()),
      hwModel_(boost::make_shared<HullWhite>(hestonProcess->riskFreeRate(),
                             hwProcess->a(), hwProcess->sigma())),
      hestonCorrMap_(SecondOrderMixedDerivativeOp(0, 1, mesher)
                     .mult(rho_*sigma_*mesher->locations(1))),
      equityIrCorrMap_(SecondOrderMixedDerivativeOp(0, 2, mesher)
                       .mult(Sqrt(mesher->locations(1))
                              * hwProcess->sigma()
                              * equityShortRateCorrelation)),
      dyMap_(SecondDerivativeOp(1u, mesher)
              .mult(0.5*sigma_*sigma_*mesher->locations(1))
            .add(FirstDerivativeOp(1, mesher)
              .mult(kappa_*(theta_ - mesher->locations(1))))),
      dxMap_(mesher, hwModel_, hestonProcess->dividendYield().currentLink()),
      hullWhiteOp_(mesher, hwModel_, 2) {

        QL_REQUIRE(  equityShortRateCorrelation*equityShortRateCorrelation
                   + hestonProcess->rho()*hestonProcess->rho() <= 1.0,
                   "correlation matrix has negative eigenvalues");
    }

    void FdmHestonHullWhiteOp::setTime(Time t1, Time t2) {
        dxMap_.setTime(t1, t2);
        hullWhiteOp_.setTime(t1, t2);
    }

    Size FdmHestonHullWhiteOp::size() const {
        return 3;
    }

    Disposable<Array> FdmHestonHullWhiteOp::apply(const Array& u) const {
        return  dyMap_.apply(u) + dxMap_.getMap().apply(u)
              + hullWhiteOp_.apply(u)
              + hestonCorrMap_.apply(u) + equityIrCorrMap_.apply(u);
    }

    Disposable<Array>
    FdmHestonHullWhiteOp::apply_direction(Size direction,
                                          const Array& r) const {
        if (direction == 0)
            return dxMap_.getMap().apply(r);
        else if (direction == 1)
            return dyMap_.apply(r);
        else if (direction == 2)
            return hullWhiteOp_.apply(r);
        else
            QL_FAIL("direction too large");
    }

    Disposable<Array> FdmHestonHullWhiteOp::apply_mixed(const Array& r) const {
        return hestonCorrMap_.apply(r) + equityIrCorrMap_.apply(r);
    }

    Disposable<Array>
    FdmHestonHullWhiteOp::solve_splitting(Size direction, const Array& r,
                                          Real a) const {
        if (direction == 0) {
            return dxMap_.getMap().solve_splitting(r, a, 1.0);
        }
        else if (direction == 1) {
            return dyMap_.solve_splitting(r, a, 1.0);
        }
        else if (direction == 2) {
            return hullWhiteOp_.solve_splitting(2, r, a);
        }
        else
            QL_FAIL("direction too large");
    }

    Disposable<Array> FdmHestonHullWhiteOp::preconditioner(const Array& r,
                                                           Real dt) const {
        return solve_splitting(0, r, dt);
    }

#if !defined(QL_NO_UBLAS_SUPPORT)
    Disposable<std::vector<SparseMatrix> >
    FdmHestonHullWhiteOp::toMatrixDecomp() const {
        std::vector<SparseMatrix> retVal(4);
        retVal[0] = dxMap_.getMap().toMatrix();
        retVal[1] = dyMap_.toMatrix();
        retVal[2] = hullWhiteOp_.toMatrixDecomp().front();
        retVal[3] = hestonCorrMap_.toMatrix() + equityIrCorrMap_.toMatrix();

        return retVal;
    }
#endif
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2017 Amir Vaxman <avaxman@gmail.com>
//
// 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 <igl/shapeup.h>
#include <igl/min_quad_with_fixed.h>
#include <igl/igl_inline.h>
#include <igl/setdiff.h>
#include <igl/cat.h>
#include <igl/PI.h>
#include <Eigen/Core>
#include <vector>

namespace igl
{

  //This projection does nothing but render points into projP. Mostly used for "echoing" the global step
  IGL_INLINE bool shapeup_identity_projection(const Eigen::PlainObjectBase<Eigen::MatrixXd>& P, const Eigen::PlainObjectBase<Eigen::VectorXi>& SC, const Eigen::PlainObjectBase<Eigen::MatrixXi>& S,  Eigen::PlainObjectBase<Eigen::MatrixXd>& projP){
    projP.conservativeResize(SC.rows(), 3*SC.maxCoeff());
    for (int i=0;i<S.rows();i++){
      Eigen::RowVector3d avgCurrP=Eigen::RowVector3d::Zero();
      for (int j=0;j<SC(i);j++)
        avgCurrP+=P.row(S(i,j))/(double)(SC(i));

      for (int j=0;j<SC(i);j++)
        projP.block(i,3*j,1,3)=P.row(S(i,j))-avgCurrP;
    }
    return true;
  }

  //the projection assumes that the sets are vertices of polygons in order
  IGL_INLINE bool shapeup_regular_face_projection(const Eigen::PlainObjectBase<Eigen::MatrixXd>& P, const Eigen::PlainObjectBase<Eigen::VectorXi>& SC, const Eigen::PlainObjectBase<Eigen::MatrixXi>& S,  Eigen::PlainObjectBase<Eigen::MatrixXd>& projP){
    projP.conservativeResize(SC.rows(), 3*SC.maxCoeff());
    for (int currRow=0;currRow<SC.rows();currRow++){
    //computing average
      int N=SC(currRow);
      const Eigen::RowVectorXi SRow=S.row(currRow);
      Eigen::RowVector3d avgCurrP=Eigen::RowVector3d::Zero();
      Eigen::MatrixXd targetPolygon(N, 3);
      Eigen::MatrixXd sourcePolygon(N, 3);
      for (int j=0;j<N;j++)
        avgCurrP+=P.row(SRow(j))/(double)(N);

      for (int j=0;j<N;j++)
        targetPolygon.row(j)=P.row(SRow(j))-avgCurrP;

      //creating perfectly regular source polygon
      for (int j=0;j<N;j++)
        sourcePolygon.row(j)<<cos(2*igl::PI*(double)j/(double(N))), sin(2*igl::PI*(double)j/(double(N))),0.0;

      //finding closest similarity transformation between source and target
      Eigen::MatrixXd corrMat=sourcePolygon.transpose()*targetPolygon;
      Eigen::JacobiSVD<Eigen::Matrix3d> svd(corrMat, Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::MatrixXd R=svd.matrixU()*svd.matrixV().transpose();
      //getting scale by edge length change average. TODO: by singular values
      Eigen::VectorXd sourceEdgeLengths(N);
      Eigen::VectorXd targetEdgeLengths(N);
      for (int j=0;j<N;j++){
        sourceEdgeLengths(j)=(sourcePolygon.row((j+1)%N)-sourcePolygon.row(j)).norm();
        targetEdgeLengths(j)=(targetPolygon.row((j+1)%N)-targetPolygon.row(j)).norm();
      }
      double scale=(targetEdgeLengths.cwiseQuotient(sourceEdgeLengths)).mean();

      for (int j=0;j<N;j++)
        projP.block(currRow,3*j,1,3)=sourcePolygon.row(j)*R*scale;
    }

    return true;
  }

  template <
  typename DerivedP,
  typename DerivedSC,
  typename DerivedS,
  typename Derivedw>
  IGL_INLINE bool shapeup_precomputation(const Eigen::PlainObjectBase<DerivedP>& P,
                                         const Eigen::PlainObjectBase<DerivedSC>& SC,
                                         const Eigen::PlainObjectBase<DerivedS>& S,
                                         const Eigen::PlainObjectBase<DerivedS>& E,
                                         const Eigen::PlainObjectBase<DerivedSC>& b,
                                         const Eigen::PlainObjectBase<Derivedw>& wShape,
                                         const Eigen::PlainObjectBase<Derivedw>& wSmooth,
                                         ShapeupData & sudata)
  {
      using namespace std;
      using namespace Eigen;
      sudata.P=P;
      sudata.SC=SC;
      sudata.S=S;
      sudata.b=b;
      typedef typename DerivedP::Scalar Scalar;

      //checking for consistency of the input
      assert(SC.rows()==S.rows());
      assert(SC.rows()==wShape.rows());
      assert(E.rows()==wSmooth.rows());
      assert(b.rows()!=0);  //would lead to matrix becoming SPD

      sudata.DShape.conservativeResize(SC.sum(), P.rows());  //Shape matrix (integration);
      sudata.DClose.conservativeResize(b.rows(), P.rows());  //Closeness matrix for positional constraints
      sudata.DSmooth.conservativeResize(E.rows(), P.rows());  //smoothness matrix

      //Building shape matrix
      std::vector<Triplet<Scalar> > DShapeTriplets;
      int currRow=0;
      for (int i=0;i<S.rows();i++){
          Scalar avgCoeff=1.0/(Scalar)SC(i);

          for (int j=0;j<SC(i);j++){
            for (int k=0;k<SC(i);k++){
              if (j==k)
                DShapeTriplets.push_back(Triplet<Scalar>(currRow+j, S(i,k), (1.0-avgCoeff)));
              else
                DShapeTriplets.push_back(Triplet<Scalar>(currRow+j, S(i,k), (-avgCoeff)));
            }
          }
        currRow+=SC(i);

      }

      sudata.DShape.setFromTriplets(DShapeTriplets.begin(), DShapeTriplets.end());

      //Building closeness matrix
      std::vector<Triplet<Scalar> > DCloseTriplets;
      for (int i=0;i<b.size();i++)
        DCloseTriplets.push_back(Triplet<Scalar>(i,b(i), 1.0));

      sudata.DClose.setFromTriplets(DCloseTriplets.begin(), DCloseTriplets.end());

      //Building smoothness matrix
      std::vector<Triplet<Scalar> > DSmoothTriplets;
      for (int i=0; i<E.rows(); i++) {
        DSmoothTriplets.push_back(Triplet<Scalar>(i, E(i, 0), -1));
        DSmoothTriplets.push_back(Triplet<Scalar>(i, E(i, 1), 1));
      }

      SparseMatrix<Scalar> tempMat;
      igl::cat(1, sudata.DShape, sudata.DClose, tempMat);
      igl::cat(1, tempMat, sudata.DSmooth, sudata.A);

      //weight matrix
      vector<Triplet<Scalar> > WTriplets;

      //one weight per set in S.
      currRow=0;
      for (int i=0;i<SC.rows();i++){
          for (int j=0;j<SC(i);j++)
              WTriplets.push_back(Triplet<double>(currRow+j,currRow+j,sudata.shapeCoeff*wShape(i)));
          currRow+=SC(i);
      }

      for (int i=0;i<b.size();i++)
          WTriplets.push_back(Triplet<double>(SC.sum()+i, SC.sum()+i, sudata.closeCoeff));

      for (int i=0;i<E.rows();i++)
          WTriplets.push_back(Triplet<double>(SC.sum()+b.size()+i, SC.sum()+b.size()+i, sudata.smoothCoeff*wSmooth(i)));

      sudata.W.conservativeResize(SC.sum()+b.size()+E.rows(), SC.sum()+b.size()+E.rows());
      sudata.W.setFromTriplets(WTriplets.begin(), WTriplets.end());

      sudata.At=sudata.A.transpose();  //for efficieny, as we use the transpose a lot in the iteration
      sudata.Q=sudata.At*sudata.W*sudata.A;

      return min_quad_with_fixed_precompute(sudata.Q,VectorXi(),SparseMatrix<double>(),true,sudata.solver_data);
  }

  template <
  typename DerivedP,
  typename DerivedSC,
  typename DerivedS>
  IGL_INLINE bool shapeup_solve(const Eigen::PlainObjectBase<DerivedP>& bc,
                                const std::function<bool(const Eigen::PlainObjectBase<DerivedP>&, const Eigen::PlainObjectBase<DerivedSC>&, const Eigen::PlainObjectBase<DerivedS>&,  Eigen::PlainObjectBase<DerivedP>&)>& local_projection,
                                const Eigen::PlainObjectBase<DerivedP>& P0,
                                const ShapeupData & sudata,
                                const bool quietIterations,
                                Eigen::PlainObjectBase<DerivedP>& P)
  {
    using namespace Eigen;
    using namespace std;
    MatrixXd currP=P0;
    MatrixXd prevP=P0;
    MatrixXd projP;

    assert(bc.rows()==sudata.b.rows());

		MatrixXd rhs(sudata.A.rows(), 3); rhs.setZero();
    rhs.block(sudata.DShape.rows(), 0, sudata.b.rows(),3)=bc;  //this stays constant throughout the iterations

    if (!quietIterations){
        cout<<"Shapeup Iterations, "<<sudata.DShape.rows()<<" constraints, solution size "<<P0.size()<<endl;
        cout<<"**********************************************************************************************"<<endl;
    }
    projP.conservativeResize(sudata.SC.rows(), 3*sudata.SC.maxCoeff());
    for (int iter=0;iter<sudata.maxIterations;iter++){

      local_projection(currP, sudata.SC,sudata.S,projP);

      //constructing the projection part of the (DShape rows of the) right hand side
      int currRow=0;
      for (int i=0;i<sudata.S.rows();i++)
        for (int j=0;j<sudata.SC(i);j++)
          rhs.row(currRow++)=projP.block(i, 3*j, 1,3);

      DerivedP lsrhs=-sudata.At*sudata.W*rhs;
      MatrixXd Y(0,3), Beq(0,3);  //We do not use the min_quad_solver fixed variables mechanism; they are treated with the closeness energy of ShapeUp.
      min_quad_with_fixed_solve(sudata.solver_data, lsrhs,Y,Beq,currP);

      double currChange=(currP-prevP).lpNorm<Infinity>();
      if (!quietIterations)
        cout << "Iteration "<<iter<<", integration Linf error: "<<currChange<< endl;
      prevP=currP;
      if (currChange<sudata.pTolerance){
        P=currP;
        return true;
      }
    }

    P=currP;
    return false;  //we went over maxIterations

  }
}

#ifdef IGL_STATIC_LIBRARY
template bool igl::shapeup_precomputation< typename Eigen::Matrix<double, -1, -1, 0, -1, -1>, typename Eigen::Matrix<int, -1, 1, 0, -1, 1>, typename Eigen::Matrix<int, -1, -1, 0, -1, -1>, typename Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&,   Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, igl::ShapeupData&);

template bool igl::shapeup_solve<typename Eigen::Matrix<double, -1, -1, 0, -1, -1>, typename Eigen::Matrix<int, -1, 1, 0, -1, 1>, typename Eigen::Matrix<int, -1, -1, 0, -1, -1> >(const Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >& bc, const std::function<bool(const Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, const Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> >&, const Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> >&,  Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >& ) >& local_projection, const Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >& P0, const igl::ShapeupData & sudata, const bool quietIterations, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >& P);
#endif


/**
 * Some part of the source code is from the SHOC benchmark suite.
 *
 * Original authors:
 * Collin McCurdy, September 08, 2009
 * Vasily Volkov
 *
 * Here is the original copyright notice and a note from the original implementation:
 * This code uses algorithm described in:
 * "Fitting FFT onto G80 Architecture". Vasily Volkov and Brian Kazian, UC Berkeley CS258 project report. May 2008.
 *
 * Written by Vasily Volkov.
 * Copyright (c) 2008-2009, The Regents of the University of California.
 * 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 University of California, Berkeley 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 OWNER 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 <boost/math/special_functions/next.hpp>
#include <boost/program_options.hpp>
#include <cstdlib>
#include <iostream>
#include <omp.h>
#include <sys/mman.h>
#include <tuple>

#include "../device_hopper/core.h"
#include "shoc_src/fftlib.h"

using namespace device_hopper;

#define REPOSITORY_PATH std::string(std::getenv("PLASTICITY_ROOT"))
#define PREFERRED_DEVICE CPU

#ifndef M_PI
# define M_PI 3.14159265358979323846f
#endif

#ifndef M_SQRT1_2
# define M_SQRT1_2      0.70710678118654752440f
#endif

#define exp_1_8   (float2){  1.0f, -1.0f }//requires post-multiply by 1/sqrt(2)
#define exp_1_4   (float2){  0.0f, -1.0f }
#define exp_3_8   (float2){ -1.0f, -1.0f }//requires post-multiply by 1/sqrt(2)

#define iexp_1_8   (float2){  1.0f, 1.0f }//requires post-multiply by 1/sqrt(2)
#define iexp_1_4   (float2){  0.0f, 1.0f }
#define iexp_3_8   (float2){ -1.0f, 1.0f }//requires post-multiply by 1/sqrt(2)

inline void globalLoads8(float2 *data, float2 *work, int stride) {
    for (int i = 0; i < 8; i++)
        data[i] = work[i * stride];
}

inline void globalStores8(float2 *data, float2 *work, int stride) {
    int reversed[] = {0, 4, 2, 6, 1, 5, 3, 7};

//#pragma unroll
    for (int i = 0; i < 8; i++) {
        float2 d = data[reversed[i]];
        work[i * stride] = d;
    }
}

inline void storex8(float2 *a, float *smem, int sx) {
    int reversed[] = {0, 4, 2, 6, 1, 5, 3, 7};

//#pragma unroll
    for (int i = 0; i < 8; i++)
        smem[i * sx] = a[reversed[i]].x;
}

inline void storey8(float2 *a, float *smem, int sx) {
    int reversed[] = {0, 4, 2, 6, 1, 5, 3, 7};

//#pragma unroll
    for (int i = 0; i < 8; i++)
        smem[i * sx] = a[reversed[i]].y;
}

inline void loadx8(float2 *a, float *smem, int sx) {
    for (int i = 0; i < 8; i++)
        a[i].x = smem[i * sx];
}

inline void loady8(float2 *a, float *smem, int sx) {
    for (int i = 0; i < 8; i++)
        a[i].y = smem[i * sx];
}

#define transpose(a, s, ds, l, dl, sync)                              \
{                                                                       \
    storex8( a, s, ds );  if( (sync)&8 ) device_hopper::batch_barrier(); \
    loadx8 ( a, l, dl );  if( (sync)&4 ) device_hopper::batch_barrier();  \
    storey8( a, s, ds );  if( (sync)&2 ) device_hopper::batch_barrier();  \
    loady8 ( a, l, dl );  if( (sync)&1 ) device_hopper::batch_barrier();  \
}

inline float2 exp_i(float phi) {
//#ifdef USE_NATIVE
//    return (float2)( native_cos(phi), native_sin(phi) );
//#else
    return (float2) {cos(phi), sin(phi)};
//#endif
}

inline float2 cmplx_mul(float2 a, float2 b) { return (float2) {a.x * b.x - a.y * b.y, a.x * b.y + a.y * b.x}; }

inline float2 cm_fl_mul(float2 a, float b) { return (float2) {b * a.x, b * a.y}; }

inline float2 cmplx_add(float2 a, float2 b) { return (float2) {a.x + b.x, a.y + b.y}; }

inline float2 cmplx_sub(float2 a, float2 b) { return (float2) {a.x - b.x, a.y - b.y}; }

#define twiddle8(a, i, n)                                              \
{                                                                       \
    int reversed8[] = {0,4,2,6,1,5,3,7};                                \
    for( int j = 1; j < 8; j++ ){                                       \
        a[j] = cmplx_mul( a[j],exp_i((-2*M_PI*reversed8[j]/(n))*(i)) ); \
    }                                                                   \
}

#define FFT2(a0, a1)                            \
{                                               \
    float2 c0 = *a0;                           \
    *a0 = cmplx_add(c0,*a1);                    \
    *a1 = cmplx_sub(c0,*a1);                    \
}

#define FFT4(a0, a1, a2, a3)                    \
{                                               \
    FFT2( a0, a2 );                             \
    FFT2( a1, a3 );                             \
    *a3 = cmplx_mul(*a3,exp_1_4);               \
    FFT2( a0, a1 );                             \
    FFT2( a2, a3 );                             \
}

#define FFT8(a)                                                 \
{                                                               \
    FFT2( &a[0], &a[4] );                                       \
    FFT2( &a[1], &a[5] );                                       \
    FFT2( &a[2], &a[6] );                                       \
    FFT2( &a[3], &a[7] );                                       \
                                                                \
    a[5] = cm_fl_mul( cmplx_mul(a[5],exp_1_8) , M_SQRT1_2 );    \
    a[6] =  cmplx_mul( a[6] , exp_1_4);                         \
    a[7] = cm_fl_mul( cmplx_mul(a[7],exp_3_8) , M_SQRT1_2 );    \
                                                                \
    FFT4( &a[0], &a[1], &a[2], &a[3] );                         \
    FFT4( &a[4], &a[5], &a[6], &a[7] );                         \
}

#define itwiddle8(a, i, n)                                            \
{                                                                       \
    int reversed8[] = {0,4,2,6,1,5,3,7};                                \
    for( int j = 1; j < 8; j++ )                                        \
        a[j] = cmplx_mul(a[j] , exp_i((2*M_PI*reversed8[j]/(n))*(i)) ); \
}

#define IFFT2 FFfloat2

#define IFFT4(a0, a1, a2, a3)                 \
{                                               \
    IFFfloat2( a0, a2 );                            \
    IFFfloat2( a1, a3 );                            \
    *a3 = cmplx_mul(*a3 , iexp_1_4);            \
    IFFfloat2( a0, a1 );                            \
    IFFfloat2( a2, a3);                             \
}

#define IFFT8(a)                                              \
{                                                               \
    IFFfloat2( &a[0], &a[4] );                                      \
    IFFfloat2( &a[1], &a[5] );                                      \
    IFFfloat2( &a[2], &a[6] );                                      \
    IFFfloat2( &a[3], &a[7] );                                      \
                                                                \
    a[5] = cm_fl_mul( cmplx_mul(a[5],iexp_1_8) , M_SQRT1_2 );   \
    a[6] = cmplx_mul( a[6] , iexp_1_4);                         \
    a[7] = cm_fl_mul( cmplx_mul(a[7],iexp_3_8) , M_SQRT1_2 );   \
                                                                \
    IFFT4( &a[0], &a[1], &a[2], &a[3] );                        \
    IFFT4( &a[4], &a[5], &a[6], &a[7] );                        \
}

typedef struct cplxflt {
    float x;
    float y;
} cplxflt;

void populate_input_buffer(cplxflt *buffer_in_out, size_t nftts) {
    size_t half_n_cmplx = (nftts * 512) / 2;
    for (size_t i = 0; i < half_n_cmplx; ++i) {
        buffer_in_out[i].x = (rand() / (float) RAND_MAX) * 2 - 1;
        buffer_in_out[i].y = (rand() / (float) RAND_MAX) * 2 - 1;
        buffer_in_out[i + half_n_cmplx].x = buffer_in_out[i].x;
        buffer_in_out[i + half_n_cmplx].y = buffer_in_out[i].y;
    }
}

bool verify_results(cplxflt *b, cplxflt *reference, unsigned long b_size, size_t n_ffts) {
    // Check results
    // To check the results we first perform an inverse FFT and then check if the results of that
    // are close enough to the original input. The epsilon for this last check is hard coded below.
    // We have to setup OpenCL for the inverse FFT.
    // OpenCL setup
    cl_uint num_platforms = 0;
    cl_platform_id platform[2];
    cl_device_id device;
    cl_int err = clGetPlatformIDs(2, platform, &num_platforms);
    err |= clGetDeviceIDs(
            platform[0],
            CL_DEVICE_TYPE_CPU,
            1,
            &device,
            NULL);
    if (err == -1 && num_platforms > 1) {
        std::cout << "INFO: First platform does not offer the target device. "
                     "Trying to use the second platform..." << std::endl;
        err = clGetDeviceIDs(
                platform[1],
                CL_DEVICE_TYPE_CPU,
                1,
                &device,
                NULL);
        if (err) return EXIT_FAILURE;
    }

    // Create the OCL context handle
    cl_context cl_ctx = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
    plasticity::cl_utils::cl_check_return_code(err, "Could not initialise the OpenCL context");

    // Load and compile the .cl file
    std::string cl_file_path = REPOSITORY_PATH + "/benchmarks/shoc_src/shoc_fft.cl";
    FILE *program_handle = fopen(cl_file_path.c_str(), "r");
    if (program_handle == NULL) {
        std::cerr << "Error: Could not open the OpenCL source code file" << std::endl;
        std::exit(EXIT_FAILURE);
    }
    err |= fseek(program_handle, 0, SEEK_END);
    size_t program_size = ftell(program_handle);
    rewind(program_handle);
    char *program_buffer = (char *) malloc(program_size + 1);
    program_buffer[program_size] = '\0';
    fread(program_buffer, sizeof(char), program_size, program_handle);
    fclose(program_handle);
    //std::cout << program_buffer << std::endl;
    cl_program program = clCreateProgramWithSource(
            cl_ctx,
            1,
            (const char **) &program_buffer,
            &program_size,
            &err);
    plasticity::cl_utils::cl_check_return_code(err, "Could create program handle");
    free(program_buffer);
    std::string flags = "";
    flags += "-cl-fast-relaxed-math ";
    flags += "-cl-mad-enable ";
    flags += " -DSINGLE_PRECISION";

    err |= clBuildProgram(program, 1, &device, flags.c_str(), NULL, NULL);
    if (err) {
        std::cerr << "Program building failed: ERROR " << err << std::endl;
        // Print warnings and errors from compilation
        static char log[65536];
        memset(log, 0, sizeof(log));
        err = clGetProgramBuildInfo(program,
                              device,
                              CL_PROGRAM_BUILD_LOG,
                              sizeof(log) - 1,
                              log,
                              NULL);
        printf("-----OpenCL Compiler Output-----\n");
        if (strstr(log, "warning:") || strstr(log, "error:"))
            printf("<<<<\n%s\n>>>>\n", log);
        printf("--------------------------------\n");
        std::exit(EXIT_FAILURE);
    }
    cl_kernel ifftKrnl = clCreateKernel(program, "ifft1D_512_original", &err);
    plasticity::cl_utils::cl_check_return_code(err, "Could not create an OpenCL kernel handle");
    cl_command_queue queue = clCreateCommandQueue(cl_ctx, device, 0, &err);
    plasticity::cl_utils::cl_check_return_code(err, "Could not create an OpenCL queue");

    // Copy the output of the FFT to the device and perform the inverse FFT.
    cl_mem device_buf = allocHostBuffer(b, b_size, cl_ctx);
    //void *device_buffer_for_checks;
    //allocDeviceBuffer(&device_buffer_for_checks, b_size, cl_ctx, queue);
    //copyToDevice(device_buffer_for_checks, b, b_size, queue);
    err = transform(device_buf, n_ffts, ifftKrnl, queue);
    plasticity::cl_utils::cl_check_return_code(err, "Could not launch kernel");
    err = clFinish(queue);
    plasticity::cl_utils::cl_check_return_code(err, "Could not run kernel");
    //copyFromDevice(host_buffer_for_checks, device_buffer_for_checks, b_size, queue);

    // Check if the results of the inverse FFT are close enough to the original input of the FFT.
    for (int i = 0; i < n_ffts / 2; ++i) {
#if defined(FFT)
        float x_diff = fabs(host_buffer_for_checks[i].x - reference[i].x);
        float y_diff = fabs(host_buffer_for_checks[i].y - reference[i].y);
#else
        float x_diff = fabs(b[i].x - reference[i].x);
        float y_diff = fabs(b[i].y - reference[i].y);
#endif
        if (x_diff > 0.0001 || y_diff > 0.0001 || std::isnan(b[i].x) || std::isnan(b[i].y)) {
            std::cout << "Error at: " << i << std::endl;
            std::cout << "Expected: " << reference[i].x << " " << reference[i].y << std::endl;
#if defined(FFT)
            std::cout << "Is: " << host_buffer_for_checks[i].x << " " << host_buffer_for_checks[i].y << std::endl;
#else
            std::cout << "Is: " << b[i].x << " " << b[i].y << std::endl;
#endif

            return false;
        }
    }
    return true;
}

DEVICE_HOPPER_MAIN(int argc, char *argv[]) {
    DEVICE_HOPPER_SETUP
    const size_t block_size = 64;

    // Parse CLI parameters
    boost::program_options::options_description desc("Options");
    desc.add_options()("problem-size", boost::program_options::value<long>(), "Sample count");
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    size_t n_ffts = 0;
    if (vm.count("problem-size") == 0) {
        std::cerr << "Error: Problem size is missing." << std::endl;
        std::exit(EXIT_FAILURE);
    } else {
        // This parameter is passed to the application by the Python scripts that automate the experiments.
        n_ffts = vm["problem-size"].as<long>();
    }
    const size_t buffer_size = n_ffts * 512u;

    // Allocate buffers
    cplxflt *buffer_in_out = (cplxflt *) device_hopper::aligned_malloc(4096, buffer_size, sizeof(cplxflt));

    // Init input data
    populate_input_buffer(buffer_in_out, n_ffts);

    // Create a copy of the input buffer to compute the reference results later
    cplxflt *reference_buffer = (cplxflt *) std::malloc(buffer_size * sizeof(cplxflt));
    std::memcpy(reference_buffer, buffer_in_out, buffer_size * sizeof(cplxflt));
    parallel_for pf(0, n_ffts * block_size, [=]DEVICE_HOPPER_LAMBDA() {
        const int tid = GET_ITERATION_WITHIN_BATCH();
        const int bid = GET_BATCH_ID() * 512 + tid;
        int hi = tid >> 3;
        int lo = tid & 7;
        float2 data[8];
        __attribute__((device_hopper_batch_shared)) float smem[8 * 8 * 9]; //__local

        float2 *work = (float2 *) buffer_in_out;

        // starting index of data to/from global memory
        work = work + bid;
        //out = out + blockIdx;
        globalLoads8(data, work, 64); // coalesced global reads

        FFT8(data);
        twiddle8(data, tid, 512);
        transpose(data, &smem[hi * 8 + lo], 66, &smem[lo * 66 + hi], 8, 0xf);
        FFT8(data);
        twiddle8(data, hi, 64);
        transpose(data, &smem[hi * 8 + lo], 8 * 9, &smem[hi * 8 * 9 + lo], 8, 0xE);
        FFT8(data);

        globalStores8(data, work, 64);
    });
    // Register buffers and describe accesses
    pf.add_buffer_access_patterns(
        device_hopper::buf(buffer_in_out, direction::IN_OUT, pattern::SUCCESSIVE_SUBSECTIONS(pf.batch_size * 8)));
    // Set optional tuning parameters and call run()
    pf.opt_set_batch_size(64).opt_set_simple_indices(true).run();

    if (!verify_results(buffer_in_out, reference_buffer, buffer_size * sizeof(cplxflt), n_ffts)) {
        std::cout << "Error: The results are incorrect" << std::endl;
        return EXIT_FAILURE;
    } else {
        std::cout << "Info: The results are correct" << std::endl;
        return EXIT_SUCCESS;
    }
}