/**
 * Copyright 2020-2021 Huawei Technologies Co., Ltd
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "backend/kernel_compiler/cpu/eigen/matmul_double_cpu_kernel.h"
#include <Eigen/Dense>
#include <vector>

namespace mindspore {
namespace kernel {
namespace {
constexpr size_t kMatMulInputsNum = 2;
constexpr size_t kMatMulOutputsNum = 1;
constexpr auto kAMatrixDimNum = 2;
const size_t kIndexOffset = 2;
using Eigen::ColMajor;
using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::Ref;
using Eigen::RowMajor;
template <int Major>
using DoubleMatrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Major>;
}  // namespace
void MatmulDoubleCPUKernel::InitKernel(const CNodePtr &kernel_node) {
  MS_EXCEPTION_IF_NULL(kernel_node);
  kernel_name_ = AnfAlgo::GetCNodeName(kernel_node);
  std::vector<size_t> a_shape = AnfAlgo::GetInputDeviceShape(kernel_node, 0);
  std::vector<size_t> b_shape = AnfAlgo::GetInputDeviceShape(kernel_node, 1);
  std::vector<size_t> out_shape = AnfAlgo::GetOutputDeviceShape(kernel_node, 0);
  if (a_shape.size() != kAMatrixDimNum || b_shape.size() != kAMatrixDimNum || out_shape.size() != kAMatrixDimNum) {
    MS_LOG(EXCEPTION) << "The tensor rank of MatMul should be equal to 2.";
  }
  trans_a_ = AnfAlgo::GetNodeAttr<bool>(kernel_node, TRANSPOSE_A);
  trans_b_ = AnfAlgo::GetNodeAttr<bool>(kernel_node, TRANSPOSE_B);

  a_row_ = a_shape[kDim0];
  a_col_ = a_shape[kDim1];
  b_row_ = b_shape[kDim0];
  b_col_ = b_shape[kDim1];
  out_row_ = out_shape[kDim0];
  out_col_ = out_shape[kDim1];
}

template <typename Derived>
inline void matmul_b(const MatrixBase<Derived> &A, double *b_addr, double *output_addr, size_t b_row_, size_t b_col_,
                     size_t out_row_, size_t out_col_, bool trans_b) {
  Map<DoubleMatrix<RowMajor>> output(output_addr, out_row_, out_col_);
  if (trans_b) {
    Map<DoubleMatrix<ColMajor>> b(b_addr, b_col_, b_row_);
    output.noalias() = A * b;
  } else {
    Map<DoubleMatrix<RowMajor>> b(b_addr, b_row_, b_col_);
    output.noalias() = A * b;
  }
}

bool MatmulDoubleCPUKernel::Launch(const std::vector<kernel::AddressPtr> &inputs,
                                   const std::vector<kernel::AddressPtr> &,
                                   const std::vector<kernel::AddressPtr> &outputs) {
  CHECK_KERNEL_INPUTS_NUM(inputs.size(), kMatMulInputsNum, kernel_name_);
  CHECK_KERNEL_OUTPUTS_NUM(outputs.size(), kMatMulOutputsNum, kernel_name_);
  const auto a_addr = reinterpret_cast<double *>(inputs[0]->addr);
  const auto b_addr = reinterpret_cast<double *>(inputs[1]->addr);
  auto output_addr = reinterpret_cast<double *>(outputs[0]->addr);

  if (trans_a_) {
    Map<DoubleMatrix<ColMajor>> A(a_addr, a_col_, a_row_);
    matmul_b(A, b_addr, output_addr, b_row_, b_col_, out_row_, out_col_, trans_b_);
  } else {
    Map<DoubleMatrix<RowMajor>> A(a_addr, a_row_, a_col_);
    matmul_b(A, b_addr, output_addr, b_row_, b_col_, out_row_, out_col_, trans_b_);
  }

  return true;
}
}  // namespace kernel
}  // namespace mindspore


#include "Ridesharing/privacy.h"

#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 run()
{

    testPerfPSI<degree,modulus,T>(10,2);

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

    return 0;
}

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

    // sampling a day in 5 minutes epoch
    //uint64_t size = 24*12;
    // initialization of users profiles

    uint64_t size = 24*60*60;
    uint8_t aliceProfile[size], bobProfile[size];
    auto start = std::chrono::steady_clock::now();
    auto end = std::chrono::steady_clock::now();

    // construction of users profiles (by default we fill everything )
    for(uint64_t i=0;i<size;i++) {

        aliceProfile[i]=1;
        bobProfile[i]=1;
    }

    start = std::chrono::steady_clock::now();
    nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
    char *aliceOut;
    uint64_t aliceOutLen;
    initiateC<degree,modulus,T>(aliceOut,(char*)aliceProfile,aliceOutLen,size,SecretKey);

    char *bobOut;
    uint64_t bobOutLen;
    initiateS<degree,modulus,T>(bobOut,aliceOut,(char*)bobProfile,bobOutLen,size);
    char match[size];
    terminateC<degree,modulus,T>(match,bobOut,bobOutLen,size,SecretKey);
    for(int i=0;i<size;i++){
        if(match[i]==1){

        }
    }
    end = std::chrono::steady_clock::now();
    std::cout << 1000*boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000) << std::endl;
}

template<size_t degree, size_t modulus, class T>
int launch() {
    Transport::GraphFactory gf("/vagrant/ubimob/graph.txt-dump", false);
    const Transport::Graph * g = gf.get();
    gf.setAll2();
    //gf.PickUpByRegion(43.5115, 1.3177, 43.6862, 1.5573, 10, 100 );
    std::cout << g->PickUpZone().size() << std::endl;
    typedef boost::tokenizer< boost::char_separator<char> > Tokenizer;
    boost::char_separator<char> sep(";");
    string filename="/vagrant/ubimob/analysis/experiments/dec2015/my_instances_16-12-2015.csv";
    ifstream in(filename.c_str());
    if (!in.is_open()) exit(0);
    vector< string > vec;
    string line;
    int i=0;
    int car_1, car_2, foot_1, foot_2;
    int iso=3600;
    int step=60;
    int ratio=1;
    Privacy::Toolbox * tb=new Privacy::Toolbox();
    tb->create_result("/vagrant/ubimob/analysis/experiments/dec2015/Results/exp004.csv");
    while (getline(in,line)){
      Tokenizer tok(line,sep);
      vec.assign(tok.begin(),tok.end());
      if(i>0){
//###############################################################################################################
        car_1=std::atoi(vec.at(0).c_str());
        foot_1=std::atoi(vec.at(1).c_str());
        car_2=std::atoi(vec.at(2).c_str());
        foot_2=std::atoi(vec.at(3).c_str());
        auto start = std::chrono::steady_clock::now();
        auto end = std::chrono::steady_clock::now();
        Privacy::Driver * d1=new Privacy::Driver(car_1, car_2);
        Privacy::Pedestrian * r1=new Privacy::Pedestrian(foot_1, foot_2);
        Privacy::Manager * m=new Privacy::Manager();
        Privacy::Output output;
        //---record scenario id, step and isochrone radius
        output.scenario_id = i;
        output.isochrone_radius = iso;
        output.step = step;
        uint64_t size = g->num_vertices();

        start = std::chrono::steady_clock::now();
        d1->findPickup(g, iso);
        end = std::chrono::steady_clock::now();
        //---record driver origin isochrone running time
        output.driver_origin_isochrone_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

        start = std::chrono::steady_clock::now();
        d1->findDropoff(g, iso);
        end = std::chrono::steady_clock::now();
        //---record driver destination isochrone running time
        output.driver_destination_isochrone_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

        start = std::chrono::steady_clock::now();
        r1->findPickup(g, iso);
        end = std::chrono::steady_clock::now();
        //---record rider origin isochrone running time
        output.rider_origin_isochrone_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

        start = std::chrono::steady_clock::now();
        r1->findDropoff(g, iso);
        end = std::chrono::steady_clock::now();
        //---record rider destination isochrone running time
        output.rider_destination_isochrone_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

        int StarUp = 0;
        int EndUp = step;
        int StarOff = 0;
        int EndOff = step;
        NodeList pickUp, dropOff;
        int num_steps_pickup = 0, num_steps_dropoff=0;
        // §§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§------Pickup selection------------§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
        start = std::chrono::steady_clock::now();
         while (EndUp <= iso){
            d1->choosePickup(StarUp/ratio, EndUp/ratio);
            r1->choosePickup(StarUp, EndUp);
            NodeList pickup_driver_set = d1->getPickup();
            NodeList pickup_rider_set = r1->getPickup();
            uint8_t DriverPickupSet[size], RiderPickupSet[size];
            for(uint64_t i=0;i<size;i++) {
                DriverPickupSet[i]=0;
                RiderPickupSet[i]=0;
            }
            //-----------------Driver
            BOOST_FOREACH(int node, pickup_driver_set){
                DriverPickupSet[node]=1;
            }
            //-----------------Pedestrian
            BOOST_FOREACH(int node, pickup_rider_set){
                RiderPickupSet[node]=1;
            }
            nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
            // ################################## PickUp PSI @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
            char *DriverPickupOut;
            uint64_t DriverPickupOutLen;
            initiateC<degree,modulus,T>(DriverPickupOut,(char*)DriverPickupSet,DriverPickupOutLen,size,SecretKey);
            char *RiderPickupOut;
            uint64_t RiderPickupOutLen;
            initiateS<degree,modulus,T>(RiderPickupOut,DriverPickupOut,(char*)RiderPickupSet,RiderPickupOutLen,size);
            char PSI_Pickup[size];
            terminateC<degree,modulus,T>(PSI_Pickup,RiderPickupOut,RiderPickupOutLen,size,SecretKey);
            //pickUp.clear();
            for(int i=0;i<size;i++){
                if(PSI_Pickup[i]==1){
                    pickUp.push_back(i);
                }
            }
            //StarUp = EndUp;
            EndUp += step;
            num_steps_pickup++;

            if (pickUp.size() > 0){
                break;
            }
         }
         end = std::chrono::steady_clock::now();
         //---record psi pickup runtime
         output.psi_pickup_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);
         output.num_steps_pickup = num_steps_pickup;

         start = std::chrono::steady_clock::now();
        // §§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§------Dropoff selection------------§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§§
         while (EndOff <= iso){

            d1->chooseDropoff(StarOff/ratio, EndOff/ratio);
            r1->chooseDropoff(StarOff, EndOff);
            NodeList dropoff_driver_set = d1->getDropoff();
            NodeList dropoff_rider_set = r1->getDropoff();
            uint8_t DriverDropoffSet[size], RiderDropoffSet[size];
            for(uint64_t i=0;i<size;i++) {
                DriverDropoffSet[i]=0;
                RiderDropoffSet[i]=0;
            }
            //-----------------Driver
            BOOST_FOREACH(int node, dropoff_driver_set){
                DriverDropoffSet[node]=1;
            }
            //-----------------Pedestrian
            BOOST_FOREACH(int node, dropoff_rider_set){
                RiderDropoffSet[node]=1;
            }
            nfl::poly_from_modulus<T, degree, modulus> *SecretKey;
            // ################################## DropOff PSI @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
            char *DriverDropoffOut;
            uint64_t DriverDropoffOutLen;
            initiateC<degree,modulus,T>(DriverDropoffOut,(char*)DriverDropoffSet,DriverDropoffOutLen,size,SecretKey);
            char *RiderDropoffOut;
            uint64_t RiderDropoffOutLen;
            initiateS<degree,modulus,T>(RiderDropoffOut,DriverDropoffOut,(char*)RiderDropoffSet,RiderDropoffOutLen,size);
            char PSI_Dropoff[size];
            terminateC<degree,modulus,T>(PSI_Dropoff,RiderDropoffOut,RiderDropoffOutLen,size,SecretKey);
            //dropOff.clear();
            for(int i=0;i<size;i++){
                if(PSI_Dropoff[i]==1){
                    dropOff.push_back(i);
                }
            }
            //StarOff = EndOff;
            EndOff += step;
            num_steps_dropoff++;

            if (dropOff.size() > 0){
                break;
            }
         }
         end = std::chrono::steady_clock::now();
        //---record psi dropoff runtime
         output.psi_dropoff_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);
         output.num_steps_dropoff = num_steps_dropoff;

         auto psi_result = m->getPSI(pickUp, dropOff);

         output.pickup_size = psi_result.PickUp.size();
         output.dropoff_size = psi_result.DropOff.size();

        start = std::chrono::steady_clock::now();
        m->GetAllPath(g, psi_result);
        end = std::chrono::steady_clock::now();
        //---record shared path computation runtime
        output.shared_path_computing_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

        if (m->shared_path_len() > 0){

            start = std::chrono::steady_clock::now();
            d1->getFavoritesB(m->shared_path);
            end = std::chrono::steady_clock::now();
            output.driver_path_ordering_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

            start = std::chrono::steady_clock::now();
            r1->getFavoritesB(m->shared_path);
            end = std::chrono::steady_clock::now();
            output.rider_path_ordering_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

            start = std::chrono::steady_clock::now();
            m->match(*d1, *r1);
            end = std::chrono::steady_clock::now();
            output.path_election_time = boost::lexical_cast<double>(get_time_us(start, end, 1)/1000000);

            //id of positions
            output.driver_origin = d1->posStart;
            output.driver_destination = d1->posEnd;
            output.rider_origin = r1->posStart;
            output.rider_destination = r1->posEnd;

            //id of pickup/dropoff
            output.privacy_pickup = m->getThePath().start;
            output.privacy_dropoff = m->getThePath().end;
            //postions of the distributed solution
            output.cd_positions = tb->cd_carpooling_positions(*d1, *r1, *m, g);
            //costs of the distributed solution
            output.cd_costs = tb->cd_costs(*d1, *r1, *m, g);
            //size of potential pickup for pedesdrian
            output.rider_pickup_size = tb->dlen(r1->data_before);
            //size of potential pickup for driver
            output.driver_pickup_size = tb->dlen(d1->data_before);
            //size of potential dropoff for pedesdrian
            output.rider_dropoff_size = tb->dlen(r1->data_after);
            //size of potential dropoff for driver
            output.driver_dropoff_size = tb->dlen(d1->data_after);

            output.shared_path_size = m->shared_path_len();
            //centralized test
            Privacy::cc_output cc;
            cc=tb->cc_carpooling_test(*d1,*r1,g);
            //costs in centralized
            output.cc_costs=cc.cc_costs;
            //pickup in centralized
            output.optimal_pickup=cc.cc_pickup;
            //dropoff in centralized
            output.optimal_dropoff=cc.cc_dropoff;
            //position in centralized
            output.cc_positions=cc.cc_positions;
            //runtime in centralized
            output.optimal_runtime=boost::lexical_cast<double>(cc.cc_time/1000);

            output.driver_original_trip_cost = tb->pointTopoint(g, car_1, car_2, true);
            output.rider_original_trip_cost = tb->pointTopoint(g, foot_1, foot_2, false);

            //output.cc_detour=tb->cc_detour(*d1, g, cc );
            //output.cd_detour=tb->cd_detour(*d1, g, output.cd_costs);

            tb->save_result("/vagrant/ubimob/analysis/experiments/dec2015/Results/exp004.csv", output, g);
            std::cout << "======================================================================> solution " << i << std::endl;
        }
//###############################################################################################################
    }
    i++;
}
    return 0;
}

int main(int argc, char const *argv[])
{
    return launch<CONFIG>();
    //return matching<CONFIG>();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_ADD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_ADD_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/two_add.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/include/functions/is_inf.hpp>
#include <boost/simd/include/functions/if_zero_else.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_add_
                                    , tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A0 const& a1,A0& a3) const
    {
      result_type a2;
      boost::simd::two_add(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_add_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef std::pair<A0,A0>                                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 const& a1) const
    {
      result_type res;
      boost::simd::two_add( a0, a1, res.first, res.second );
      return res;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_add_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE
    result_type operator()(A0 const& a,A0 const& b, A0 & r0,A0 & r1) const
    {
      r0   = a  + b;
      A0 z = r0 - a;
#if defined(BOOST_SIMD_NO_INFINITIES)
      r1   = a-(r0-z))+(b-z);
#else
      r1   = if_zero_else(is_inf(r0), (a-(r0-z))+(b-z));
#endif
    }
  };
} } }

#endif


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SQRT1PM1
#define BOOST_MATH_SQRT1PM1

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>

//
// This algorithm computes sqrt(1+x)-1 for small x:
//

namespace boost{ namespace math{

template <class T, class Policy>
inline typename tools::promote_args<T>::type sqrt1pm1(const T& val, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   BOOST_MATH_STD_USING

   if(fabs(result_type(val)) > 0.75)
      return sqrt(1 + result_type(val)) - 1;
   return boost::math::expm1(boost::math::log1p(val, pol) / 2, pol);
}

template <class T>
inline typename tools::promote_args<T>::type sqrt1pm1(const T& val)
{
   return sqrt1pm1(val, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SQRT1PM1


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <random>
#include <limits>
#include <benchmark/benchmark.h>
#include <boost/math/special_functions/logaddexp.hpp>

using boost::math::logaddexp;

template <typename Real>
void logaddexp_performance(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt {rd()};
    std::uniform_real_distribution<long double> dist(1e-50, 5e-50);

    Real x = static_cast<Real>(dist(mt));
    Real y = static_cast<Real>(dist(mt));

    for (auto _ : state)
    {
        benchmark::DoNotOptimize(logaddexp(x, y));
        x += std::numeric_limits<Real>::epsilon();
        y += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(logaddexp_performance, float);
BENCHMARK_TEMPLATE(logaddexp_performance, double);
BENCHMARK_TEMPLATE(logaddexp_performance, long double);

BENCHMARK_MAIN();


// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

#include <Eigen/LU>
// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/jntarrayacc.hpp>
#include <kdl/jntarray.hpp>
#include <math.h>
#include <numeric>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainjnttojacdotsolver.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/framevel.hpp>

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

#define PI 3.141592
#define D2R PI / 180.0
#define R2D 180.0 / PI
#define SaveDataMax 49

namespace arm_controllers
{
class DemoController : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
  public:
    bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
    {
        // ********* 1. Get joint name / gain from the parameter server *********
        // 1.1 Joint Name
        if (!n.getParam("joints", joint_names_))
        {
            ROS_ERROR("Could not find joint name");
            return false;
        }
        n_joints_ = joint_names_.size();

        if (n_joints_ == 0)
        {
            ROS_ERROR("List of joint names is empty.");
            return false;
        }
        else
        {
            ROS_INFO("Found %d joint names", n_joints_);
            for (int i = 0; i < n_joints_; i++)
            {
                ROS_INFO("%s", joint_names_[i].c_str());
            }
        }

        // 1.2 Gain
        // 1.2.1 Joint Controller
        Kp_.resize(n_joints_);
        Kd_.resize(n_joints_);
        Ki_.resize(n_joints_);
        seg_jac_1.resize(1);
        seg_jac_2.resize(2);
        seg_jac_3.resize(3);
        seg_jac_4.resize(4);
        seg_jac_5.resize(5);
        J_.resize(n_joints_);
        K_kine_.resize(n_joints_);
        K_kine_.data(0) = 1.0;
        K_kine_.data(1) = 1.0;
        K_kine_.data(2) = 1.0;
        K_kine_.data(3) = 1.0;
        K_kine_.data(4) = 1.0;
        K_kine_.data(5) = 1.0;

        std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);
        for (size_t i = 0; i < n_joints_; i++)
        {
            std::string si = boost::lexical_cast<std::string>(i + 1);
            if (n.getParam("/elfin/demo_controller/gains/elfin_joint" + si + "/pid/p", Kp[i]))
            {
                Kp_(i) = Kp[i];
            }
            else
            {
                std::cout << "/elfin/demo_controller/gains/elfin_joint" + si + "/pid/p" << std::endl;
                ROS_ERROR("Cannot find pid/p gain");
                return false;
            }

            if (n.getParam("/elfin/demo_controller/gains/elfin_joint" + si + "/pid/i", Ki[i]))
            {
                Ki_(i) = Ki[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/i gain");
                return false;
            }

            if (n.getParam("/elfin/demo_controller/gains/elfin_joint" + si + "/pid/d", Kd[i]))
            {
                Kd_(i) = Kd[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/d gain");
                return false;
            }
        }

        // 2. ********* urdf *********
        urdf::Model urdf;
        if (!urdf.initParam("robot_description"))
        {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }
        else
        {
            ROS_INFO("Found robot_description");
        }

        // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
        for (int i = 0; i < n_joints_; i++)
        {
            try
            {
                joints_.push_back(hw->getHandle(joint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException &e)
            {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

            urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
            if (!joint_urdf)
            {
                ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                return false;
            }
            joint_urdfs_.push_back(joint_urdf);
        }

        // 4. ********* KDL *********
        // 4.1 kdl parser
        if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
        {
            ROS_ERROR("Failed to construct kdl tree");
            return false;
        }
        else
        {
            ROS_INFO("Constructed kdl tree");
        }

        // 4.2 kdl chain
        std::string root_name, tip_name;
        if (!n.getParam("root_link", root_name))
        {
            ROS_ERROR("Could not find root link name");
            return false;
        }
        if (!n.getParam("tip_link", tip_name))
        {
            ROS_ERROR("Could not find tip link name");
            return false;
        }
        if (!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
        {
            ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
            ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name);
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfSegments() << " segments");
            ROS_ERROR_STREAM("  The segments are:");

            KDL::SegmentMap segment_map = kdl_tree_.getSegments();
            KDL::SegmentMap::iterator it;

            for (it = segment_map.begin(); it != segment_map.end(); it++)
                ROS_ERROR_STREAM("    " << (*it).first);

            return false;
        }
        else
        {
            ROS_INFO("Got kdl chain");
        }

        // 4.3 inverse dynamics solver 초기화
        gravity_ = KDL::Vector::Zero(); // ?
        gravity_(2) = -9.81;            // 0: x-axis 1: y-axis 2: z-axis

        id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));
        FKSolver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
        FKSolver_vel_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
        jnt_to_jac_dot_solver_.reset(new KDL::ChainJntToJacDotSolver(kdl_chain_));

        kdl_tree_.getChain("world", "elfin_link1", kdl_chain_1);
        jac_solver_1.reset(new KDL::ChainJntToJacSolver(kdl_chain_1));
        fk_solver_1.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_1));

        kdl_tree_.getChain("world", "elfin_link2", kdl_chain_2);
        jac_solver_2.reset(new KDL::ChainJntToJacSolver(kdl_chain_2));
        fk_solver_2.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_2));

        kdl_tree_.getChain("world", "elfin_link3", kdl_chain_3);
        jac_solver_3.reset(new KDL::ChainJntToJacSolver(kdl_chain_3));
        fk_solver_3.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_3));

        kdl_tree_.getChain("world", "elfin_link4", kdl_chain_4);
        jac_solver_4.reset(new KDL::ChainJntToJacSolver(kdl_chain_4));
        fk_solver_4.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_4));

        kdl_tree_.getChain("world", "elfin_link5", kdl_chain_5);
        jac_solver_5.reset(new KDL::ChainJntToJacSolver(kdl_chain_5));
        fk_solver_5.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_5));

        // ********* 5. 각종 변수 초기화 *********

        // 5.1 Vector 초기화 (사이즈 정의 및 값 0)
        tau_d_.data = Eigen::VectorXd::Zero(n_joints_);

        qd_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

        q_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_.data = Eigen::VectorXd::Zero(n_joints_);
        xdot_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_task_ = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_lower_.data = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_obstacle_prev_ = Eigen::VectorXd::Zero(n_joints_);
        qdot_vel_array_.q.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_vel_array_.qdot.data = Eigen::VectorXd::Zero(n_joints_);

        obstacle_.p(0) = -0.1;
        obstacle_.p(1) = -0.1;
        obstacle_.p(2) = 0.75;
        obstacle_.M.RPY(0,0,0);

        jnt_limits_lower_(0) = -0.6;
        jnt_limits_lower_(1) = -0.6;
        jnt_limits_lower_(2) = -0.6;
        jnt_limits_lower_(3) = -0.6;
        jnt_limits_lower_(4) = -0.6;
        jnt_limits_lower_(5) = -0.6;

        jnt_limits_upper_(0) = 0.6;
        jnt_limits_upper_(1) = 0.6;
        jnt_limits_upper_(2) = 0.6;
        jnt_limits_upper_(3) = 0.6;
        jnt_limits_upper_(4) = 0.6;
        jnt_limits_upper_(5) = 0.6;

        table_corner_1 = KDL::Vector(-0.2, -0.15, 0.3);
        table_corner_2 = KDL::Vector(-0.9, -0.15, 0.3);
        table_corner_3 = KDL::Vector(-0.9, 0.15, 0.3);
        table_corner_4 = KDL::Vector(-0.2, 0.15, 0.3);

        table2_corner_1 = KDL::Vector(-0.1, -0.45, 0.2);
        table2_corner_2 = KDL::Vector(-0.8, -0.45, 0.2);
        table2_corner_3 = KDL::Vector(-0.8, -0.15, 0.2);
        table2_corner_4 = KDL::Vector(-0.1, -0.15, 0.2);

        table3_corner_1 = KDL::Vector(-0.1, 0.15, 0.2);
        table3_corner_2 = KDL::Vector(-0.8, 0.15, 0.2);
        table3_corner_3 = KDL::Vector(-0.8, 0.45, 0.2);
        table3_corner_4 = KDL::Vector(-0.1, 0.45, 0.2);

        interpolated_table_surface_.push_back(table_corner_1);
        interpolated_table_surface_.push_back(table_corner_2);

        interpolated_table_surface2_.push_back(table2_corner_1);
        interpolated_table_surface2_.push_back(table2_corner_2);

        interpolated_table_surface3_.push_back(table3_corner_1);
        interpolated_table_surface3_.push_back(table3_corner_2);

        int split_num = 20;
        for(int i = 1; i < split_num; i++){
            double diff12 = (table_corner_2(0) - table_corner_1(0)) * i/split_num;
            interpolated_table_surface_.push_back(KDL::Vector(table_corner_1(0) + diff12, -0.15, 0.3));

            diff12 = (table2_corner_2(0) - table2_corner_1(0)) * i/split_num;
            interpolated_table_surface2_.push_back(KDL::Vector(table2_corner_1(0) + diff12, -0.45, 0.2));

            diff12 = (table3_corner_2(0) - table3_corner_1(0)) * i/split_num;
            interpolated_table_surface3_.push_back(KDL::Vector(table3_corner_1(0) + diff12, 0.15, 0.2));
        }

        std::vector<KDL::Vector> tmp_copy = interpolated_table_surface_;
        std::vector<KDL::Vector> tmp_copy2 = interpolated_table_surface2_;
        std::vector<KDL::Vector> tmp_copy3 = interpolated_table_surface3_;

        for(int i = 1; i < (int)tmp_copy.size(); i++){
            double curr_x = tmp_copy[i](0);
            double curr_x2 = tmp_copy2[i](0);
            double curr_x3 = tmp_copy3[i](0);
            for(int j = 0; j <= split_num; j++){
                double next_y = tmp_copy[i](1) + 0.3*j/split_num;
                double next_y2 = tmp_copy2[i](1) + 0.3*j/split_num;
                double next_y3 = tmp_copy3[i](1) + 0.3*j/split_num;
                interpolated_table_surface_.push_back(KDL::Vector(curr_x, next_y, 0.3));
                interpolated_table_surface_.push_back(KDL::Vector(curr_x2, next_y2, 0.2));
                interpolated_table_surface_.push_back(KDL::Vector(curr_x3, next_y3, 0.2));
            }
        }
        std::cout << "TABLE HAS: " << interpolated_table_surface_.size() << std::endl;

        for(int i = 1; i <= 5; i++){
            KDL::JntArray arr;
            arr.data = Eigen::VectorXd::Zero(i);
            arm_segment_states_.push_back(arr);
        }
        std::cout << "JntArrays done " << std::endl;

        e_.data = Eigen::VectorXd::Zero(n_joints_);
        e_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        e_int_.data = Eigen::VectorXd::Zero(n_joints_);

        // 5.2 Matrix 초기화 (사이즈 정의 및 값 0)
        M_.resize(kdl_chain_.getNrOfJoints());
        C_.resize(kdl_chain_.getNrOfJoints());
        G_.resize(kdl_chain_.getNrOfJoints());

        // ********* 6. ROS 명령어 *********
        // 6.1 publisher
        pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
        pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
        pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

        pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000); // 뒤에 숫자는?

        // 6.2 subsriber

        return true;
    }

    void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
    {
        if (msg->data.size() != n_joints_)
        {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
            return;
        }
    }

    void starting(const ros::Time &time)
    {
        t = 0.0;
        ROS_INFO("Starting Reactive Controller");
    }

    void update(const ros::Time &time, const ros::Duration &period)
    {
        // ********* 0. Get states from gazebo *********
        // 0.1 sampling time
        double dt = period.toSec();
        t = t + 0.001;

        // 0.2 joint state
        for (int i = 0; i < n_joints_; i++)
        {
            q_(i) = joints_[i].getPosition();
            qdot_(i) = joints_[i].getVelocity();
        }

        int segment_length = 1;
        for(int i = 0; i < 5; i++){
            for(int j = 1; j <= segment_length; j++){
                arm_segment_states_[i](j) = q_(j);
            }
            segment_length++;
        }

        std::vector<KDL::Jacobian> jacobians_;
   //     jac_solver_1->JntToJac(arm_segment_states_[0], seg_jac_1);
   //     jacobians_.push_back(seg_jac_1);

   //     jac_solver_2->JntToJac(arm_segment_states_[1], seg_jac_2);
   //     jacobians_.push_back(seg_jac_2);

   //     jac_solver_3->JntToJac(arm_segment_states_[2], seg_jac_3);
   //     jacobians_.push_back(seg_jac_3);

   //     jac_solver_4->JntToJac(arm_segment_states_[3], seg_jac_4);
   //     jacobians_.push_back(seg_jac_4);

        jac_solver_5->JntToJac(arm_segment_states_[4], seg_jac_5);
        jacobians_.push_back(seg_jac_5);

        KDL::Frame xd_;
        FKSolver_->JntToCart(q_, x_);
        double Q_star_table_ = 0.15;
        double Q_star_table_min_ = 0.122;

        if( (t > 3 && t < 5) ){
            Q_star_table_ -= (t-3) * 0.016;
        }
        else if( (t > 5 && t < 11.9) ){
            Q_star_table_ = Q_star_table_min_;
        }
        else if( (t >= 11.9 && t < 12.5) ){
            Q_star_table_ -= (12.5-t) * 0.5;
        }
        else if( (t >= 15 && t < 16) ){
            Q_star_table_ -= (t-15) * 0.03;
        }
        else if( (t >= 16 && t < 21.9) ){
            Q_star_table_ = Q_star_table_min_;
        }
        else if( (t >= 21.9 && t < 23) ){
            Q_star_table_ -= (23-t) * 0.272;
        }

        if( Q_star_table_ < Q_star_table_min_){
            Q_star_table_ = Q_star_table_min_;
         }

        // ********* 1. Desired Trajecoty in Joint Space *********

        for (size_t i = 0; i < n_joints_; i++)
        {
       //     qd_ddot_(i) = -M_PI * M_PI / 4 * 45 * KDL::deg2rad * sin(M_PI / 2 * t);
       //     qd_dot_(i) = M_PI / 2 * 45 * KDL::deg2rad * cos(M_PI / 2 * t);
       //     qd_(i) = 45 * KDL::deg2rad * sin(M_PI / 2* t);
          //  continue;
         //
            if( t < 1 ){
                qd_(i) = 0;
            }
           // qd_(i) = 0;
            qd_ddot_(i) = 0;
            qd_dot_(i) = 0;

            // Move to first table
            if(t > 1 && t < 6){
                qd_(0) = -1.0;
                qd_(1) = 0;
                qd_(2) = -1.75;
                qd_(3) = 0.01;
                qd_(4) = -1.4;
                qd_(5) = 0;
            }

            // Perform maneuver
            if(t >= 6 && t < 12){
                if( !task_space_ ){
                    circleDrawerAngle_ = -M_PI/2;
                    FKSolver_->JntToCart(q_, x_);
                    circle_x = x_.p(0);
                    circle_y = x_.p(1) + 0.1;
                    Ki_(0) = 400.0;
                    Ki_(1) = 400.0;
                    Ki_(2) = 400.0;
                    Ki_(3) = 400.0;
                    Ki_(4) = 400.0;
                    Ki_(5) = 400.0;
                    Kd_(0) = 40.0;
                    Kd_(1) = 40.0;
                    Kd_(2) = 40.0;
                    Kd_(3) = 40.0;
                    Kd_(4) = 40;
                    Kd_(5) = 40;
                }
                drawing_circle_ = true;
                task_space_ = true;
                FKSolver_->JntToCart(q_, x_);
                circleDrawerAngle_ += 0.0004;
                xd_.p(0) = circle_x + 0.07 * cos(circleDrawerAngle_);
                xd_.p(1) = circle_y + 0.07 * sin(circleDrawerAngle_);
                xd_.p(2) = 0.3;
                xd_.M = KDL::Rotation(KDL::Rotation::RPY(M_PI/2, 0, 0));
              //  std::cout << circleDrawerAngle_ << "  " << xd_.p(0) << " " << xd_.p(1) << std::endl;
            }

            //Move to second table
            if(t >= 12 && t < 16){
                if( task_space_ ){
                    task_space_ = false;
                    drawing_circle_ = false;
                    Ki_(0) = 36.0;
                    Ki_(1) = 36.0;
                    Ki_(2) = 36.0;
                    Ki_(3) = 36.0;
                    Ki_(4) = 36.0;
                    Ki_(5) = 36.0;
                    Kd_(0) = 12.0;
                    Kd_(1) = 12.0;
                    Kd_(2) = 12.0;
                    Kd_(3) = 12.0;
                    Kd_(4) = 12;
                    Kd_(5) = 12;
                }
                if( q_(0) < 0.7){
                    qd_(0) = qd_(0) + 0.00015;
                }
                else{
                    qd_(0) = 1.0;
                }
                qd_(1) = 0;
                qd_(2) = -1.75;
                qd_(3) = 0.01;
                qd_(4) = -1.4;
                qd_(5) = 0;
            }

            // Perform maneuver
            if(t >= 16 && t < 22){
                if( !task_space_ ){
                    circleDrawerAngle_ = M_PI/2;
                    FKSolver_->JntToCart(q_, x_);
                    circle_x = x_.p(0) - 0.05;
                    circle_y = x_.p(1) - 0.1;
                    Ki_(0) = 400.0;
                    Ki_(1) = 400.0;
                    Ki_(2) = 400.0;
                    Ki_(3) = 400.0;
                    Ki_(4) = 400.0;
                    Ki_(5) = 400.0;
                    Kd_(0) = 40.0;
                    Kd_(1) = 40.0;
                    Kd_(2) = 40.0;
                    Kd_(3) = 40.0;
                    Kd_(4) = 40;
                    Kd_(5) = 40;
                }
                drawing_circle_ = true;
                task_space_ = true;
                FKSolver_->JntToCart(q_, x_);
                circleDrawerAngle_ -= 0.0004;
                xd_.p(0) = circle_x - 0.07 * cos(circleDrawerAngle_);
                xd_.p(1) = circle_y - 0.07 * sin(circleDrawerAngle_);
                xd_.p(2) = 0.3;
                xd_.M = KDL::Rotation(KDL::Rotation::RPY(M_PI/2, 0, 0));
           //     std::cout << circleDrawerAngle_ << "  " << xd_.p(0) << " " << xd_.p(1) << std::endl;
            }

            if(t >= 22 && t < 25){
                if( task_space_ ){
                    task_space_ = false;
                    drawing_circle_ = false;
                    Ki_(0) = 36.0;
                    Ki_(1) = 36.0;
                    Ki_(2) = 36.0;
                    Ki_(3) = 36.0;
                    Ki_(4) = 36.0;
                    Ki_(5) = 36.0;
                    Kd_(0) = 12.0;
                    Kd_(1) = 12.0;
                    Kd_(2) = 12.0;
                    Kd_(3) = 12.0;
                    Kd_(4) = 12;
                    Kd_(5) = 12;
                }
                if( q_(0) > 0.05){
                    qd_(0) = qd_(0) - 0.00012;
                }
                else{
                    qd_(0) = 0;
                }
                qd_(1) = 0;
                qd_(2) = -1.75;
                qd_(3) = 0.01;
                qd_(4) = -1.4;
                qd_(5) = 0;
            }
            if( t >= 25 && t < 27 ){
                qd_(0) = 0;
                qd_(1) = 0;
                qd_(2) = 0;
                qd_(3) = 0;
                qd_(4) = 0;
                qd_(5) = 0;
            }
            if( t >= 27 ){
                t = 0;
            }
        }

        tau_rep_ = Eigen::VectorXd::Zero(n_joints_);

        // *** 2.2.1 Compute model(M,C,G) ***
        id_solver_->JntToMass(q_, M_);
        id_solver_->JntToCoriolis(q_, qdot_, C_);
        id_solver_->JntToGravity(q_, G_);

        // Apply Joint limits as repulsive forces/torques
        KDL::JntArray diff_q_;
        Subtract(q_, qd_, diff_q_);
        Subtract(q_, jnt_limits_lower_, diff_to_low_);
        Subtract(q_, jnt_limits_upper_, diff_to_upper_);

        KDL::Frame x_up_lim_;
        KDL::Frame x_low_lim_;

        jnt_to_jac_solver_->JntToJac(q_, J_);

        // Calculate workspace positions for joints and middle parts of links.
        std::vector<KDL::Frame> arm_segment_states_cart_;

        KDL::Frame seg_cart_;
 //       fk_solver_1->JntToCart(arm_segment_states_[0], seg_cart_);
 //       arm_segment_states_cart_.push_back(seg_cart_);
 //       fk_solver_2->JntToCart(arm_segment_states_[1], seg_cart_);
 //       arm_segment_states_cart_.push_back(seg_cart_);
 //       fk_solver_3->JntToCart(arm_segment_states_[2], seg_cart_);
 //       arm_segment_states_cart_.push_back(seg_cart_);
 //       fk_solver_4->JntToCart(arm_segment_states_[3], seg_cart_);
 //       arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_5->JntToCart(arm_segment_states_[4], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);

        double closest_dist_ = 999;
        int closest_idx_ = -2;
        Eigen::Matrix<double, 6, 1> ex_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> ex_closest_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> F_rep_table_total_ = Eigen::VectorXd::Zero(6);

        // Include repulsion field of the table WRT end effector only.
        for(int i = 0; i < (int)interpolated_table_surface_.size(); i++){
           // break;
            KDL::Vector curr_table_point_ = interpolated_table_surface_[i];
            ex_table_point_(0) = curr_table_point_(0) - x_.p(0);
            ex_table_point_(1) = curr_table_point_(1) - x_.p(1);
            ex_table_point_(2) = curr_table_point_(2) - x_.p(2);
            table_point_distances_[i] = -ex_table_point_;

            double dist = sqrt( ex_table_point_(0)*ex_table_point_(0) + ex_table_point_(1)*ex_table_point_(1) + ex_table_point_(2)*ex_table_point_(2));
            if( dist < Q_star_table_ ){
                closest_dist_ = dist;
                closest_idx_ = i;
                ex_closest_table_point_ = -ex_table_point_;

                Eigen::Matrix<double, 6, 1> grad_D_table_ = ex_table_point_ - table_point_distances_prev_[i];

              //  F_rep_table_total_(0) = -0.1 * (1 / ex_table_point_(0) - 1 / Q_star_table_) * (1 / (ex_table_point_(0) * ex_table_point_(0)) * -grad_D_table_(0));
             //   F_rep_table_total_(1) = -0.1 * (1 / ex_table_point_(1) - 1 / Q_star_table_) * (1 / (ex_table_point_(1) * ex_table_point_(1)) * -grad_D_table_(1));
                F_rep_table_total_(2) = -1 * (1 / ex_table_point_(2) - 1 / Q_star_table_) * (1 / (ex_table_point_(2) * ex_table_point_(2)) * -grad_D_table_(2));
                for(int f = 0; f < 3; f++){
                    if( F_rep_table_total_(f) > 90 ){
                        F_rep_table_total_(f) = 90;
                    }
                }

                // UNCOMMENT THIS to include the tables repulsion forces in the arm.
               tau_rep_ += J_.data.transpose() * F_rep_table_total_;
            }
        }
        tau_rep_ -= M_.data * qdot_.data;

       // ex_table_prev_ = ex_closest_table_point_;
        table_point_distances_prev_ = table_point_distances_;
        table_point_distances_arm_prev_ = table_point_distances_arm_;

        //Eigen::Matrix<double, 6, 1> F_rep_ = F_obstacle_;
        Eigen::Matrix<double, 6, 1> F_rep_ = F_rep_table_total_;
        F_rep_(3) = 0;
        F_rep_(4) = 0;
        F_rep_(5) = 0;

        // ********* 2. Motion Controller in Joint Space*********
        // *** 2.1 Error Definition in Joint Space ***
        e_.data = qd_.data - q_.data;
        e_dot_.data = qd_dot_.data - qdot_.data;
        e_int_.data = qd_.data - q_.data; // (To do: e_int 업데이트 필요요)

        // *** 2.2.2 gravity + PD as in slides ***

        if( !task_space_){
        // This can be considered as u_att
            tau_PD_grav_.data = G_.data + Kp_.data.cwiseProduct(e_.data) - Kd_.data.cwiseProduct(qdot_.data);

            KDL::JntArray tau_total_;
            tau_total_.data = tau_PD_grav_.data + tau_rep_;

           for (int i = 0; i < n_joints_; i++)
            {
                joints_[i].setCommand(tau_total_(i));
            }
        }

        else if( task_space_ ){

           // FKSolver_->JntToCart(qd_, xd_);

            xd_dot_task_(0) = 0;
            xd_dot_task_(1) = 0;
            xd_dot_task_(2) = 0;
            xd_dot_task_(3) = 0;
            xd_dot_task_(4) = 0;
            xd_dot_task_(5) = 0;

            xd_ddot_task_(0) = 0;
            xd_ddot_task_(1) = 0;
            xd_ddot_task_(2) = 0;
            xd_ddot_task_(3) = 0;
            xd_ddot_task_(4) = 0;
            xd_ddot_task_(5) = 0;

            xdot_task_ = J_.data * qdot_.data;
            ex_dot_task_ = xd_dot_task_ - xdot_task_;

            ex_temp_ = diff(x_, xd_);

            ex_task_(0) = ex_temp_(0);
            ex_task_(1) = ex_temp_(1);
            ex_task_(2) = ex_temp_(2);
            ex_task_(3) = ex_temp_(3);
            ex_task_(4) = ex_temp_(4);
            ex_task_(5) = ex_temp_(5);

            qdot_vel_array_.q.data = q_.data;
            qdot_vel_array_.qdot.data = qdot_.data;
            jnt_to_jac_dot_solver_->JntToJacDot(qdot_vel_array_, Jdot_);
            jnt_to_jac_dot_solver_->JntToJacDot(qdot_vel_array_, J_dot_q_dot_);

            KDL::JntArray J_dot_q_dot_array_; J_dot_q_dot_array_.resize(6);
            J_dot_q_dot_array_(0) = J_dot_q_dot_(0);
            J_dot_q_dot_array_(1) = J_dot_q_dot_(1);
            J_dot_q_dot_array_(2) = J_dot_q_dot_(2);
            J_dot_q_dot_array_(3) = J_dot_q_dot_(3);
            J_dot_q_dot_array_(4) = J_dot_q_dot_(4);
            J_dot_q_dot_array_(5) = J_dot_q_dot_(5);

            aux_d_.data = M_.data * J_.data.inverse() * (xd_ddot_task_ + Kp_.data.cwiseProduct(ex_task_) + Kd_.data.cwiseProduct(ex_dot_task_) - J_dot_q_dot_array_.data);
            comp_d_.data = C_.data + G_.data;
            tau_d_.data = aux_d_.data + comp_d_.data + tau_rep_;
           for (int i = 0; i < n_joints_; i++)
            {
                joints_[i].setCommand(tau_d_(i));
            }
        }

        // *** 2.3 Apply Torque Command to Actuator ***

        // Use 'e_dot_.data = qdot_c_.data - qdot_.data' for kinematic controller.
        // Otherwise use 'e_dot_.data = qd_dot_.data - qdot_.data'.
        //e_dot_.data = qdot_c_.data - qdot_.data;
        //aux_d_.data = M_.data * (qd_ddot_.data + Kd_.data.cwiseProduct(e_dot_.data));
        //comp_d_.data = C_.data + G_.data;
        //tau_d_.data = aux_d_.data + comp_d_.data;

        // ********* 3. data 저장 *********
        save_data();

        // ********* 4. state 출력 *********
        print_state();
    }

		double trajectory_generator_pos(double dStart, double dEnd, double dDuration, double time_)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return dA0 + dA3*time_*time_*time_ + dA4*time_*time_*time_*time_ + dA5*time_*time_*time_*time_*time_;
		}

		double trajectory_generator_vel(double dStart, double dEnd, double dDuration, double time_)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return 3.0*dA3*time_*time_ + 4.0*dA4*time_*time_*time_ + 5.0*dA5*time_*time_*time_*time_;
		}

		double trajectory_generator_acc(double dStart, double dEnd, double dDuration, double time_)
		{
			double dA0 = dStart;
			double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
			double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
			double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

			return 6.0*dA3*time_ + 12.0*dA4*time_*time_ + 20.0*dA5*time_*time_*time_;
		}

    void stopping(const ros::Time &time)
    {
    }

    void save_data()
    {
        // 1
        // Simulation time (unit: sec)
        SaveData_[0] = t;

        // Desired position in joint space (unit: rad)
        SaveData_[1] = qd_(0);
        SaveData_[2] = qd_(1);
        SaveData_[3] = qd_(2);
        SaveData_[4] = qd_(3);
        SaveData_[5] = qd_(4);
        SaveData_[6] = qd_(5);

        // Desired velocity in joint space (unit: rad/s)
        SaveData_[7] = qd_dot_(0);
        SaveData_[8] = qd_dot_(1);
        SaveData_[9] = qd_dot_(2);
        SaveData_[10] = qd_dot_(3);
        SaveData_[11] = qd_dot_(4);
        SaveData_[12] = qd_dot_(5);

        // Desired acceleration in joint space (unit: rad/s^2)
        SaveData_[13] = qd_ddot_(0);
        SaveData_[14] = qd_ddot_(1);
        SaveData_[15] = qd_ddot_(2);
        SaveData_[16] = qd_ddot_(3);
        SaveData_[17] = qd_ddot_(4);
        SaveData_[18] = qd_ddot_(5);

        // Actual position in joint space (unit: rad)
        SaveData_[19] = q_(0);
        SaveData_[20] = q_(1);
        SaveData_[21] = q_(2);
        SaveData_[22] = q_(3);
        SaveData_[23] = q_(4);
        SaveData_[24] = q_(5);

        // Actual velocity in joint space (unit: rad/s)
        SaveData_[25] = qdot_(0);
        SaveData_[26] = qdot_(1);
        SaveData_[27] = qdot_(2);
        SaveData_[28] = qdot_(3);
        SaveData_[29] = qdot_(4);
        SaveData_[30] = qdot_(5);

        // Error position in joint space (unit: rad)
        SaveData_[31] = e_(0);
        SaveData_[32] = e_(1);
        SaveData_[33] = e_(2);
        SaveData_[34] = e_(3);
        SaveData_[35] = e_(4);
        SaveData_[36] = e_(5);

        j1_avg.push_back(abs(R2D * e_(0)));
        j2_avg.push_back(abs(R2D * e_(1)));
        j3_avg.push_back(abs(R2D * e_(2)));
        j4_avg.push_back(abs(R2D * e_(3)));
        j5_avg.push_back(abs(R2D * e_(4)));
        j6_avg.push_back(abs(R2D * e_(5)));

        // Error velocity in joint space (unit: rad/s)
        SaveData_[37] = e_dot_(0);
        SaveData_[38] = e_dot_(1);
        SaveData_[39] = e_dot_(3);
        SaveData_[40] = e_dot_(4);
        SaveData_[41] = e_dot_(5);
        SaveData_[42] = e_dot_(6);

        j1_vel_avg.push_back(abs(R2D * e_dot_(0)));
        j2_vel_avg.push_back(abs(R2D * e_dot_(1)));
        j3_vel_avg.push_back(abs(R2D * e_dot_(2)));
        j4_vel_avg.push_back(abs(R2D * e_dot_(3)));
        j5_vel_avg.push_back(abs(R2D * e_dot_(4)));
        j6_vel_avg.push_back(abs(R2D * e_dot_(5)));

        // Error intergal value in joint space (unit: rad*sec)
        SaveData_[43] = e_int_(0);
        SaveData_[44] = e_int_(1);
        SaveData_[45] = e_int_(2);
        SaveData_[46] = e_int_(3);
        SaveData_[47] = e_int_(4);
        SaveData_[48] = e_int_(5);

        // 2
        msg_qd_.data.clear();
        msg_q_.data.clear();
        msg_e_.data.clear();

        msg_SaveData_.data.clear();

        // 3
        for (int i = 0; i < n_joints_; i++)
        {
            msg_qd_.data.push_back(qd_(i));
            msg_q_.data.push_back(q_(i));
            msg_e_.data.push_back(e_(i));
        }

        for (int i = 0; i < SaveDataMax; i++)
        {
            msg_SaveData_.data.push_back(SaveData_[i]);
        }

        // 4
        pub_qd_.publish(msg_qd_);
        pub_q_.publish(msg_q_);
        pub_e_.publish(msg_e_);

        pub_SaveData_.publish(msg_SaveData_);
    }

    void print_state()
    {
        static int count = 0;
        if (count > 99)
        {
            printf("*********************************************************\n\n");
            printf("*** Simulation Time (unit: sec)  ***\n");
            printf("t = %f\n", t);
            printf("\n");

            printf("*** Desired State in Joint Space (unit: deg) ***\n");
            printf("qd_(0): %f, ", qd_(0)*R2D);
            printf("qd_(1): %f, ", qd_(1)*R2D);
            printf("qd_(2): %f, ", qd_(2)*R2D);
            printf("qd_(3): %f, ", qd_(3)*R2D);
            printf("qd_(4): %f, ", qd_(4)*R2D);
            printf("qd_(5): %f\n", qd_(5)*R2D);
            printf("\n");

            printf("*** Actual State in Joint Space (unit: deg) ***\n");
            printf("q_(0): %f, ", q_(0) * R2D);
            printf("q_(1): %f, ", q_(1) * R2D);
            printf("q_(2): %f, ", q_(2) * R2D);
            printf("q_(3): %f, ", q_(3) * R2D);
            printf("q_(4): %f, ", q_(4) * R2D);
            printf("q_(5): %f\n", q_(5) * R2D);
            printf("\n");

            printf("*** Joint Space Vel Error (unit: deg)  ***\n");
            printf("%f, ", R2D * e_dot_(0));
            printf("%f, ", R2D * e_dot_(1));
            printf("%f, ", R2D * e_dot_(2));
            printf("%f, ", R2D * e_dot_(3));
            printf("%f, ", R2D * e_dot_(4));
            printf("%f\n", R2D * e_dot_(5));
            printf("\n");

            int num_vals = j1_vel_avg.size();
            double average = 0.0;
            if ( num_vals != 0) {
                average = std::accumulate( j1_vel_avg.begin(), j1_vel_avg.end(), 0.0) / num_vals;
                std::cout << "JOINT VEL ERROR AVERAGES: " << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j2_vel_avg.begin(), j2_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j3_vel_avg.begin(), j3_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j4_vel_avg.begin(), j4_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j5_vel_avg.begin(), j5_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j6_vel_avg.begin(), j6_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << std::endl;
            }

            count = 0;
        }
        count++;
    }

  private:
    // others
    double t;

    //Joint handles
    unsigned int n_joints_;                               // joint 숫자
    std::vector<std::string> joint_names_;                // joint name ??
    std::vector<hardware_interface::JointHandle> joints_; // ??
    std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

    // kdl
    KDL::Tree kdl_tree_;   // tree?
    KDL::Chain kdl_chain_; // chain?

    // kdl M,C,G
    KDL::JntSpaceInertiaMatrix M_; // intertia matrix
    KDL::JntSpaceInertiaMatrix M_inv_;
    KDL::JntArray C_;              // coriolis
    KDL::JntArray G_;              // gravity torque vector
    KDL::Vector gravity_;

    // kdl solver
    boost::scoped_ptr<KDL::ChainDynParam> id_solver_;                  // Solver To compute the inverse dynamics

    // Joint Space State
    KDL::JntArray qd_, qd_dot_, qd_ddot_;
    KDL::JntArray qd_old_;
    KDL::JntArray q_, qdot_, qddot_;
    KDL::JntArray e_, e_dot_, e_int_, e_ddot_;
    KDL::Jacobian J_;
    KDL::Jacobian Jdot_;
    KDL::Twist J_dot_q_dot_;
    KDL::Jacobian seg_jac_1;
    KDL::Jacobian seg_jac_2;
    KDL::Jacobian seg_jac_3;
    KDL::Jacobian seg_jac_4;
    KDL::Jacobian seg_jac_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    FKSolver_;
    boost::scoped_ptr<KDL::ChainFkSolverVel>  FKSolver_vel_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
    boost::scoped_ptr<KDL::ChainJntToJacDotSolver> jnt_to_jac_dot_solver_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_1;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_2;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_3;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_4;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_1;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_2;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_3;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_4;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_5;
    KDL::JntArray  xdot_;

    // Input
    KDL::JntArray aux_d_;
    KDL::JntArray aux_d_Kpzero_;
    KDL::JntArray comp_d_;
    KDL::JntArray tau_d_;
    KDL::JntArray tau_PD_grav_;
    KDL::JntArray vel_control_;
    KDL::JntArray kine_control_;
    KDL::JntArray qdot_c_;
    double rotX_pos, rotY_pos, rotZ_pos, rotW_pos, rotX_des, rotY_des, rotZ_des, rotW_des;

    // Joint limits
    KDL::JntArray jnt_limits_lower_;
    KDL::JntArray jnt_limits_upper_;
    KDL::JntArray diff_to_low_;
    KDL::JntArray diff_to_upper_;
    KDL::JntArray diff_to_low_prev_;
    KDL::JntArray diff_to_upper_prev_;
    KDL::Frame obstacle_;
    std::vector<KDL::JntArray> arm_segment_states_;

    Eigen::Matrix<double, 6, 1> ex_obstacle_prev_;
    Eigen::Matrix<double, 3, 1> ex_table_prev_;
    KDL::Vector table_corner_1;
    KDL::Vector table_corner_2;
    KDL::Vector table_corner_3;
    KDL::Vector table_corner_4;
    KDL::Vector table2_corner_1;
    KDL::Vector table2_corner_2;
    KDL::Vector table2_corner_3;
    KDL::Vector table2_corner_4;
    KDL::Vector table3_corner_1;
    KDL::Vector table3_corner_2;
    KDL::Vector table3_corner_3;
    KDL::Vector table3_corner_4;
    std::vector<KDL::Vector> interpolated_table_surface_;
    std::vector<KDL::Vector> interpolated_table_surface1_;
    std::vector<KDL::Vector> interpolated_table_surface2_;
    std::vector<KDL::Vector> interpolated_table_surface3_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_prev_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_prev_;
    Eigen::Matrix<double ,6, 1> tau_rep_;
    std::vector<boost::scoped_ptr<KDL::ChainJntToJacSolver>> jac_seg_solvers_;

    // gains
    KDL::JntArray Kp_, Ki_, Kd_, K_kine_;
    KDL::Frame fKine_pos_Frame_;
    KDL::Frame fKine_des_Frame_;
    KDL::Twist ex_temp_;
    KDL::Frame x_;
    KDL::FrameVel fKine_vel_Frame_;
    KDL::JntArrayVel qdot_vel_array_;

    KDL::Chain kdl_chain_1;
    KDL::Chain kdl_chain_2;
    KDL::Chain kdl_chain_3;
    KDL::Chain kdl_chain_4;
    KDL::Chain kdl_chain_5;

    double circleDrawerAngle_ = 0;
    double circle_x, circle_y;
    bool task_space_ = false;
    bool drawing_circle_ = false;
    bool drawing_line_ = false;
    Eigen::Matrix<double, 6, 1> ex_task_;
    Eigen::Matrix<double, 6, 1> xd_dot_task_, xd_ddot_task_;
    Eigen::Matrix<double, 6, 1> ex_dot_task_, xdot_task_;

    // save the data
    double SaveData_[SaveDataMax];
    std::vector<double> j1_avg;
    std::vector<double> j2_avg;
    std::vector<double> j3_avg;
    std::vector<double> j4_avg;
    std::vector<double> j5_avg;
    std::vector<double> j6_avg;

    std::vector<double> j1_vel_avg;
    std::vector<double> j2_vel_avg;
    std::vector<double> j3_vel_avg;
    std::vector<double> j4_vel_avg;
    std::vector<double> j5_vel_avg;
    std::vector<double> j6_vel_avg;

    // ros publisher
    ros::Publisher pub_qd_, pub_q_, pub_e_;
    ros::Publisher pub_SaveData_;

    // ros message
    std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
    std_msgs::Float64MultiArray msg_SaveData_;
};
}; // namespace arm_controllers
PLUGINLIB_EXPORT_CLASS(arm_controllers::DemoController, controller_interface::ControllerBase)