/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
#include <NTL/BasicThreadPool.h>

#include <helib/recryption.h>
#include <helib/EncryptedArray.h>
#include <helib/EvalMap.h>
#include <helib/powerful.h>
#include <helib/CtPtrs.h>
#include <helib/intraSlot.h>
#include <helib/norms.h>
#include <helib/sample.h>
#include <helib/debugging.h>
#include <helib/fhe_stats.h>
#include <helib/log.h>

#ifdef HELIB_DEBUG

#include <helib/debugging.h>

namespace helib {

long printFlag = FLAG_PRINT_VEC;
/************************ Some local functions ***********************/
/*********************************************************************/

static void checkCriticalValue(const std::vector<NTL::ZZX>& zzParts,
                               const DoubleCRT& sKey,
                               const RecryptData& rcData,
                               long q);

static void checkRecryptBounds(const std::vector<NTL::ZZX>& zzParts,
                               const DoubleCRT& sKey,
                               const Context& context,
                               long q);

static void checkRecryptBounds_v(const std::vector<NTL::ZZX>& v,
                                 const DoubleCRT& sKey,
                                 const Context& context,
                                 long q);
} // namespace helib

#endif // HELIB_DEBUG

namespace helib {

// Return in poly a polynomial with X^i encoded in all the slots
static void x2iInSlots(NTL::ZZX& poly,
                       long i,
                       std::vector<NTL::ZZX>& xVec,
                       const EncryptedArray& ea)
{
  xVec.resize(ea.size());
  NTL::ZZX x2i = NTL::ZZX(i, 1);
  for (long j = 0; j < (long)xVec.size(); j++)
    xVec[j] = x2i;
  ea.encode(poly, xVec);
}

// Make every entry of vec divisible by p2e by adding/subtracting q, while
// keeping the added multiples small.  Specifically, for q = 1 mod p2e and any
// integer z can be made divisible by p2e via z' = z + v*q, with |v| <= p2e/2.

static void newMakeDivisible(NTL::ZZX& poly,
                             long p2e,
                             long q,
                             const Context& context,
                             NTL::ZZX& vpoly)
{
  if (p2e == 1) {
    vpoly = 0;
    return;
  }

  assertTrue<InvalidArgument>(q > 0l, "q must be positive");
  assertTrue<InvalidArgument>(p2e > 0l, "p2e must be positive");

  assertEq<InvalidArgument>(q % p2e, 1l, "q must equal 1 modulo p2e");

  long p = context.getP();

  const RecryptData& rcData = context.getRcData();
  const PowerfulDCRT& p2d_conv = *rcData.p2dConv;

  NTL::Vec<NTL::ZZ> pwrfl;
  p2d_conv.ZZXtoPowerful(pwrfl, poly);

#ifdef HELIB_DEBUG
  NTL::Vec<NTL::ZZ> vvec(NTL::INIT_SIZE, pwrfl.length());
#endif

  for (long i : range(pwrfl.length())) {
    NTL::ZZ& z = pwrfl[i];
    long v;

    // What to add to z to make it divisible by p2e?
    long zMod = rem(z, p2e); // zMod is in [0,p2e-1]
    // NOTE: this makes sure we get a truly balanced remainder
    if (zMod > p2e / 2 || (p == 2 && zMod == p2e / 2 && NTL::RandomBnd(2))) {
      // randomize so that v has expected value 0
      zMod = p2e - zMod;
    } else {
      // need to add a negative number
      zMod = -zMod;
    }
    v = zMod;
    z += NTL::to_ZZ(q) * v; // make z divisible by p2e

    if (rem(z, p2e) != 0) { // sanity check
      std::cerr << "**error: original z[" << i
                << "]=" << (z - (NTL::to_ZZ(q) * v)) << std::dec
                << ", p^e=" << p2e << std::endl;
      std::cerr << "z' = z + " << v << "*q = " << z << std::endl;
      exit(1);
    }

#ifdef HELIB_DEBUG
    vvec[i] = v;
#endif
  }

  p2d_conv.powerfulToZZX(poly, pwrfl);

#ifdef HELIB_DEBUG
  p2d_conv.powerfulToZZX(vpoly, vvec);
#endif
}

/*********************************************************************/
/*********************************************************************/

/**
 * Summary of Appendix A from https://ia.cr/2014/873 (version from 2019):
 * Assume that we already chosen e, e' and t.
 *
 * Based in this analysis, we need
 *    (1) (f*p^{e'} + 2*p^r+2))*B <= p^e/2
 * where B is a certain high-probability bound and f is a certain
 * fudge factor.
 *
 **/

// the routine compute_fudge is used to correct for the fact that
// the v-coeffs are not quite uniform

static double compute_fudge(long p2ePrime, long p2e)
{
  double eps = 0;

  if (p2ePrime > 1) {

    if (p2ePrime % 2 == 0) {
      eps = 1 / fsquare(p2ePrime);

      // The exact variance in this case is at most the variance
      // of a random variable that is distributed over
      //    -N..+N
      // where N = 2^{e'}/2.
      // Each endpoint occurs with probability 1/(4*N),
      // and the remaining values each occur with the same probability
      // 1/(2*N)

      // This variance is exactly computed as
      //    (N^2)/3 + 1/6 = ((N^2)/3)*(1 + 1/(2*N^2)), where N = 2^{e'}/2
      // So the std dev is at most
      //    N/sqrt(3)*(1 + 1/(4*N^2))

    } else {
      eps = 1 / double(p2e);

      // We are computing X + Y mod p^{e'}, where
      // X and Y are independent.
      // Y is uniformly distributed over
      //    -floor(p^{r}/2)..floor(p^{r}/2)
      // X is distributed over
      //    -floor(p^e/2)-1..floor(p^e/2)+1,
      // where each endpoint occurs with probability 1 / (2*(p^e+1)),
      // and the remaining p^e values are equally likely

      // The variance in this case is bounded by
      //   (N^2)/3*(1-eps) + (N^2)*eps = (N^2)/3*(1+2*eps),
      //       where N = p^{e'}/2 and eps < 1/p^e
      // So the std dev is bounded by
      //    N/sqrt(3)*sqrt(1+2*eps) <= N/sqrt(3)*(1+eps)
    }
  }

  return 1 + eps;
}

void RecryptData::setAE(long& e, long& ePrime, const Context& context)
{
  double coeff_bound = context.boundForRecryption();
  // coeff_bound is ultimately a high prob bound on |w0+w1*s|,
  // the coeffs of w0, w1 are chosen uniformly on [-1/2,1/2]

  long p = context.getP();
  long p2r = context.getAlMod().getPPowR();
  long r = context.getAlMod().getR();
  long frstTerm = 2 * p2r + 2;

  long e_bnd = 0;
  long p2e_bnd = 1;
  while (p2e_bnd <= ((1L << 30) - 2) / p) { // NOTE: this avoids overflow
    e_bnd++;
    p2e_bnd *= p;
  }
  // e_bnd is largest e such that p^e+1 < 2^30

  // Start with the smallest e s.t. p^e/2 >= frstTerm*coeff_bound
  ePrime = 0;
  e = r + 1;
  while (e <= e_bnd && NTL::power_long(p, e) < frstTerm * coeff_bound * 2)
    e++;

  //  if (e > e_bnd) Error("setAE: cannot find suitable e");
  assertFalse<RuntimeError>(e > e_bnd, "setAE: cannot find suitable e");

  // long ePrimeTry = r+1;
  long ePrimeTry = 1;

  while (ePrimeTry <= e_bnd) {
    long p2ePrimeTry = NTL::power_long(p, ePrimeTry);
    // long eTry = ePrimeTry+1;
    long eTry = std::max(r + 1, ePrimeTry + 1);
    while (eTry <= e_bnd && eTry - ePrimeTry < e - ePrime) {
      long p2eTry = NTL::power_long(p, eTry);
      double fudge = compute_fudge(p2ePrimeTry, p2eTry);
      if (p2eTry >= (p2ePrimeTry * fudge + frstTerm) * coeff_bound * 2)
        break;

      eTry++;
    }

    if (eTry <= e_bnd && eTry - ePrimeTry < e - ePrime) {
      e = eTry;
      ePrime = ePrimeTry;
    }

    ePrimeTry++;
  }

#ifdef HELIB_DEBUG
  std::cerr << "RecryptData::setAE(): e=" << e << ", e'=" << ePrime
            << std::endl;
#endif
}

bool RecryptData::operator==(const RecryptData& other) const
{
  if (mvec != other.mvec)
    return false;

  if (skHwt != other.skHwt)
    return false;

  return true;
}

// The main method
void RecryptData::init(const Context& context,
                       const NTL::Vec<long>& mvec_,
                       bool enableThick,
                       bool build_cache_,
                       bool minimal)
{
  if (alMod != nullptr) { // were we called for a second time?
    std::cerr << "@Warning: multiple calls to RecryptData::init\n";
    return;
  }

  // sanity check
  assertEq(computeProd(mvec_),
           context.getM(),
           "Cyclotomic polynomial mismatch");

  // Record the arguments to this function
  mvec = mvec_;
  build_cache = build_cache_;
  alsoThick = enableThick;

  bool mvec_ok = true;
  for (long i : range(mvec.length())) {
    NTL::Vec<NTL::Pair<long, long>> factors;
    factorize(factors, mvec[i]);
    if (factors.length() > 1)
      mvec_ok = false;
  }

  if (!mvec_ok) {
    Warning("prime power factorization recommended for bootstrapping");
  }

  skHwt = context.getHwt();
  e = context.getE();
  ePrime = context.getEPrime();

  long r = context.getAlMod().getR();

  // First part of Bootstrapping works wrt plaintext space p^{r'}
  alMod = std::make_shared<PAlgebraMod>(context.getZMStar(), e - ePrime + r);
  ea = std::make_shared<EncryptedArray>(context, *alMod);
  // Polynomial defaults to F0, PAlgebraMod explicitly given

  p2dConv = std::make_shared<PowerfulDCRT>(context, mvec);

  if (!enableThick)
    return;

  // Initialize the linear polynomial for unpacking the slots
  NTL::zz_pBak bak;
  bak.save();
  ea->getAlMod().restoreContext();
  long nslots = ea->size();
  long d = ea->getDegree();

  const NTL::Mat<NTL::zz_p>& CBi =
      ea->getDerived(PA_zz_p()).getNormalBasisMatrixInverse();

  std::vector<NTL::ZZX> LM;
  LM.resize(d);
  for (long i = 0; i < d; i++) // prepare the linear polynomial
    LM[i] = rep(CBi[i][0]);

  std::vector<NTL::ZZX> C;
  ea->buildLinPolyCoeffs(C, LM); // "build" the linear polynomial

  unpackSlotEncoding.resize(d); // encode the coefficients

  for (long j = 0; j < d; j++) {
    std::vector<NTL::ZZX> v(nslots);
    for (long k = 0; k < nslots; k++)
      v[k] = C[j];
    ea->encode(unpackSlotEncoding[j], v);
  }
  firstMap = std::make_shared<EvalMap>(*ea, minimal, mvec, true, build_cache);
  secondMap = std::make_shared<EvalMap>(context.getEA(),
                                        minimal,
                                        mvec,
                                        false,
                                        build_cache);
}

/********************************************************************/
/********************************************************************/

// Extract digits from fully packed slots
void extractDigitsPacked(Ctxt& ctxt,
                         long botHigh,
                         long r,
                         long ePrime,
                         const std::vector<NTL::ZZX>& unpackSlotEncoding);

// Extract digits from unpacked slots
void extractDigitsThin(Ctxt& ctxt, long botHigh, long r, long ePrime);

// bootstrap a ciphertext to reduce noise
void PubKey::reCrypt(Ctxt& ctxt) const
{
  HELIB_TIMER_START;

  // Some sanity checks for dummy ciphertext
  long ptxtSpace = ctxt.getPtxtSpace();
  if (ctxt.isEmpty())
    return;
  if (ctxt.parts.size() == 1 && ctxt.parts[0].skHandle.isOne()) {
    // Dummy encryption, just ensure that it is reduced mod p
    NTL::ZZX poly = to_ZZX(ctxt.parts[0]);
    for (long i = 0; i < poly.rep.length(); i++)
      poly[i] = NTL::to_ZZ(rem(poly[i], ptxtSpace));
    poly.normalize();
    ctxt.DummyEncrypt(poly);
    return;
  }

  // check that we have bootstrapping data
  assertTrue(recryptKeyID >= 0l, "No bootstrapping data");

  long p = getContext().getP();
  long r = getContext().getAlMod().getR();
  long p2r = getContext().getAlMod().getPPowR();

  long intFactor = ctxt.intFactor;

  // the bootstrapping key is encrypted relative to plaintext space p^{e-e'+r}.
  const RecryptData& rcData = getContext().getRcData();
  long e = rcData.e;
  long ePrime = rcData.ePrime;
  long p2ePrime = NTL::power_long(p, ePrime);
  long q = NTL::power_long(p, e) + 1;
  assertTrue(e >= r, "rcData.e must be at least alMod.r");

#ifdef HELIB_DEBUG
  std::cerr << "reCrypt: p=" << p << ", r=" << r << ", e=" << e
            << " ePrime=" << ePrime << ", q=" << q << std::endl;
  CheckCtxt(ctxt, "init");
#endif

  // can only bootstrap ciphertext with plaintext-space dividing p^r
  assertEq(p2r % ptxtSpace, 0l, "ptxtSpace must divide p^r when bootstrapping");

  ctxt.dropSmallAndSpecialPrimes();

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after mod down");
#endif

  HELIB_NTIMER_START(AAA_preProcess);

  // Make sure that this ciphertext is in canonical form
  if (!ctxt.inCanonicalForm())
    ctxt.reLinearize();

  // Mod-switch down if needed
  IndexSet s = ctxt.getPrimeSet() / context.getSpecialPrimes();
  assertTrue(s <= context.getCtxtPrimes(), "prime set is messed up");
  if (s.card() > 3) { // leave only first three ciphertext primes
    long first = s.first();
    IndexSet s3(first, first + 2);
    s.retain(s3);
  }
  ctxt.modDownToSet(s);

  // key-switch to the bootstrapping key
  ctxt.reLinearize(recryptKeyID);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after key switching");
#endif

  // "raw mod-switch" to the bootstrapping modulus q=p^e+1.
  std::vector<NTL::ZZX> zzParts; // the mod-switched parts, in ZZX format

  double mfac = ctxt.getContext().getZMStar().getNormBnd();
  double noise_est = ctxt.rawModSwitch(zzParts, q) * mfac;
  // noise_est is an upper bound on the L-infty norm of the scaled noise
  // in the pwrfl basis
  double noise_bnd =
      HELIB_MIN_CAP_FRAC * p2r * ctxt.getContext().boundForRecryption();
  // noise_bnd is the bound assumed in selecting the parameters
  double noise_rat = noise_est / noise_bnd;

  HELIB_STATS_UPDATE("raw-mod-switch-noise", noise_rat);

  if (noise_rat > 1) {
    // TODO: Turn the following preprocessor logics into a warnOrThrow function
    std::string message =
        "rawModSwitch scaled noise exceeds bound: " + std::to_string(noise_rat);
#ifdef HELIB_DEBUG
    Warning(message);
#else
    throw LogicError(message);
#endif
  }

  assertEq(zzParts.size(),
           (std::size_t)2,
           "Exactly 2 parts required for mod-switching in thin bootstrapping");

#ifdef HELIB_DEBUG
  if (dbgKey) {
    checkRecryptBounds(zzParts, dbgKey->getRecryptKey(), ctxt.getContext(), q);
  }
#endif

  std::vector<NTL::ZZX> v;
  v.resize(2);

  // Add multiples of q to make the zzParts divisible by p^{e'}
  for (long i : range(2)) {
    // make divisible by p^{e'}

    newMakeDivisible(zzParts[i], p2ePrime, q, ctxt.getContext(), v[i]);
  }

#ifdef HELIB_DEBUG
  if (dbgKey) {
    checkRecryptBounds_v(v, dbgKey->getRecryptKey(), ctxt.getContext(), q);
    checkCriticalValue(zzParts,
                       dbgKey->getRecryptKey(),
                       ctxt.getContext().getRcData(),
                       q);
  }
#endif

  for (long i : range(zzParts.size())) {
    zzParts[i] /= p2ePrime; // divide by p^{e'}
  }

  // NOTE: here we lose the intFactor associated with ctxt.
  // We will restore it below.
  ctxt = recryptEkey;

  ctxt.multByConstant(zzParts[1]);
  ctxt.addConstant(zzParts[0]);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after preProcess");
#endif
  HELIB_NTIMER_STOP(AAA_preProcess);

  // Move the powerful-basis coefficients to the plaintext slots
  HELIB_NTIMER_START(AAA_LinearTransform1);
  ctxt.getContext().getRcData().firstMap->apply(ctxt);
  HELIB_NTIMER_STOP(AAA_LinearTransform1);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after LinearTransform1");
#endif

  // Extract the digits e-e'+r-1,...,e-e' (from fully packed slots)
  HELIB_NTIMER_START(AAA_extractDigitsPacked);
  extractDigitsPacked(ctxt,
                      e - ePrime,
                      r,
                      ePrime,
                      context.getRcData().unpackSlotEncoding);
  HELIB_NTIMER_STOP(AAA_extractDigitsPacked);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after extractDigitsPacked");
#endif

  // Move the slots back to powerful-basis coefficients
  HELIB_NTIMER_START(AAA_LinearTransform2);
  ctxt.getContext().getRcData().secondMap->apply(ctxt);
  HELIB_NTIMER_STOP(AAA_LinearTransform2);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after linearTransform2");
#endif

  // restore intFactor
  if (intFactor != 1)
    ctxt.intFactor = NTL::MulMod(ctxt.intFactor, intFactor, ptxtSpace);
}

#ifdef HELIB_BOOT_THREADS

// Extract digits from fully packed slots, multithreaded version
void extractDigitsPacked(Ctxt& ctxt,
                         long botHigh,
                         long r,
                         long ePrime,
                         const std::vector<NTL::ZZX>& unpackSlotEncoding)
{
  HELIB_TIMER_START;

  // Step 1: unpack the slots of ctxt
  HELIB_NTIMER_START(unpack);
  ctxt.cleanUp();

  // Apply the d automorphisms and store them in scratch area
  long d = ctxt.getContext().getOrdP();

  std::vector<Ctxt> unpacked(d, Ctxt(ZeroCtxtLike, ctxt));
  { // explicit scope to force all temporaries to be released
    std::vector<std::shared_ptr<DoubleCRT>> coeff_vector;
    std::vector<double> coeff_vector_sz;
    coeff_vector.resize(d);
    coeff_vector_sz.resize(d);

    HELIB_NTIMER_START(unpack1);
    for (long i = 0; i < d; i++) {
      coeff_vector[i] = std::make_shared<DoubleCRT>(unpackSlotEncoding[i],
                                                    ctxt.getContext(),
                                                    ctxt.getPrimeSet());
      coeff_vector_sz[i] = NTL::conv<double>(
          embeddingLargestCoeff(unpackSlotEncoding[i],
                                ctxt.getContext().getZMStar()));
    }
    HELIB_NTIMER_STOP(unpack1);

    HELIB_NTIMER_START(unpack2);
    std::vector<Ctxt> frob(d, Ctxt(ZeroCtxtLike, ctxt));

    NTL_EXEC_RANGE(d, first, last)
    // FIXME: implement using hoisting!
    for (long j = first; j < last; j++) { // process jth Frobenius
      frob[j] = ctxt;
      frob[j].frobeniusAutomorph(j);
      frob[j].cleanUp();
      // FIXME: not clear if we should call cleanUp here
    }
    NTL_EXEC_RANGE_END

    HELIB_NTIMER_STOP(unpack2);

    HELIB_NTIMER_START(unpack3);
    Ctxt tmp1(ZeroCtxtLike, ctxt);
    for (long i = 0; i < d; i++) {
      for (long j = 0; j < d; j++) {
        tmp1 = frob[j];
        tmp1.multByConstant(*coeff_vector[mcMod(i + j, d)],
                            coeff_vector_sz[mcMod(i + j, d)]);
        unpacked[i] += tmp1;
      }
    }
    HELIB_NTIMER_STOP(unpack3);
  }
  HELIB_NTIMER_STOP(unpack);

  //#ifdef HELIB_DEBUG
  //  CheckCtxt(unpacked[0], "after unpack");
  //#endif

  NTL_EXEC_RANGE(d, first, last)
  for (long i = first; i < last; i++) {
    extractDigitsThin(unpacked[i], botHigh, r, ePrime);
  }
  NTL_EXEC_RANGE_END

  //#ifdef HELIB_DEBUG
  // CheckCtxt(unpacked[0], "before repack");
  //#endif

  // Step 3: re-pack the slots
  HELIB_NTIMER_START(repack);
  const EncryptedArray& ea2 = ctxt.getContext().getEA();
  NTL::ZZX xInSlots;
  std::vector<NTL::ZZX> xVec(ea2.size());
  ctxt = unpacked[0];
  for (long i = 1; i < d; i++) {
    x2iInSlots(xInSlots, i, xVec, ea2);
    unpacked[i].multByConstant(xInSlots);
    ctxt += unpacked[i];
  }
  HELIB_NTIMER_STOP(repack);
  //#ifdef HELIB_DEBUG
  // CheckCtxt(ctxt, "after repack");
  //#endif
}

#else

// Extract digits from fully packed slots
void extractDigitsPacked(Ctxt& ctxt,
                         long botHigh,
                         long r,
                         long ePrime,
                         const std::vector<NTL::ZZX>& unpackSlotEncoding)
{
  HELIB_TIMER_START;

  // Step 1: unpack the slots of ctxt
  HELIB_NTIMER_START(unpack);
  ctxt.cleanUp();

  // Apply the d automorphisms and store them in scratch area
  long d = ctxt.getContext().getOrdP();

  std::vector<Ctxt> unpacked(d, Ctxt(ZeroCtxtLike, ctxt));
  { // explicit scope to force all temporaries to be released
    std::vector<std::shared_ptr<DoubleCRT>> coeff_vector;
    std::vector<double> coeff_vector_sz;
    coeff_vector.resize(d);
    coeff_vector_sz.resize(d);
    for (long i = 0; i < d; i++) {
      coeff_vector[i] = std::make_shared<DoubleCRT>(unpackSlotEncoding[i],
                                                    ctxt.getContext(),
                                                    ctxt.getPrimeSet());
      coeff_vector_sz[i] = NTL::conv<double>(
          embeddingLargestCoeff(unpackSlotEncoding[i],
                                ctxt.getContext().getZMStar()));
    }

    Ctxt tmp1(ZeroCtxtLike, ctxt);
    Ctxt tmp2(ZeroCtxtLike, ctxt);

    // FIXME: implement using hoisting!
    for (long j = 0; j < d; j++) { // process jth Frobenius
      tmp1 = ctxt;
      tmp1.frobeniusAutomorph(j);
      tmp1.cleanUp();
      // FIXME: not clear if we should call cleanUp here

      for (long i = 0; i < d; i++) {
        tmp2 = tmp1;
        tmp2.multByConstant(*coeff_vector[mcMod(i + j, d)],
                            coeff_vector_sz[mcMod(i + j, d)]);
        unpacked[i] += tmp2;
      }
    }
  }
  HELIB_NTIMER_STOP(unpack);

  //#ifdef HELIB_DEBUG
  //  CheckCtxt(unpacked[0], "after unpack");
  //#endif

  for (long i = 0; i < (long)unpacked.size(); i++) {
    extractDigitsThin(unpacked[i], botHigh, r, ePrime);
  }

  //#ifdef HELIB_DEBUG
  //  CheckCtxt(unpacked[0], "before repack");
  //#endif

  // Step 3: re-pack the slots
  HELIB_NTIMER_START(repack);
  const EncryptedArray& ea2 = *ctxt.getContext().ea;
  NTL::ZZX xInSlots;
  std::vector<NTL::ZZX> xVec(ea2.size());
  ctxt = unpacked[0];
  for (long i = 1; i < d; i++) {
    x2iInSlots(xInSlots, i, xVec, ea2);
    unpacked[i].multByConstant(xInSlots);
    ctxt += unpacked[i];
  }
  HELIB_NTIMER_STOP(repack);
}

#endif

// Use packed bootstrapping, so we can bootstrap all in just one go.
void packedRecrypt(const CtPtrs& cPtrs,
                   const std::vector<zzX>& unpackConsts,
                   const EncryptedArray& ea)
{
  PubKey& pKey = (PubKey&)cPtrs[0]->getPubKey();

  // Allocate temporary ciphertexts for the recryption
  int nPacked = divc(cPtrs.size(), ea.getDegree()); // ceil(totalNum/d)
  std::vector<Ctxt> cts(nPacked, Ctxt(pKey));

  repack(CtPtrs_vectorCt(cts), cPtrs, ea); // pack ciphertexts
  //  cout << "@"<< lsize(cts)<<std::flush;
  for (Ctxt& c : cts) {   // then recrypt them
    c.reducePtxtSpace(2); // we only have recryption data for binary ctxt
    pKey.reCrypt(c);
  }
  unpack(cPtrs, CtPtrs_vectorCt(cts), ea, unpackConsts);
}

// recrypt all ctxt at level < belowLvl
void packedRecrypt(const CtPtrs& array,
                   const std::vector<zzX>& unpackConsts,
                   const EncryptedArray& ea,
                   long belowLvl)
{
  std::vector<Ctxt*> v;
  for (long i = 0; i < array.size(); i++)
    if (array.isSet(i) && !array[i]->isEmpty() &&
        array[i]->bitCapacity() < belowLvl * (array[i]->getContext().BPL()))
      v.push_back(array[i]);
  packedRecrypt(CtPtrs_vectorPt(v), unpackConsts, ea);
}
void packedRecrypt(const CtPtrMat& m,
                   const std::vector<zzX>& unpackConsts,
                   const EncryptedArray& ea,
                   long belowLvl)
{
  std::vector<Ctxt*> v;
  for (long i = 0; i < m.size(); i++)
    for (long j = 0; j < m[i].size(); j++)
      if (m[i].isSet(j) && !m[i][j]->isEmpty() &&
          m[i][j]->bitCapacity() < belowLvl * (m[i][j]->getContext().BPL()))
        v.push_back(m[i][j]);
  packedRecrypt(CtPtrs_vectorPt(v), unpackConsts, ea);
}

//===================== Thin Bootstrapping stuff ==================

void ThinRecryptData::init(const Context& context,
                           const NTL::Vec<long>& mvec_,
                           bool alsoThick,
                           bool build_cache_,
                           bool minimal)
{
  RecryptData::init(context, mvec_, alsoThick, build_cache_, minimal);
  coeffToSlot =
      std::make_shared<ThinEvalMap>(*ea, minimal, mvec, true, build_cache);
  slotToCoeff = std::make_shared<ThinEvalMap>(context.getEA(),
                                              minimal,
                                              mvec,
                                              false,
                                              build_cache);
}

// Extract digits from thinly packed slots

long fhe_force_chen_han = 0;

void extractDigitsThin(Ctxt& ctxt, long botHigh, long r, long ePrime)
{
  HELIB_TIMER_START;

  Ctxt unpacked(ctxt);
  unpacked.cleanUp();

  std::vector<Ctxt> scratch;

  long p = ctxt.getContext().getP();
  long p2r = NTL::power_long(p, r);
  long topHigh = botHigh + r - 1;

  // degree Chen/Han technique is p^{bot-1}(p-1)r
  // degree of basic technique is p^{bot-1}p^r,
  //     or p^{bot-1}p^{r-1} if p==2, r > 1, and bot+r > 2

  bool use_chen_han = false;
  if (r > 1) {
    double chen_han_cost = log(p - 1) + log(r);
    double basic_cost;
    if (p == 2 && r > 2 && botHigh + r > 2)
      basic_cost = (r - 1) * log(p);
    else
      basic_cost = r * log(p);

    // std::cerr << "*** basic: " << basic_cost << "\n";
    // std::cerr << "*** chen/han: " << chen_han_cost << "\n";

    double thresh = 1.5;
    if (p == 2)
      thresh = 1.75;
    // increasing thresh makes chen_han less likely to be chosen.
    // For p == 2, the basic algorithm is just squaring,
    // and so is a bit cheaper, so we raise thresh a bit.
    // This is all a bit heuristic.

    if (basic_cost > thresh * chen_han_cost)
      use_chen_han = true;
  }

  if (fhe_force_chen_han > 0)
    use_chen_han = true;
  else if (fhe_force_chen_han < 0)
    use_chen_han = false;

  if (use_chen_han) {
    // use Chen and Han technique

    extendExtractDigits(scratch, unpacked, botHigh, r);

#if 0
    for (long i: range(scratch.size())) {
      CheckCtxt(scratch[i], "**");
    }
#endif

    for (long j = 0; j < botHigh; j++) {
      unpacked -= scratch[j];
      unpacked.divideByP();
    }

    if (p == 2 && botHigh > 0) // For p==2, subtract also the previous bit
      unpacked += scratch[botHigh - 1];
    unpacked.negate();

    if (r > ePrime) { // Add in digits from the bottom part, if any
      long topLow = r - 1 - ePrime;
      Ctxt tmp = scratch[topLow];
      for (long j = topLow - 1; j >= 0; --j) {
        tmp.multByP();
        tmp += scratch[j];
      }
      if (ePrime > 0)
        tmp.multByP(ePrime); // multiply by p^e'
      unpacked += tmp;
    }
    unpacked.reducePtxtSpace(p2r); // Our plaintext space is now mod p^r

    ctxt = unpacked;
  } else {

    if (p == 2 && r > 2 && topHigh + 1 > 2)
      topHigh--; // For p==2 we sometime get a bit for free

    extractDigits(scratch, unpacked, topHigh + 1);

    // set unpacked = -\sum_{j=botHigh}^{topHigh} scratch[j] * p^{j-botHigh}
    if (topHigh >= LONG(scratch.size())) {
      topHigh = scratch.size() - 1;
      std::cerr << " @ suspect: not enough digits in extractDigitsPacked\n";
    }

    unpacked = scratch[topHigh];
    for (long j = topHigh - 1; j >= botHigh; --j) {
      unpacked.multByP();
      unpacked += scratch[j];
    }
    if (p == 2 && botHigh > 0) // For p==2, subtract also the previous bit
      unpacked += scratch[botHigh - 1];
    unpacked.negate();

    if (r > ePrime) { // Add in digits from the bottom part, if any
      long topLow = r - 1 - ePrime;
      Ctxt tmp = scratch[topLow];
      for (long j = topLow - 1; j >= 0; --j) {
        tmp.multByP();
        tmp += scratch[j];
      }
      if (ePrime > 0)
        tmp.multByP(ePrime); // multiply by p^e'
      unpacked += tmp;
    }
    unpacked.reducePtxtSpace(p2r); // Our plaintext space is now mod p^r
    ctxt = unpacked;
  }
}

// Hack to get at private fields of public key
struct PubKeyHack
{                         // The public key
  const Context& context; // The context

  //! @var Ctxt pubEncrKey
  //! The public encryption key is an encryption of 0,
  //! relative to the first secret key
  Ctxt pubEncrKey;

  std::vector<long> skHwts;            // The Hamming weight of the secret keys
  std::vector<KeySwitch> keySwitching; // The key-switching matrices

  // The keySwitchMap structure contains pointers to key-switching matrices
  // for re-linearizing automorphisms. The entry keySwitchMap[i][n] contains
  // the index j such that keySwitching[j] is the first matrix one needs to
  // use when re-linearizing s_i(X^n).
  std::vector<std::vector<long>> keySwitchMap;

  NTL::Vec<int> KS_strategy; // NTL Vec's support I/O, which is
                             // more convenient

  // bootstrapping data

  long recryptKeyID; // index of the bootstrapping key
  Ctxt recryptEkey;  // the key itself, encrypted under key #0
};

// bootstrap a ciphertext to reduce noise
void PubKey::thinReCrypt(Ctxt& ctxt) const
{
  HELIB_TIMER_START;

  // Some sanity checks for dummy ciphertext
  long ptxtSpace = ctxt.getPtxtSpace();
  if (ctxt.isEmpty())
    return;

  if (ctxt.parts.size() == 1 && ctxt.parts[0].skHandle.isOne()) {
    // Dummy encryption, just ensure that it is reduced mod p
    NTL::ZZX poly = to_ZZX(ctxt.parts[0]);
    for (long i = 0; i < poly.rep.length(); i++)
      poly[i] = NTL::to_ZZ(rem(poly[i], ptxtSpace));
    poly.normalize();
    ctxt.DummyEncrypt(poly);
    return;
  }

  // check that we have bootstrapping data
  assertTrue(recryptKeyID >= 0l, "Bootstrapping data not present");

  long p = ctxt.getContext().getP();
  long r = ctxt.getContext().getAlMod().getR();
  long p2r = ctxt.getContext().getAlMod().getPPowR();

  long intFactor = ctxt.intFactor;

  const ThinRecryptData& trcData = ctxt.getContext().getRcData();

  // the bootstrapping key is encrypted relative to plaintext space p^{e-e'+r}.
  long e = trcData.e;
  long ePrime = trcData.ePrime;
  long p2ePrime = NTL::power_long(p, ePrime);
  long q = NTL::power_long(p, e) + 1;
  assertTrue(e >= r, "trcData.e must be at least alMod.r");

  // can only bootstrap ciphertext with plaintext-space dividing p^r
  assertEq(p2r % ptxtSpace,
           0l,
           "ptxtSpace must divide p^r when thin bootstrapping");

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "init");
#endif

  ctxt.dropSmallAndSpecialPrimes();

#define DROP_BEFORE_THIN_RECRYPT
#define THIN_RECRYPT_NLEVELS (3)
#ifdef DROP_BEFORE_THIN_RECRYPT
  // experimental code...we should drop down to a reasonably low level
  // before doing the first linear map.
  long first = context.getCtxtPrimes().first();
  long last = std::min(context.getCtxtPrimes().last(),
                       first + THIN_RECRYPT_NLEVELS - 1);
  ctxt.bringToSet(IndexSet(first, last));
#endif

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after mod down");
#endif

  // Move the slots to powerful-basis coefficients
  HELIB_NTIMER_START(AAA_slotToCoeff);
  trcData.slotToCoeff->apply(ctxt);
  HELIB_NTIMER_STOP(AAA_slotToCoeff);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after slotToCoeff");
#endif

  HELIB_NTIMER_START(AAA_bootKeySwitch);

  // Make sure that this ciphertext is in canonical form
  if (!ctxt.inCanonicalForm())
    ctxt.reLinearize();

  // Mod-switch down if needed
  IndexSet s = ctxt.getPrimeSet() / context.getSpecialPrimes();
  assertTrue(s <= context.getCtxtPrimes(), "prime set is messed up");
  if (s.card() > 3) { // leave only first three ciphertext primes
    long first = s.first();
    IndexSet s3(first, first + 2);
    s.retain(s3);
  }
  ctxt.modDownToSet(s);

  // key-switch to the bootstrapping key
  ctxt.reLinearize(recryptKeyID);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after key switching");
#endif

  // "raw mod-switch" to the bootstrapping mosulus q=p^e+1.
  std::vector<NTL::ZZX> zzParts; // the mod-switched parts, in ZZX format

  double mfac = ctxt.getContext().getZMStar().getNormBnd();
  double noise_est = ctxt.rawModSwitch(zzParts, q) * mfac;
  // noise_est is an upper bound on the L-infty norm of the scaled noise
  // in the pwrfl basis
  double noise_bnd =
      HELIB_MIN_CAP_FRAC * p2r * ctxt.getContext().boundForRecryption();
  // noise_bnd is the bound assumed in selecting the parameters
  double noise_rat = noise_est / noise_bnd;

  HELIB_STATS_UPDATE("raw-mod-switch-noise", noise_rat);

  if (noise_rat > 1) {
    // TODO: Turn the following preprocessor logics into a warnOrThrow function
    std::string message =
        "rawModSwitch scaled noise exceeds bound: " + std::to_string(noise_rat);
#ifdef HELIB_DEBUG
    Warning(message);
#else
    throw LogicError(message);
#endif
  }

  assertEq(zzParts.size(),
           (std::size_t)2,
           "Exactly 2 parts required for mod-switching in thin bootstrapping");

#ifdef HELIB_DEBUG
  if (dbgKey) {
    checkRecryptBounds(zzParts, dbgKey->getRecryptKey(), ctxt.getContext(), q);
  }
#endif

  std::vector<NTL::ZZX> v;
  v.resize(2);

  // Add multiples of q to make the zzParts divisible by p^{e'}
  for (long i : range(2)) {
    // make divisible by p^{e'}

    newMakeDivisible(zzParts[i], p2ePrime, q, ctxt.getContext(), v[i]);
  }

#ifdef HELIB_DEBUG
  if (dbgKey) {
    checkRecryptBounds_v(v, dbgKey->getRecryptKey(), ctxt.getContext(), q);
    checkCriticalValue(zzParts,
                       dbgKey->getRecryptKey(),
                       ctxt.getContext().getRcData(),
                       q);
  }
#endif

  for (long i : range(zzParts.size())) {
    zzParts[i] /= p2ePrime; // divide by p^{e'}
  }

  // NOTE: here we lose the intFactor associated with ctxt.
  // We will restore it below.
  ctxt = recryptEkey;

  ctxt.multByConstant(zzParts[1]);
  ctxt.addConstant(zzParts[0]);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after bootKeySwitch");
#endif

  HELIB_NTIMER_STOP(AAA_bootKeySwitch);

  // Move the powerful-basis coefficients to the plaintext slots
  HELIB_NTIMER_START(AAA_coeffToSlot);
  trcData.coeffToSlot->apply(ctxt);
  HELIB_NTIMER_STOP(AAA_coeffToSlot);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after coeffToSlot");
#endif

  // Extract the digits e-e'+r-1,...,e-e' (from fully packed slots)
  HELIB_NTIMER_START(AAA_extractDigitsThin);
  extractDigitsThin(ctxt, e - ePrime, r, ePrime);
  HELIB_NTIMER_STOP(AAA_extractDigitsThin);

#ifdef HELIB_DEBUG
  CheckCtxt(ctxt, "after extractDigitsThin");
#endif

  // restore intFactor
  if (intFactor != 1)
    ctxt.intFactor = NTL::MulMod(ctxt.intFactor, intFactor, ptxtSpace);
}

#ifdef HELIB_DEBUG

static void checkCriticalValue(const std::vector<NTL::ZZX>& zzParts,
                               const DoubleCRT& sKey,
                               const RecryptData& rcData,
                               long q)
{
  NTL::ZZX ptxt;
  rawDecrypt(ptxt, zzParts, sKey); // no mod q

  NTL::Vec<NTL::ZZ> powerful;
  rcData.p2dConv->ZZXtoPowerful(powerful, ptxt);
  NTL::xdouble max_pwrfl = NTL::conv<NTL::xdouble>(largestCoeff(powerful));
  double critical_value = NTL::conv<double>((max_pwrfl / q) / q);

  vecRed(powerful, powerful, q, false);
  max_pwrfl = NTL::conv<NTL::xdouble>(largestCoeff(powerful));
  critical_value += NTL::conv<double>(max_pwrfl / q);

  HELIB_STATS_UPDATE("critical-value", critical_value);

  std::cerr << "=== critical_value=" << critical_value;
  if (critical_value > 0.5)
    std::cerr << " BAD-BOUND";

  std::cerr << "\n";
}

static void checkRecryptBounds(const std::vector<NTL::ZZX>& zzParts,
                               const DoubleCRT& sKey,
                               const Context& context,
                               long q)
{
  const RecryptData& rcData = context.getRcData();
  double coeff_bound = context.boundForRecryption();
  long p2r = context.getAlMod().getPPowR();

  NTL::ZZX ptxt;
  rawDecrypt(ptxt, zzParts, sKey); // no mod q

  NTL::Vec<NTL::ZZ> powerful;
  rcData.p2dConv->ZZXtoPowerful(powerful, ptxt);
  double max_pwrfl = NTL::conv<double>(largestCoeff(powerful));
  double ratio = max_pwrfl / (2 * q * coeff_bound);

  HELIB_STATS_UPDATE("|x|/bound", ratio);

  std::cerr << "=== |x|/bound=" << ratio;
  if (ratio > 1.0)
    std::cerr << " BAD-BOUND";

  vecRed(powerful, powerful, q, false);
  max_pwrfl = NTL::conv<double>(largestCoeff(powerful));
  ratio = max_pwrfl / (2 * p2r * coeff_bound);

  HELIB_STATS_UPDATE("|x%q|/bound", ratio);

  std::cerr << ", (|x%q|)/bound=" << ratio;
  if (ratio > 1.0)
    std::cerr << " BAD-BOUND";

  std::cerr << "\n";
}

static void checkRecryptBounds_v(const std::vector<NTL::ZZX>& v,
                                 const DoubleCRT& sKey,
                                 const Context& context,
                                 UNUSED long q)
{
  const RecryptData& rcData = context.getRcData();

  long p = context.getP();
  long e = rcData.e;
  long p2e = NTL::power_long(p, e);
  long ePrime = rcData.ePrime;
  long p2ePrime = NTL::power_long(p, ePrime);
  long phim = context.getPhiM();

  double fudge = compute_fudge(p2ePrime, p2e);

  double coeff_bound = context.boundForRecryption() * fudge;

  double sigma = context.stdDevForRecryption() * fudge;

  NTL::ZZX ptxt;
  rawDecrypt(ptxt, v, sKey); // no mod q

  NTL::Vec<NTL::ZZ> powerful;
  rcData.p2dConv->ZZXtoPowerful(powerful, ptxt);
  double max_pwrfl = NTL::conv<double>(largestCoeff(powerful));

  double denom = p2ePrime * coeff_bound;
  double ratio = max_pwrfl / denom;

  HELIB_STATS_UPDATE("|v|/bound", ratio);

  std::cerr << "=== |v|/bound=" << ratio;
  if (ratio > 1.0)
    std::cerr << " BAD-BOUND";
  std::cerr << "\n";

  ptxt -= v[0]; // so now ptxt is just sKey * v[1]
  rcData.p2dConv->ZZXtoPowerful(powerful, ptxt);

  assertEq(powerful.length(), phim, "length should be phim");

  double ran_pwrfl = NTL::conv<double>(powerful[NTL::RandomBnd(phim)]);
  // pick a random coefficient in the poweful basis

  double std_devs = fabs(ran_pwrfl) / (p2ePrime * sigma);
  // number of standard deviations away from mean

  // update various indicator variables
  HELIB_STATS_UPDATE("sigma_0_5", double(std_devs <= 0.5)); // 0.383
  HELIB_STATS_UPDATE("sigma_1_0", double(std_devs <= 1.0)); // 0.683
  HELIB_STATS_UPDATE("sigma_1_5", double(std_devs <= 1.5)); // 0.866
  HELIB_STATS_UPDATE("sigma_2_0", double(std_devs <= 2.0)); // 0.954
  HELIB_STATS_UPDATE("sigma_2_5", double(std_devs <= 2.5)); // 0.988
  HELIB_STATS_UPDATE("sigma_3_0", double(std_devs <= 3.0)); // 0.997, 1 in 370
  HELIB_STATS_UPDATE("sigma_3_5",
                     double(std_devs <= 3.5)); // 0.999535, 1 in 2149
  HELIB_STATS_UPDATE("sigma_4_0",
                     double(std_devs <= 4.0)); // 0.999937, 1 in 15787

  // compute sample variance, and scale by the variance we expect
  HELIB_STATS_UPDATE("sigma_calc",
                     fsquare(ran_pwrfl) / fsquare(p2ePrime * sigma));

  // save the scaled value for application of other tests
  HELIB_STATS_SAVE("v_values", ran_pwrfl / (p2ePrime * sigma));
}

#endif

#if 0
void fhe_stats_print(long iter, const Context& context)
{
   long phim = context.getPhiM();

   std::cerr << "||||| recryption stats ||||\n";
   std::cerr << "**** averages ****\n";
   std::cerr << "=== critical_value=" << (fhe_stats_cv_sum/iter) << "\n";
   std::cerr << "=== |x|/bound=" << (fhe_stats_x_sum/iter) << "\n";
   std::cerr << "=== |x%q|/bound=" << (fhe_stats_xmod_sum/iter) << "\n";
   std::cerr << "=== |u|/bound=" << (fhe_stats_u_sum/iter) << "\n";
   std::cerr << "=== |v|/bound=" << (fhe_stats_v_sum/iter) << "\n";
   std::cerr << "**** maxima ****\n";
   std::cerr << "=== critical_value=" << (fhe_stats_cv_max) << "\n";
   std::cerr << "=== |x|/bound=" << (fhe_stats_x_max) << "\n";
   std::cerr << "=== |x%q|/bound=" << (fhe_stats_xmod_max) << "\n";
   std::cerr << "=== |u|/bound=" << (fhe_stats_u_max) << "\n";
   std::cerr << "=== |v|/bound=" << (fhe_stats_v_max) << "\n";
   std::cerr << "**** theoretical bounds ***\n";
   std::cerr << "=== single-max=" << (sqrt(2.0*log(phim))/context.scale) << "\n";
   std::cerr << "=== global-max=" << (sqrt(2.0*(log(iter)+log(phim)))/context.scale) << "\n";

}
#endif

} // namespace helib


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_MAX_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE4_1_SIMD_FUNCTION_MAX_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( max_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_max_epi8(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( max_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_max_epu16(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( max_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_max_epu32(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( max_
                          , (typename A0)
                          , bs::sse4_1_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_max_epi32(a0,a1);
    }
  };
} } }

#endif


<<<<<<< HEAD
/*    Copyright (c) 2010-2017, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <Tudat/SimulationSetup/tudatEstimationHeader.h>
#include "Tudat/SimulationSetup/PropagationSetup/propagationLambertTargeterFullProblem.h"
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <Eigen/Core>
#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/Ephemerides/approximatePlanetPositions.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/trajectory.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/exportTrajectory.h"
#include "Tudat/Astrodynamics/TrajectoryDesign/planetTrajectory.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat;

//! Test if the difference between the Lambert targeter solution and the full dynamics problem is computed correctly.
BOOST_AUTO_TEST_CASE( testFullPropagationLambertTargeter )
{

    std::cout.precision(20);

    double initialTime = 0.0;
    double fixedStepSize = 1.0;

    Eigen::Vector3d cartesianPositionAtDeparture ( 2.0 * 6.378136e6, 0.0, 0.0 );
    Eigen::Vector3d cartesianPositionAtArrival ( 2.0 * 6.378136e6, 2.0 * std::sqrt( 3.0 ) * 6.378136e6, 0.0 );

    double timeOfFlight = 806.78 * 5.0;

    std::string bodyToPropagate = "spacecraft" ;
    std::string centralBody = "Earth";

    // Define integrator settings.
    std::shared_ptr< numerical_integrators::IntegratorSettings< double > > integratorSettings =
            std::make_shared < numerical_integrators::IntegratorSettings < > >
                ( numerical_integrators::rungeKutta4, initialTime, fixedStepSize);

    std::vector< std::string > departureAndArrivalBodies;
    departureAndArrivalBodies.push_back("departure");
    departureAndArrivalBodies.push_back("arrival");

    // Define the body map.
    simulation_setup::NamedBodyMap bodyMap = propagators::setupBodyMapFromUserDefinedStatesForLambertTargeter("Earth", "spacecraft", departureAndArrivalBodies,
                                                                                cartesianPositionAtDeparture, cartesianPositionAtArrival);

    basic_astrodynamics::AccelerationMap accelerationModelMap = propagators::setupAccelerationMapLambertTargeter(
                "Earth", "spacecraft", bodyMap);

   // Compute the difference in state between the full problem and the Lambert targeter solution at departure and at arrival
   std::pair< Eigen::Vector6d, Eigen::Vector6d > differenceState =
            propagators::getDifferenceFullPropagationWrtLambertTargeterAtDepartureAndArrival(cartesianPositionAtDeparture,
             cartesianPositionAtArrival, timeOfFlight, initialTime, bodyMap, accelerationModelMap, bodyToPropagate,
             centralBody, integratorSettings, departureAndArrivalBodies, false);

    Eigen::Vector6d differenceStateAtDeparture = differenceState.first;
    Eigen::Vector6d differenceStateAtArrival = differenceState.second;

    std::cout << "differenceStateAtDeparture: " << differenceStateAtDeparture << "\n\n";
    std::cout << "differenceStateAtArrival: " << differenceStateAtArrival << "\n\n";

    for( int i = 0; i < 3; i++ )
    {
        BOOST_CHECK_SMALL( std::fabs( differenceStateAtDeparture( i ) ), 1.0 );
        BOOST_CHECK_SMALL( std::fabs( differenceStateAtDeparture( i + 3 ) ), 1.0E-6 );
        BOOST_CHECK_SMALL( std::fabs( differenceStateAtArrival( i ) ), 1.0 );
        BOOST_CHECK_SMALL( std::fabs( differenceStateAtArrival( i + 3 ) ), 1.0E-6 );
    }

}

}

}


#include "../include/grasp_sampler/ArmPlanner.h"
#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/bind.hpp>
#include <boost/algorithm/string.hpp>
FILE *fp1 = fopen("/home/min/catkin_ws/src/position_data.txt","w");

#define DEGREE M_PI/180.0

using std::ofstream;

ArmPlanner::ArmPlanner(YAMLConfig yaml_config):
config_(yaml_config)
{

}
ArmPlanner::~ArmPlanner()
{
}
void ArmPlanner::initModel() {
	RigidBodyDynamics::Addons::URDFReadFromFile((config_.urdf_path).c_str(), &rbdl_model_, false, false);

	nb_of_joints_ = rbdl_model_.q_size;
	// Fixed Body : The value of max(unsigned int) is
	// * determined via std::numeric_limits<unsigned int>::max() and the
	// * default value of fixed_body_discriminator is max (unsigned int) / 2.
	rrt_.dofSize = nb_of_joints_;

	// rbdl이 상체 모든 걸 다 받아버리면 말단 장치 위치 계산할때 모든 관절의 위치를 알고 있어야함.
	end_effector_id_ = rbdl_model_.GetBodyId((config_.chain_end).c_str());

	if (rbdl_model_.IsFixedBodyId(end_effector_id_))
	{
		end_effector_com_ = rbdl_model_.mFixedBodies[end_effector_id_ - rbdl_model_.fixed_body_discriminator].mCenterOfMass;
	}
	else
	{
		end_effector_com_ = rbdl_model_.mBodies[end_effector_id_].mCenterOfMass;
	}
	// end_effector_com_ : 0.0, 0.0, 0.0 for robocare

	//cout << "end_effector_com_" << end_effector_com_.transpose() << endl;

}
// void ArmPlanner::initializeData(arm_motion_planner::plan_arm_motion::Request &req) {

// 	// joint limit
// 	assert(nb_of_joints_ == config_.joint_limit_lower.size());
// 	assert(nb_of_joints_ == config_.joint_limit_upper.size());

// 	joint_limit_.lower_.resize(nb_of_joints_);
// 	joint_limit_.lower_rad_.resize(nb_of_joints_);
// 	joint_limit_.upper_.resize(nb_of_joints_);
// 	joint_limit_.upper_rad_.resize(nb_of_joints_);

// 	for (std::vector<int>::size_type i=0;i<config_.joint_limit_lower.size();i++){
// 		joint_limit_.lower_(i) = config_.joint_limit_lower[i];
// 		joint_limit_.upper_(i) = config_.joint_limit_upper[i];
// 	}

// 	joint_limit_.lower_rad_ = joint_limit_.lower_ / 180.0*M_PI;
// 	joint_limit_.upper_rad_ = joint_limit_.upper_ / 180.0*M_PI;

// 	// current joint position
// 	joint_state_.qInit_.resize(req.current_joint_state.position.size());
// 	for (int i =0;i<req.current_joint_state.position.size();i++)
// 		joint_state_.qInit_(i) = req.current_joint_state.position[i];

// 	output_trajectory_.joint_names = req.current_joint_state.name;

// 	// current base position
// 	mobile_pose_.qInit_.resize(3);
// 	mobile_pose_.qInit_(0) = req.current_mobile_state.x;
// 	mobile_pose_.qInit_(1) = req.current_mobile_state.y;
// 	mobile_pose_.qInit_(2) = req.current_mobile_state.theta;
// 	mobile_pose_.rotInit_ = rotateZaxis(req.current_mobile_state.theta);

// 	// initial pose &  target pose in Global frame
// 	init_pose_.translation().head(2) = mobile_pose_.qInit_.head(2);
// 	init_pose_.translation()(2) = 0.0;
// 	//init_pose_.translation() += mobile_pose_.rotInit_ * base_frame_pos_;
// 	init_pose_.translation() += mobile_pose_.rotInit_ * CalcBodyToBaseCoordinates(rbdl_model_, joint_state_.qInit_, end_effector_id_, end_effector_com_, true);
// 	init_pose_.linear() = CalcBodyWorldOrientation(rbdl_model_ , joint_state_.qInit_, end_effector_id_, true).transpose();

// 	//cout << "init_pose_" <<init_pose_.translation().transpose() << endl;

// 	target_pose_.translation()(0) = req.target_ee_pose.position.x;
// 	target_pose_.translation()(1) = req.target_ee_pose.position.y;
// 	target_pose_.translation()(2) = req.target_ee_pose.position.z;
// 	Eigen::Quaterniond quat(req.target_ee_pose.orientation.w, req.target_ee_pose.orientation.x, req.target_ee_pose.orientation.y, req.target_ee_pose.orientation.z);
// 	target_pose_.linear() = quat.toRotationMatrix();
// 	//cout << "target_pose_.linear()" << target_pose_.linear() << endl;

// 	Vector3d from_init_to_goal_in_local = mobile_pose_.rotInit_.transpose()*(init_pose_.translation() - target_pose_.translation());

// 	//cout << from_init_to_goal_in_local.transpose() << endl;

// 	// Obstacles
// 	rrt_.box_num_obs = req.Obstacles3D.size();
// 	for (int i=0;i<rrt_.box_num_obs;i++)
// 	{
// 		rrt_.Box_obs[i].fAxis = Vector3d(req.Obstacles3D[i].Box_dimension.x, req.Obstacles3D[i].Box_dimension.y, req.Obstacles3D[i].Box_dimension.z);
// 		rrt_.Box_obs[i].vAxis[0] = mobile_pose_.rotInit_.transpose() * Vector3d(1, 0, 0);
// 		rrt_.Box_obs[i].vAxis[1] = mobile_pose_.rotInit_.transpose() * Vector3d(0, 1, 0);
// 		rrt_.Box_obs[i].vAxis[2] = mobile_pose_.rotInit_.transpose() * Vector3d(0, 0, 1);
// 		rrt_.Box_obs[i].vPos =  mobile_pose_.rotInit_.transpose()* Vector3d(req.Obstacles3D[i].Box_pose.position.x - mobile_pose_.qInit_(0), req.Obstacles3D[i].Box_pose.position.y - mobile_pose_.qInit_(1), req.Obstacles3D[i].Box_pose.position.z); // axis center pos
// 	}

// 	// link data (link dimension)
// 	rrt_.box_num_link = config_.link_name.size();
// 	for (int i=0;i<rrt_.box_num_link;i++){
// 		Vector3d link_dim;
// 		for (int j=0;j<config_.link_dimension[i].size();j++){
// 			link_dim(j) =  config_.link_dimension[i][j];
// 			rrt_.Box_link[i].vCenter(j) = config_.link_position[i][j];
// 		}

// 		rrt_.Box_link[i].vRot = rotateXaxis(config_.link_orientation[i][0])*rotateYaxis(config_.link_orientation[i][1])*rotateZaxis(config_.link_orientation[i][2]);
// 		rrt_.Box_link[i].fAxis = link_dim;
// 	}

// 	// Constraint bound
// 	constrain_pose_ = req.Pose_bound.constrain_pose.data;
// 	rrt_.C.resize(6,2);
// 	for (int i = 0; i < 3; i++)
// 	{
// 		if (from_init_to_goal_in_local(i) > 0)
// 		{
// 			rrt_.C(i, 0) = from_init_to_goal_in_local(i) + 0.05;
// 			rrt_.C(i, 1) = -0.05;
// 		}
// 		else
// 		{
// 			rrt_.C(i, 0) = 0.05;
// 			rrt_.C(i, 1) = from_init_to_goal_in_local(i) - 0.05;
// 		}
// 	}

// 	// rrt_.C(0,0) = req.Pose_bound.position_bound_upper.x;
// 	// rrt_.C(1,0) = req.Pose_bound.position_bound_upper.y;
// 	// rrt_.C(2,0) = req.Pose_bound.position_bound_upper.z;

// 	// rrt_.C(0,1) = req.Pose_bound.position_bound_lower.x;
// 	// rrt_.C(1,1) = req.Pose_bound.position_bound_lower.y;
// 	// rrt_.C(2,1) = req.Pose_bound.position_bound_lower.z;

// 	rrt_.C(3,0) = req.Pose_bound.orientation_bound_upper.x;
// 	rrt_.C(4,0) = req.Pose_bound.orientation_bound_upper.y;
// 	rrt_.C(5,0) = req.Pose_bound.orientation_bound_upper.z;

// 	rrt_.C(3,1) = req.Pose_bound.orientation_bound_lower.x;
// 	rrt_.C(4,1) = req.Pose_bound.orientation_bound_lower.y;
// 	rrt_.C(5,1) = req.Pose_bound.orientation_bound_lower.z;

// 	// Trajectory library
// 	interpolate_path_ = req.interpolate_path.data;

// 	maxVelocity.resize(nb_of_joints_);
// 	maxVelocity.setZero();
// 	maxAcceleration.resize(nb_of_joints_);
// 	maxAcceleration.setZero();
// 	for (size_t i = 0; i < nb_of_joints_; i++)
// 	{
// 		maxAcceleration(i) = 10.0;
// 		maxVelocity(i) = 10.0;
// 	}
// 	wayPoints.clear();
// 	playTime_ = 0.0;

// }

trajectory_msgs::JointTrajectory ArmPlanner::compute() {
	// get Body Id of Link
	body_id_collision_.clear();
	body_com_position_.clear();
	for (std::vector<int>::size_type i = 0; i < config_.link_dimension.size(); i++)
	{
		body_id_collision_.push_back(rbdl_model_.GetBodyId(config_.link_name[i].c_str()));
		body_com_position_.push_back(rbdl_model_.mBodies[rbdl_model_.GetBodyId(config_.link_name[i].c_str())].mCenterOfMass);
	};

	// push back end_effector
	body_id_collision_.push_back(end_effector_id_);
	body_com_position_.push_back(end_effector_com_);

	// Write the model data
	rrt_model_.model_ = rbdl_model_;
	rrt_model_.body_id_vec.clear();
	rrt_model_.body_id_vec.assign(body_id_collision_.begin(), body_id_collision_.end());
	rrt_model_.body_com_pos.clear();
	rrt_model_.body_com_pos.assign(body_com_position_.begin(), body_com_position_.end());

	// for (int i=0;i<5;i++){
	// cout << CalcBodyToBaseCoordinates(rbdl_model_, joint_state_.qInit_, body_id_collision_[i], body_com_position_[i], true).transpose()<< endl;
	// }
	// Calculate IK solution
	std::vector<VectorXd> q_goal_set;
	int nb_of_sols = 0 ;
	for (int i = 0; i < 10; i++)
	{
		if (solveIK(target_pose_, rrt_model_))
		{
			q_goal_set.push_back(joint_state_.qGoal_);
			nb_of_sols ++;
		}
	}

	if (nb_of_sols == 0)
	{
		ROS_INFO_STREAM("IK solutions does not exist!!");
		output_trajectory_.points.clear();
		return output_trajectory_;
	}

	// Plan the trajectory
	if(constrain_pose_)
	{
		rrt_.refer_pos = target_pose_.translation();
		rrt_.refer_rot = target_pose_.linear();

		rrt_.local_pos.head(2) = mobile_pose_.qInit_.head(2);
		rrt_.local_pos(2) = 0.0;
		rrt_.local_rot = mobile_pose_.rotInit_;

		VectorXd q_tra(7);
		Vector3d ee_pos;
		//if (CRRT_planning(joint_state_.qInit_, joint_state_.qGoal_, joint_target2_))

		if (solveCRRTwithMultipleGoals(joint_state_.qInit_, q_goal_set, joint_target2_))
		{
			if (!interpolate_path_)
			{
				output_trajectory_.points.clear();
				for (int i = 0; i < joint_target2_.rows(); i++)
				{
					wayPoints.push_back(joint_target2_.row(i));
				}
				trajectory_generator_ = new Trajectory(Path(wayPoints, 0.1), maxVelocity, maxAcceleration);
				//	trajectory_generator_->outputPhasePlaneTrajectory();
				duration_ = trajectory_generator_->getDuration();
				//cout <<"duration" << duration_ << endl;
				while (playTime_ / 10.0 < duration_)
				{
					trajectory_point_.positions.clear();
					trajectory_point_.velocities.clear();

					for (int i = 0; i < nb_of_joints_; i++)
					{
						trajectory_point_.positions.push_back(trajectory_generator_->getPosition(playTime_ / 10.0)[i]);
						trajectory_point_.velocities.push_back(trajectory_generator_->getVelocity(playTime_ / 10.0)[i]);
					}
					trajectory_point_.time_from_start = ros::Duration(playTime_ / 10.0);

					output_trajectory_.points.push_back(trajectory_point_);

					ee_pos = CalcBodyToBaseCoordinates(rbdl_model_, trajectory_generator_->getPosition(playTime_ / 10.0) / 180.0 * M_PI, end_effector_id_, end_effector_com_, true);
					//cout << ee_pos.transpose() << endl;
					playTime_++;
				}
			}
			else
			{
				output_trajectory_.points.clear();

				for (int i = 0; i < joint_target2_.rows(); i++)
				{
					q_tra = joint_target2_.row(i) / 180.0 * M_PI;
					ee_pos = CalcBodyToBaseCoordinates(rbdl_model_, q_tra, end_effector_id_, end_effector_com_, true);
					//cout << ee_pos.transpose() << endl;
					//cout << joint_target2_.row(i)/180.0*M_PI << endl;
					//cout << "\n"
					//	 << endl;
					fprintf(fp1, "%f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t \n", joint_target2_(i, 0), joint_target2_(i, 1), joint_target2_(i, 2), joint_target2_(i, 3), joint_target2_(i, 4), joint_target2_(i, 5), joint_target2_(i, 6), ee_pos(0), ee_pos(1), ee_pos(2));
					trajectory_point_.positions.clear();
					for (int j = 0; j < joint_target2_.cols(); j++)
						trajectory_point_.positions.push_back(joint_target2_(i, j));

					output_trajectory_.points.push_back(trajectory_point_);
				}
			}
		}
		else
		{
			output_trajectory_.points.clear();
		}
	}
	else
	{
		if (solveRRTwithMultipleGoals(joint_state_.qInit_, q_goal_set, joint_target_))
		{
			if (!interpolate_path_)
			{
				output_trajectory_.points.clear();
				for (int i = 0; i < joint_target_.rows(); i++)
				{
					wayPoints.push_back(joint_target_.row(i));
				}
				trajectory_generator_ = new Trajectory(Path(wayPoints, 0.1), maxVelocity, maxAcceleration);
				//	trajectory_generator_->outputPhasePlaneTrajectory();
				duration_ = trajectory_generator_->getDuration();
				//cout <<"duration" << duration_ << endl;
				while (playTime_ / 10.0 < duration_)
				{
					trajectory_point_.positions.clear();
					trajectory_point_.velocities.clear();

					for (int i = 0; i < nb_of_joints_; i++)
					{
						trajectory_point_.positions.push_back(trajectory_generator_->getPosition(playTime_ / 10.0)[i]);
						trajectory_point_.velocities.push_back(trajectory_generator_->getVelocity(playTime_ / 10.0)[i]);
					}
					trajectory_point_.time_from_start = ros::Duration(playTime_ / 10.0);

					output_trajectory_.points.push_back(trajectory_point_);

					playTime_++;
				}
			}
			else
			{
				output_trajectory_.points.clear();

				for (int i = 0; i < joint_target_.rows(); i++)
				{
					//q_tra = joint_target_.row(i) / 180.0 * M_PI;
					// ee_pos = CalcBodyToBaseCoordinates(rbdl_model_, q_tra, end_effector_id_, end_effector_com_, true);
					// cout << ee_pos.transpose() << endl;
					cout <<"Path\t" << i << "\t" << "rad:" << joint_target_.row(i)/180.0*M_PI << endl;
					cout << "Path\t" << i << "\t" << "angle:" << joint_target_.row(i) << endl;
					//fprintf(fp1, "%f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t \n", joint_target2_(i, 0), joint_target2_(i, 1), joint_target2_(i, 2), joint_target2_(i, 3), joint_target2_(i, 4), joint_target2_(i, 5), joint_target2_(i, 6), ee_pos(0), ee_pos(1), ee_pos(2));
					trajectory_point_.positions.clear();
					for (int j = 0; j < joint_target_.cols(); j++)
						trajectory_point_.positions.push_back(joint_target_(i, j));

					output_trajectory_.points.push_back(trajectory_point_);
				}
			}

		}
		else
		{
			output_trajectory_.points.clear();

		}
	}

	return output_trajectory_;
}

bool ArmPlanner::solveIK(Transform3d pose, Robotmodel& model) // from panda_arm.xacro
{
	double eps = 1e-7;
	double num_samples = 100;
	double timeout = 0.005;
	std::string chain_start;
	std::string chain_end;

	chain_start = config_.chain_start;
	chain_end = config_.chain_end;
	TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param_, timeout, eps);

	KDL::Chain chain;
	KDL::JntArray ll, ul; //lower joint limits, upper joint limits

	bool valid = tracik_solver.getKDLChain(chain);

	if (!valid)
	{
		ROS_ERROR("there was no valid KDL chain found");
		return false;
	}

	valid = tracik_solver.getKDLLimits(ll, ul);

	if (!valid)
	{
		ROS_ERROR("there was no valid KDL joint limits found");
		return false;
	}

	assert(chain.getNrOfJoints() == ll.data.size());
	assert(chain.getNrOfJoints() == ul.data.size());

	// Set up KDL IK
	KDL::ChainFkSolverPos_recursive fk_solver(chain);									  // Forward kin. solver
	KDL::ChainIkSolverVel_pinv vik_solver(chain);										  // PseudoInverse vel solver
	KDL::ChainIkSolverPos_NR_JL kdl_solver(chain, ll, ul, fk_solver, vik_solver, 1, eps); // Joint Limit Solver
	// 1 iteration per solve (will wrap in timed loop to compare with TRAC-IK)

	// Create Nominal chain configuration midway between all joint limits
	KDL::JntArray nominal(chain.getNrOfJoints());

	// Create desired number of valid, random joint configurations
	std::vector<KDL::JntArray> JointList;
	KDL::JntArray q(chain.getNrOfJoints());

	boost::posix_time::ptime start_time;
	boost::posix_time::time_duration diff;

	KDL::JntArray result;
	KDL::Frame end_effector_pose;

	Matrix4d target_from_base;
	Matrix4d target_global;
	Matrix4d base_global;
	base_global.setIdentity();
	target_from_base.setIdentity();
	target_global.setIdentity();

	base_global.block(0,0,3,3) = rotateZaxis(mobile_pose_.qInit_(2));
	base_global(0, 3) = mobile_pose_.qInit_(0);
	base_global(1, 3) = mobile_pose_.qInit_(1);
	base_global(2, 3) = 0.0;

	target_global.block(0, 0, 3, 3) = pose.linear();
	target_global.block(0, 3, 3, 1) = pose.translation();

	target_from_base = base_global.inverse() * target_global;

	//cout << "target_global" << target_global << endl;
	//cout << "base_global" << base_global << endl;

	for (int i = 0; i < 3; i++)
	{
		end_effector_pose.p(i) = target_from_base(i,3);
	}
	//cout << "waist to desired ee in local : " << target_from_base.block(0, 3, 3, 1).transpose() << endl;

	KDL::Rotation A;
	A.data[0] = target_from_base(0, 0);
	A.data[1] = target_from_base(0, 1);
	A.data[2] = target_from_base(0, 2);
	A.data[3] = target_from_base(1, 0);
	A.data[4] = target_from_base(1, 1);
	A.data[5] = target_from_base(1, 2);
	A.data[6] = target_from_base(2, 0);
	A.data[7] = target_from_base(2, 1);
	A.data[8] = target_from_base(2, 2);
	end_effector_pose.M = A;

	int rc;

	double total_time = 0;
	uint success = 0;
	bool solved = true;

	while(true) //for (uint i = 0; i < num_samples; i++)
	{
		if(--num_samples == 0){
			solved = false;
			break;
		}

		std::vector<double> R;
		for (int i = 0; i < chain.getNrOfJoints(); i++)
		{
			double jointrange = joint_limit_.upper_rad_(i) - joint_limit_.lower_rad_(i); // angle
			double r = ((double)rand() / (double)RAND_MAX) * jointrange;
			R.push_back(joint_limit_.lower_rad_(i) + r);
		}

		for (size_t j = 0; j < nominal.data.size(); j++)
		{
			nominal(j) = R[j];
		}

		//cout <<"iteration?" << endl;
		double elapsed = 0;
		start_time = boost::posix_time::microsec_clock::local_time();
		rc = tracik_solver.CartToJnt(nominal, end_effector_pose, result);
		// int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist& bounds=KDL::Twist::Zero());

		std::vector<double> config;
		config.clear();
		if (rc >= 0)
		{
			//ROS_INFO_STREAM("IK solution is : " << result.data.transpose() * 180 / M_PI);

			for (int i = 0; i < nb_of_joints_; i++)
			{
				config.push_back(result.data(i) * 180 / M_PI);
			}
			// joint_state_.qGoal_ = result.data;
			// solved = true;
			// break;
			if (!rrt_.checkExternalCollision(model, config) && !rrt_.checkSelfCollision(model, config) )
			{

				ROS_INFO_STREAM("IK solution is : " << result.data.transpose() * 180 / M_PI);
				joint_state_.qGoal_ = result.data;
				solved = true;
				break;
			}
			else
			{
				continue;
			}
		}
		else
		{
			continue;
		}
	}
	return solved;
}
bool ArmPlanner::setupRRT(VectorXd q_start, VectorXd q_goal, MatrixXd& joint_target)
{
	rrt_.lower_limit = joint_limit_.lower_;
	rrt_.upper_limit = joint_limit_.upper_;

	rrt_.qinit = q_start;
	rrt_.qgoal = q_goal;

	ofstream outFile("path_result.txt", ios::out); // open file for "writing"

	if (rrt_.solveRRT(rrt_model_, outFile))
	{
		ofstream outFile2("path_result2.txt", ios::out); // "writing"
		// path_result -> Smooth -> path_result2
		ifstream inFile("path_result.txt"); // "reading"
		rrt_.smoothPath(outFile2, inFile);

		outFile2.close();
		MatrixXd joint_temp(100, nb_of_joints_);

		ifstream inFile2("path_result2.txt"); // "reading"
		int size = 0;
		std::vector<std::string> parameters;
		char inputString[1000];
		while (!inFile2.eof())
		{ // eof : end of file
			inFile2.getline(inputString, 1000);
			boost::split(parameters, inputString, boost::is_any_of(","));
			if (parameters.size() == nb_of_joints_)
			{
				for (int j = 0; j < parameters.size(); j++)
					joint_temp(size, j) = atof(parameters[j].c_str()); // c_str() : string -> char* // atof : char* -> float
				size++;
			}
		}
		//cout << "trajectory size" << size << endl;
		inFile2.close();
		joint_target = joint_temp.topLeftCorner(size, nb_of_joints_);
		return true;
	}
	else
	{
		cout << "Time out!!" << endl;
		return false;
	}
}
bool ArmPlanner::setupCRRT(VectorXd q_start, VectorXd q_goal, MatrixXd &joint_target)
{
	rrt_.lower_limit = joint_limit_.lower_;
	rrt_.upper_limit = joint_limit_.upper_;

	rrt_.qinit = q_start;
	rrt_.qgoal = q_goal;

	ofstream outFile3("path_result3.txt", ios::out);

	if (rrt_.solveCRRT(rrt_model_, outFile3))
	{
		outFile3.close();

		MatrixXd joint_temp(5000, nb_of_joints_);

		ifstream inFile3("path_result3.txt");
		int size = 0;
		std::vector<std::string> parameters;
		char inputString[50000];
		while (!inFile3.eof())
		{
			inFile3.getline(inputString, 50000);
			boost::split(parameters, inputString, boost::is_any_of(","));
			if (parameters.size() == nb_of_joints_)
			{
				for (int j = 0; j < parameters.size(); j++)
					joint_temp(size, j) = atof(parameters[j].c_str());
				size++;
			}
		}
		inFile3.close();
		//cout << "size" << size << endl;
		joint_target = joint_temp.topLeftCorner(size, nb_of_joints_);

		return true;
	}
	else
	{
		cout << "Time out!!" << endl;
		return false;
	}

}

bool ArmPlanner::solveRRTwithMultipleGoals(VectorXd q_start, std::vector<VectorXd> q_goal_set, MatrixXd &joint_target)
{
	rrt_.lower_limit = joint_limit_.lower_;
	rrt_.upper_limit = joint_limit_.upper_;

	rrt_.qinit = q_start;
	rrt_.qgoals.assign(q_goal_set.begin(), q_goal_set.end());

	ofstream outFile("path_result.txt", ios::out); // open file for "writing"

	if (rrt_.solveRRT(rrt_model_, outFile))
	{

		outFile.close();
		MatrixXd joint_temp(5000, nb_of_joints_);

		ifstream inFile2("path_result.txt"); // "reading"
		int size = 0;
		std::vector<std::string> parameters;
		char inputString[1000];
		while (!inFile2.eof())
		{
			inFile2.getline(inputString, 50000);
			boost::split(parameters, inputString, boost::is_any_of(","));
			if (parameters.size() == nb_of_joints_)
			{
				for (int j = 0; j < parameters.size(); j++)
					joint_temp(size, j) = atof(parameters[j].c_str());
				size++;
			}
		}
		inFile2.close();
		//cout << "size" << size << endl;
		joint_target = joint_temp.topLeftCorner(size, nb_of_joints_);

		return true;
	}
	else
	{
		cout << "Time out!!" << endl;
		return false;
	}

}

bool ArmPlanner::solveCRRTwithMultipleGoals(VectorXd q_start, std::vector<VectorXd> q_goal_set, MatrixXd &joint_target)
{
	rrt_.lower_limit = joint_limit_.lower_;
	rrt_.upper_limit = joint_limit_.upper_;

	rrt_.qinit = q_start;
	rrt_.qgoals.assign(q_goal_set.begin(), q_goal_set.end());

	ofstream outFile3("path_result3.txt", ios::out);

	if (rrt_.solveCRRT(rrt_model_, outFile3))
	{
		outFile3.close();

		//cout << "111"<<endl;
		MatrixXd joint_temp(5000, nb_of_joints_);

		ifstream inFile3("path_result3.txt");
		int size = 0;
		std::vector<std::string> parameters;
		char inputString[50000];
		while (!inFile3.eof())
		{
			inFile3.getline(inputString, 50000);
			boost::split(parameters, inputString, boost::is_any_of(","));
			if (parameters.size() == nb_of_joints_)
			{
				for (int j = 0; j < parameters.size(); j++)
					joint_temp(size, j) = atof(parameters[j].c_str());
				size++;
			}
		}
		inFile3.close();
		//cout << "size" << size << endl;
		joint_target = joint_temp.topLeftCorner(size, nb_of_joints_);

		return true;
	}
	else
	{
		cout << "Time out!!" << endl;
		return false;
	}

}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2014, 2015.
// Modifications copyright (c) 2014-2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_EQUALS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_EQUALS_HPP

#include <cstddef>
#include <vector>

#include <boost/range.hpp>

#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/static_visitor.hpp>
#include <boost/variant/variant_fwd.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/geometry_id.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/algorithms/detail/equals/point_point.hpp>
#include <boost/geometry/algorithms/detail/not.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>

// For trivial checks
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#include <boost/geometry/algorithms/detail/equals/collect_vectors.hpp>
#include <boost/geometry/algorithms/relate.hpp>
#include <boost/geometry/algorithms/detail/relate/relate_impl.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace equals
{

template
<
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct box_box
{
    template <typename Box1, typename Box2>
    static inline bool apply(Box1 const& box1, Box2 const& box2)
    {
        if (!geometry::math::equals(get<min_corner, Dimension>(box1), get<min_corner, Dimension>(box2))
            || !geometry::math::equals(get<max_corner, Dimension>(box1), get<max_corner, Dimension>(box2)))
        {
            return false;
        }
        return box_box<Dimension + 1, DimensionCount>::apply(box1, box2);
    }
};

template <std::size_t DimensionCount>
struct box_box<DimensionCount, DimensionCount>
{
    template <typename Box1, typename Box2>
    static inline bool apply(Box1 const& , Box2 const& )
    {
        return true;
    }
};

struct segment_segment
{
    template <typename Segment1, typename Segment2>
    static inline bool apply(Segment1 const& segment1, Segment2 const& segment2)
    {
        return equals::equals_point_point(
                    indexed_point_view<Segment1 const, 0>(segment1),
                    indexed_point_view<Segment2 const, 0>(segment2) )
                ? equals::equals_point_point(
                    indexed_point_view<Segment1 const, 1>(segment1),
                    indexed_point_view<Segment2 const, 1>(segment2) )
                : ( equals::equals_point_point(
                        indexed_point_view<Segment1 const, 0>(segment1),
                        indexed_point_view<Segment2 const, 1>(segment2) )
                 && equals::equals_point_point(
                        indexed_point_view<Segment1 const, 1>(segment1),
                        indexed_point_view<Segment2 const, 0>(segment2) )
                  );
    }
};

struct area_check
{
    template <typename Geometry1, typename Geometry2>
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2)
    {
        return geometry::math::equals(
                geometry::area(geometry1),
                geometry::area(geometry2));
    }
};

struct length_check
{
    template <typename Geometry1, typename Geometry2>
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2)
    {
        return geometry::math::equals(
                geometry::length(geometry1),
                geometry::length(geometry2));
    }
};

template <typename TrivialCheck>
struct equals_by_collection
{
    template <typename Geometry1, typename Geometry2>
    static inline bool apply(Geometry1 const& geometry1, Geometry2 const& geometry2)
    {
        if (! TrivialCheck::apply(geometry1, geometry2))
        {
            return false;
        }

        typedef typename geometry::select_most_precise
            <
                typename select_coordinate_type
                    <
                        Geometry1, Geometry2
                    >::type,
                double
            >::type calculation_type;

        typedef std::vector<collected_vector<calculation_type> > v;
        v c1, c2;

        geometry::collect_vectors(c1, geometry1);
        geometry::collect_vectors(c2, geometry2);

        if (boost::size(c1) != boost::size(c2))
        {
            return false;
        }

        std::sort(c1.begin(), c1.end());
        std::sort(c2.begin(), c2.end());

        // Just check if these vectors are equal.
        return std::equal(c1.begin(), c1.end(), c2.begin());
    }
};

template<typename Geometry1, typename Geometry2>
struct equals_by_relate
    : detail::relate::relate_impl
        <
            detail::de9im::static_mask_equals_type,
            Geometry1,
            Geometry2
        >
{};

}} // namespace detail::equals
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type,
    std::size_t DimensionCount = dimension<Geometry1>::type::value,
    bool Reverse = reverse_dispatch<Geometry1, Geometry2>::type::value
>
struct equals: not_implemented<Tag1, Tag2>
{};

// If reversal is needed, perform it
template
<
    typename Geometry1, typename Geometry2,
    typename Tag1, typename Tag2,
    std::size_t DimensionCount
>
struct equals<Geometry1, Geometry2, Tag1, Tag2, DimensionCount, true>
    : equals<Geometry2, Geometry1, Tag2, Tag1, DimensionCount, false>
{
    static inline bool apply(Geometry1 const& g1, Geometry2 const& g2)
    {
        return equals
            <
                Geometry2, Geometry1,
                Tag2, Tag1,
                DimensionCount,
                false
            >::apply(g2, g1);
    }
};

template <typename P1, typename P2, std::size_t DimensionCount, bool Reverse>
struct equals<P1, P2, point_tag, point_tag, DimensionCount, Reverse>
    : geometry::detail::not_
        <
            detail::disjoint::point_point<P1, P2, 0, DimensionCount>
        >
{};

template <typename Box1, typename Box2, std::size_t DimensionCount, bool Reverse>
struct equals<Box1, Box2, box_tag, box_tag, DimensionCount, Reverse>
    : detail::equals::box_box<0, DimensionCount>
{};

template <typename Ring1, typename Ring2, bool Reverse>
struct equals<Ring1, Ring2, ring_tag, ring_tag, 2, Reverse>
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Polygon1, typename Polygon2, bool Reverse>
struct equals<Polygon1, Polygon2, polygon_tag, polygon_tag, 2, Reverse>
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Polygon, typename Ring, bool Reverse>
struct equals<Polygon, Ring, polygon_tag, ring_tag, 2, Reverse>
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Ring, typename Box, bool Reverse>
struct equals<Ring, Box, ring_tag, box_tag, 2, Reverse>
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Polygon, typename Box, bool Reverse>
struct equals<Polygon, Box, polygon_tag, box_tag, 2, Reverse>
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Segment1, typename Segment2, std::size_t DimensionCount, bool Reverse>
struct equals<Segment1, Segment2, segment_tag, segment_tag, DimensionCount, Reverse>
    : detail::equals::segment_segment
{};

template <typename LineString1, typename LineString2, bool Reverse>
struct equals<LineString1, LineString2, linestring_tag, linestring_tag, 2, Reverse>
    //: detail::equals::equals_by_collection<detail::equals::length_check>
    : detail::equals::equals_by_relate<LineString1, LineString2>
{};

template <typename LineString, typename MultiLineString, bool Reverse>
struct equals<LineString, MultiLineString, linestring_tag, multi_linestring_tag, 2, Reverse>
    : detail::equals::equals_by_relate<LineString, MultiLineString>
{};

template <typename MultiLineString1, typename MultiLineString2, bool Reverse>
struct equals<MultiLineString1, MultiLineString2, multi_linestring_tag, multi_linestring_tag, 2, Reverse>
    : detail::equals::equals_by_relate<MultiLineString1, MultiLineString2>
{};

template <typename MultiPolygon1, typename MultiPolygon2, bool Reverse>
struct equals
    <
        MultiPolygon1, MultiPolygon2,
        multi_polygon_tag, multi_polygon_tag,
        2,
        Reverse
    >
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

template <typename Polygon, typename MultiPolygon, bool Reverse>
struct equals
    <
        Polygon, MultiPolygon,
        polygon_tag, multi_polygon_tag,
        2,
        Reverse
    >
    : detail::equals::equals_by_collection<detail::equals::area_check>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_variant {

template <typename Geometry1, typename Geometry2>
struct equals
{
    static inline bool apply(Geometry1 const& geometry1,
                             Geometry2 const& geometry2)
    {
        concept::check_concepts_and_equal_dimensions
        <
            Geometry1 const,
            Geometry2 const
        >();

        return dispatch::equals<Geometry1, Geometry2>
                       ::apply(geometry1, geometry2);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T), typename Geometry2>
struct equals<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>, Geometry2>
{
    struct visitor: static_visitor<bool>
    {
        Geometry2 const& m_geometry2;

        visitor(Geometry2 const& geometry2)
            : m_geometry2(geometry2)
        {}

        template <typename Geometry1>
        inline bool operator()(Geometry1 const& geometry1) const
        {
            return equals<Geometry1, Geometry2>
                   ::apply(geometry1, m_geometry2);
        }

    };

    static inline bool apply(
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry1,
        Geometry2 const& geometry2
    )
    {
        return boost::apply_visitor(visitor(geometry2), geometry1);
    }
};

template <typename Geometry1, BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct equals<Geometry1, boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    struct visitor: static_visitor<bool>
    {
        Geometry1 const& m_geometry1;

        visitor(Geometry1 const& geometry1)
            : m_geometry1(geometry1)
        {}

        template <typename Geometry2>
        inline bool operator()(Geometry2 const& geometry2) const
        {
            return equals<Geometry1, Geometry2>
                   ::apply(m_geometry1, geometry2);
        }

    };

    static inline bool apply(
        Geometry1 const& geometry1,
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> const& geometry2
    )
    {
        return boost::apply_visitor(visitor(geometry1), geometry2);
    }
};

template <
    BOOST_VARIANT_ENUM_PARAMS(typename T1),
    BOOST_VARIANT_ENUM_PARAMS(typename T2)
>
struct equals<
    boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)>,
    boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)>
>
{
    struct visitor: static_visitor<bool>
    {
        template <typename Geometry1, typename Geometry2>
        inline bool operator()(Geometry1 const& geometry1,
                               Geometry2 const& geometry2) const
        {
            return equals<Geometry1, Geometry2>
                   ::apply(geometry1, geometry2);
        }

    };

    static inline bool apply(
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T1)> const& geometry1,
        boost::variant<BOOST_VARIANT_ENUM_PARAMS(T2)> const& geometry2
    )
    {
        return boost::apply_visitor(visitor(), geometry1, geometry2);
    }
};

} // namespace resolve_variant

/*!
\brief \brief_check{are spatially equal}
\details \details_check12{equals, is spatially equal}. Spatially equal means
    that the same point set is included. A box can therefore be spatially equal
    to a ring or a polygon, or a linestring can be spatially equal to a
    multi-linestring or a segment. This only works theoretically, not all
    combinations are implemented yet.
\ingroup equals
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\return \return_check2{are spatially equal}

\qbk{[include reference/algorithms/equals.qbk]}

 */
template <typename Geometry1, typename Geometry2>
inline bool equals(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    return resolve_variant::equals<Geometry1, Geometry2>
                          ::apply(geometry1, geometry2);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_EQUALS_HPP


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file main.cpp
 * @author Johann Prankl (prankl@acin.tuwien.ac.at)
 * @date 2017
 * @brief
 *
 */

#ifndef KP_TSF_POSE_TRACKER_KLT_HH
#define KP_TSF_POSE_TRACKER_KLT_HH

#include <float.h>
#include <pcl/io/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <v4r/camera_tracking_and_mapping/TSFData.h>
#include <v4r/core/macros.h>
#include <v4r/recognition/RansacSolvePnPdepth.h>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <v4r/common/impl/DataMatrix2D.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/video/tracking.hpp"

namespace v4r {

/**
 * TSFPoseTrackerKLT
 */
class V4R_EXPORTS TSFPoseTrackerKLT {
 public:
  /**
   * Parameter
   */
  class Parameter {
   public:
    cv::TermCriteria termcrit;
    cv::Size win_size;
    cv::Size subpix_win_size;
    int max_count;
    double pcent_reinit;
    double conf_tracked_points_norm;
    v4r::RansacSolvePnPdepth::Parameter rt;
    Parameter()
    : termcrit(cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03)), win_size(cv::Size(31, 31)),
      subpix_win_size(cv::Size(10, 10)), max_count(500), pcent_reinit(0.5), conf_tracked_points_norm(250),
      rt(v4r::RansacSolvePnPdepth::Parameter(1.5, 0.01, 5000, INT_MIN, 4, 0.015)) {}
  };

 private:
  Parameter param;

  cv::Mat_<double> intrinsic;

  std::vector<uchar> status;
  std::vector<float> err;
  std::vector<int> inliers;

  bool run, have_thread;

  boost::thread th_obectmanagement;
  boost::thread th_init;

  TSFData *data;

  v4r::RansacSolvePnPdepth::Ptr pnp;
  std::vector<int> plk_converged;
  std::vector<float> depth;

  double kal_dt;
  cv::Mat_<double> measurement;
  cv::KalmanFilter kalmanFilter;

  void operate();

  void getImage(const v4r::DataMatrix2D<Surfel> &cloud, cv::Mat &im);
  bool needReinit(const std::vector<cv::Point2f> &points);
  bool trackCamera(double &conf_ransac_iter, double &conf_tracked_points);
  void getPoints3D(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const std::vector<cv::Point2f> &points,
                   std::vector<Eigen::Vector3f> &points3d);
  void filterValidPoints3D(std::vector<cv::Point2f> &points, std::vector<Eigen::Vector3f> &points3d);
  void filterValidPoints3D(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1,
                           std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2);
  void filterInliers(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1,
                     std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2, std::vector<int> &inliers);
  void filterConverged(std::vector<cv::Point2f> &pts1, std::vector<Eigen::Vector3f> &pts3d1,
                       std::vector<cv::Point2f> &pts2, std::vector<Eigen::Vector3f> &pts3d2,
                       std::vector<int> &converged);

  void initKalmanFilter(cv::KalmanFilter &kf, double dt);
  void updateKalmanFilter(cv::KalmanFilter &kf, const Eigen::Matrix4f &pose, Eigen::Matrix4f &kal_pose, bool have_pose);

  cv::Mat euler2rot(const cv::Mat &euler);
  cv::Mat rot2euler(const cv::Mat &rotationMatrix);

  inline float getInterpolated(const v4r::DataMatrix2D<v4r::Surfel> &cloud, const cv::Point2f &pt);
  inline float getInterpolated(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const cv::Point2f &pt);
  inline float sqr(const float &d) {
    return d * d;
  }

 public:
  cv::Mat dbg;

  TSFPoseTrackerKLT(const Parameter &p = Parameter());
  ~TSFPoseTrackerKLT();

  void start();
  void stop();

  inline bool isStarted() {
    return have_thread;
  }

  void reset();

  void setData(TSFData *_data) {
    data = _data;
  }
  void track(double &conf_ransac_iter, double &conf_tracked_points);

  void setCameraParameter(const cv::Mat &_intrinsic);
  void setParameter(const Parameter &p);

  typedef std::shared_ptr<::v4r::TSFPoseTrackerKLT> Ptr;
  typedef std::shared_ptr<::v4r::TSFPoseTrackerKLT const> ConstPtr;
};

/*************************** INLINE METHODES **************************/

/**
 * @brief TSFPoseTrackerKLT::getInterpolated
 * @param cloud
 * @param pt
 * @return
 */
inline float TSFPoseTrackerKLT::getInterpolated(const v4r::DataMatrix2D<v4r::Surfel> &cloud, const cv::Point2f &pt) {
  int xt = (int)pt.x;
  int yt = (int)pt.y;
  float ax = pt.x - xt;
  float ay = pt.y - yt;
  float d = 0;
  float sn = 0, n;

  if (!std::isnan(cloud(yt, xt).pt[0])) {
    n = (1. - ax) * (1. - ay);
    d += n * cloud(yt, xt).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt, xt + 1).pt[0])) {
    n = ax * (1. - ay);
    d += n * cloud(yt, xt + 1).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt + 1, xt).pt[0])) {
    n = (1. - ax) * ay;
    d += n * cloud(yt + 1, xt).pt[2];
    sn += n;
  }
  if (!std::isnan(cloud(yt + 1, xt + 1).pt[0])) {
    n = ax * ay;
    d += n * cloud(yt + 1, xt + 1).pt[2];
    sn += n;
  }

  return (d > 0. ? d / sn : std::numeric_limits<float>::quiet_NaN());
}

/**
 * @brief TSFPoseTrackerKLT::getInterpolated
 * @param cloud
 * @param pt
 * @return
 */
inline float TSFPoseTrackerKLT::getInterpolated(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, const cv::Point2f &pt) {
  int xt = (int)pt.x;
  int yt = (int)pt.y;
  float ax = pt.x - xt;
  float ay = pt.y - yt;
  float d = 0;
  float sn = 0, n;

  if (!std::isnan(cloud(xt, yt).x)) {
    n = (1. - ax) * (1. - ay);
    d += n * cloud(xt, yt).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt + 1, yt).x)) {
    n = ax * (1. - ay);
    d += n * cloud(xt + 1, yt).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt, yt + 1).x)) {
    n = (1. - ax) * ay;
    d += n * cloud(xt, yt + 1).z;
    sn += n;
  }
  if (!std::isnan(cloud(xt + 1, yt + 1).x)) {
    n = ax * ay;
    d += n * cloud(xt + 1, yt + 1).z;
    sn += n;
  }

  return (d > 0. ? d / sn : std::numeric_limits<float>::quiet_NaN());
}

}  // namespace v4r

#endif


#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/min_max.hpp>
#include <boost/proto/core.hpp>
#include <boost/proto/traits.hpp>
#include <boost/proto/context.hpp>
#include <boost/proto/transform.hpp>
namespace mpl = boost::mpl;
namespace proto = boost::proto;
using proto::_;

// Will be used to define the placeholders _1 and _2
template<typename I> struct placeholder : I {};

// This grammar basically says that a calculator expression is one of:
//   - A placeholder terminal
//   - Some other terminal
//   - Some non-terminal whose children are calculator expressions
// In addition, it has transforms that say how to calculate the
// expression arity for each of the three cases.
struct CalculatorGrammar
  : proto::or_<

        // placeholders have a non-zero arity ...
        proto::when< proto::terminal< placeholder<_> >, proto::_value >

        // Any other terminals have arity 0 ...
      , proto::when< proto::terminal<_>, mpl::int_<0>() >
        // For any non-terminals, find the arity of the children and
        // take the maximum. This is recursive.
      , proto::when< proto::nary_expr<_, proto::vararg<_> >
             , proto::fold<_, mpl::int_<0>(), mpl::max<CalculatorGrammar, proto::_state>() > >

    >
{};

// Simple wrapper for calculating a calculator expression's arity.
// It specifies mpl::int_<0> as the initial state. The data, which
// is not used, is mpl::void_.
template<typename Expr>
struct calculator_arity
  : nt2::meta::result_of<CalculatorGrammar(Expr, mpl::int_<0>, mpl::void_)>
{};

// For expressions in the calculator domain, operator ()
// will be special; it will evaluate the expression.
struct calculator_domain;
struct numeric_domain;

// Wrap all calculator expressions in this type, which defines
// operator () to evaluate the expression.
template<typename Expr>
struct calculator_expression
  : proto::extends<Expr, calculator_expression<Expr>, calculator_domain>
{
    typedef
        proto::extends<Expr, calculator_expression<Expr>, calculator_domain>
    base_type;

    explicit calculator_expression(Expr const &expr = Expr())
      : base_type(expr)
    {}

    BOOST_PROTO_EXTENDS_USING_ASSIGN(calculator_expression)
};

// Tell proto how to generate expressions in the calculator_domain
struct numeric_domain
    : proto::domain<proto::generator<calculator_expression> >
  {};

struct calculator_domain
  : proto::domain<proto::generator<calculator_expression>, _, numeric_domain >
{};

// Define some placeholders (notice they're wrapped in calculator_expression<>)
calculator_expression<proto::terminal< placeholder< mpl::int_<1> > >::type> const _p1;
calculator_expression<proto::terminal< placeholder< mpl::int_<2> > >::type> const _p2;
calculator_expression<proto::terminal< float >::type> const _pf;
calculator_expression<proto::terminal< unsigned short >::type> const _pus;
calculator_expression<proto::terminal< bool >::type> const _pb;

// TABLES DOMAIN TEST

template<int N, template<int,typename> class Extends>
struct table_generator
{
	BOOST_PROTO_CALLABLE()
	BOOST_PROTO_USE_BASIC_EXPR()

	template<typename Sig>
	struct result;

	template<typename This, typename Expr>
	struct result<This(Expr)>
	{
		typedef Extends<N,Expr> type;
	};

	template<typename This, typename Expr>
	struct result<This(Expr &)>
	{
		typedef Extends<N,Expr> type;
	};

	template<typename This, typename Expr>
	struct result<This(Expr const &)>
	{
		typedef Extends<N,Expr> type;
	};

	/// \param expr A Proto expression
	/// \return Extends<Expr>(expr)
	template<typename Expr>
	Extends<N,Expr> operator ()(Expr const &e) const
	{
		return Extends<N,Expr>(e);
	}
};

struct tables_domain;
template<int N> struct table_domain;
struct matrix_domain;
struct vector_domain;

template<typename Expr>
struct tables_expression
: proto::extends<Expr, tables_expression<Expr>, tables_domain>
{
	typedef
	proto::extends<Expr, tables_expression<Expr>, tables_domain>
	base_type;

	explicit tables_expression(Expr const &expr = Expr())
	: base_type(expr)
	{}

	BOOST_PROTO_EXTENDS_USING_ASSIGN(tables_expression)
};

template<int N,typename Expr>
struct table_expression
: proto::extends<Expr, table_expression<N,Expr>, tables_domain>
{
	typedef
	proto::extends<Expr, table_expression<N,Expr>, tables_domain>
	base_type;

	explicit table_expression(Expr const &expr = Expr())
	: base_type(expr)
	{}
};

template<typename Expr>
struct matrix_expression
: proto::extends<Expr, matrix_expression<Expr>, matrix_domain>
{
	typedef
	proto::extends<Expr, matrix_expression<Expr>, matrix_domain>
	base_type;

	explicit matrix_expression(Expr const &expr = Expr())
	: base_type(expr)
	{}

	BOOST_PROTO_EXTENDS_USING_ASSIGN(matrix_expression)
};

template<typename Expr>
struct vector_expression
: proto::extends<Expr, vector_expression<Expr>, vector_domain>
{
	typedef
	proto::extends<Expr, vector_expression<Expr>, vector_domain>
	base_type;

	explicit vector_expression(Expr const &expr = Expr())
	: base_type(expr)
	{}

	BOOST_PROTO_EXTENDS_USING_ASSIGN(vector_expression)
};

struct tables_domain : proto::domain<proto::generator<tables_expression>, _ > {};

template<int N>
struct table_domain
     : proto::domain<table_generator<N,table_expression>, _, table_domain<N> > {};

struct matrix_domain
: proto::domain<proto::generator<matrix_expression>, _, table_domain<2> > {};

struct vector_domain
: proto::domain<proto::generator<vector_expression>, _, table_domain<1> > {};

template<int N>
typename boost::proto::result_of
::make_expr< boost::proto::tag::terminal
, table_domain<N>
, int const

>::type
make_table()
{
	return	boost::proto::
	make_expr<boost::proto::tag::terminal,table_domain<N> >(N);
}

boost::proto::result_of
::make_expr< boost::proto::tag::terminal
, matrix_domain
, int const

>::type
make_matrix()
{
	return	boost::proto::
	make_expr<boost::proto::tag::terminal,matrix_domain >(20);
}

boost::proto::result_of
::make_expr< boost::proto::tag::terminal
, vector_domain
, int const

>::type
make_vector()
{
	return	boost::proto::
	make_expr<boost::proto::tag::terminal,vector_domain >(20);
}

////// Domain hierarchy
#include <nt2/sdk/details/type_id.hpp>
#include <nt2/sdk/functor/operators.hpp>

namespace nt2 { namespace tag
{
  struct expr_ {};
} }

namespace nt2 { namespace meta
{
  template<class T,class Domain>
  struct expr_ : expr_<T,typename Domain::parent>
  {
    typedef expr_<T,typename Domain::parent> parent;
    typedef tag::expr_      type(typename Domain::type);
  };

  template<class T,class Domain>
  struct expr_<T,unspecified_<Domain> > : unspecified_<T>
  {
    typedef unspecified_<T> parent;
    typedef tag::expr_      type(typename unspecified_<Domain>::type);
  };

  template<class T>
  struct domain_ : domain_<typename T::proto_super_domain>
  {
    typedef domain_<typename T::proto_super_domain> parent;
    typedef T      type;
  };

  template<>
  struct  domain_<boost::proto::detail::not_a_domain>
        : unspecified_<boost::proto::detail::not_a_domain>
  {
    typedef unspecified_<boost::proto::detail::not_a_domain> parent;
    typedef boost::proto::detail::not_a_domain      type;
  };
} }

namespace nt2 { namespace details
{
  template<class T>
  struct hierarchy_of < T
                      , typename T::proto_is_expr_
                      >
  {
    typedef typename boost::proto::domain_of<T>::type domain_type;
    typedef meta::expr_<T, typename meta::hierarchy_of<domain_type>::type> type;
  };

  template<class T>
  struct hierarchy_of < T
                      , typename T::proto_is_domain_
                      >
  {
    typedef meta::domain_<T> type;
  };

} }

template<class X> void
type_from(X const& )
{
  std::cout << nt2::type_id<typename boost::proto::domain_of<X>::type >() << "\n";
}

template<class X> void
tag_from(X const& )
{
  std::cout << nt2::type_id<typename X::type>() << "\n";
  tag_from(typename X::parent());
}

template<class X> void
tag_from(nt2::meta::unknown_<X> const& )
{
  std::cout << ".\n\n";
}

template<class X> void
info(X const& x)
{
  tag_from(typename nt2::meta::hierarchy_of<X>::type() );
}

int main()
{
	type_from(make_table<1>()+make_table<2>());

	type_from(make_table<1>()+make_table<1>());
	type_from(make_vector()+make_vector());
	type_from(make_matrix()+make_matrix());
	//type_from(make_matrix()+make_vector());
	/*
  // terminals
  info(_p1);
  info(_pf);
  info(_pus);
  info(_pb);

  // unary node
  info(-_p1);
  info(+_pf);
  info(!_pus);
  info(~_pb);

  info(-!-+_pb);

  info(_pb+_pb);

  info(_pus + _pus + _pus + _pus + _pus
            + _pus * _pus + _pus + _pus
            + _pus - _pus + !!!_pus
      );
	*/
  return 0;
}


#include <boost/geometry/strategies/geographic/intersection.hpp>