/* Copyright (C) 2012-2019 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.
 */

#if defined(__unix__) || defined(__unix) || defined(unix)
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <NTL/BasicThreadPool.h>
#include <helib/helib.h>
#include <helib/matmul.h>
#include <helib/debugging.h>
#include <helib/fhe_stats.h>

#include "gtest/gtest.h"
#include "test_common.h"

#include <algorithm>
#include <cmath>
#include <string>
#include <NTL/ZZ.h>

namespace {

/********************************************************************
 ********************************************************************/
    struct Parameters {
        long p; // plaintext base
        long r; // exponent
        // p^r is the plaintext-space modulus
        long c; // number of columns in the key-switching matrices
        long bits; // # of bits in the modulus chain
        long skHwt; // Hamming weight of recryption secret key
        long nthreads; // number of threads
        long seed; // random number seed
        int useCache; // 0: zzX cache, 1: DCRT cache
        int force_bsgs;
        int force_hoist;
        // int disable_intFactor // fhe_disable_intFactor (disabled by Victor)
        int chen_han;
        bool debug; // generate debugging output
        int scale; // scale parameter
        long special_bits;
        NTL::Vec<long> global_gens;
        NTL::Vec<long> global_ords;
        NTL::Vec<long> global_mvec;
        const int c_m; // = 100;
        const long iter;
        const std::string v_values_name;

        Parameters(long p, long r, long c, long bits, long skHwt, long nthreads, long seed, long useCache,
                   int c_m, int force_bsgs, int force_hoist, int chen_han, bool debug, int scale,
                   long special_bits, const std::vector<long> &global_gens, const std::vector<long> &global_ords,
                   const std::vector<long> &global_mvec, long iter, const std::string& v_values_name) :
                p(p),
                r(r),
                c(c),
                bits(bits),
                skHwt(skHwt),
                nthreads(nthreads),
                seed(seed),
                useCache(useCache),
                force_bsgs(force_bsgs),
                force_hoist(force_hoist),
                chen_han(chen_han),
                debug(debug),
                scale(scale),
                special_bits(special_bits),
                global_gens(helib::convert<NTL::Vec<long>>(global_gens)),
                global_ords(helib::convert<NTL::Vec<long>>(global_ords)),
                global_mvec(helib::convert<NTL::Vec<long>>(global_mvec)),
                c_m(c_m),
                iter(iter),
                v_values_name(v_values_name) {
            if (global_gens.empty() || global_ords.empty() || global_mvec.empty())
                throw helib::LogicError("gens, ords, and mvec must be non-empty");
        };

        friend std::ostream &operator<<(std::ostream &os, const Parameters &params) {
            return os << "{"
                      << "p" << params.p << ","
                      << "r" << params.r << ","
                      << "c" << params.c << ","
                      << "bits" << params.bits << ","
                      << "skHwt" << params.skHwt << ","
                      << "nthreads" << params.nthreads << ","
                      << "seed" << params.seed << ","
                      << "useCache" << params.useCache << ","
                      << "force_bsgs" << params.force_bsgs << ","
                      << "force_hoist" << params.force_hoist << ","
                      << "chen_han" << params.chen_han << ","
                      << "debug" << params.debug << ","
                      << "scale" << params.scale << ","
                      << "special_bits" << params.special_bits << ","
                      << "global_gens" << params.global_gens << ","
                      << "global_ords" << params.global_ords << ","
                      << "global_mvec" << params.global_mvec << ","
                      << "c_m" << params.c_m << ","
                      << "iter" << params.iter << ","
                      << "v_values_name" << params.v_values_name
                      << "}";
        }

//
//
//  if (seed)
//    SetSeed(ZZ(seed));
//
//  SetNumThreads(nthreads);
//
//  TestIt(p,r,bits,c,t,useCache);

    };

    class GTestThinboot : public ::testing::TestWithParam<Parameters> {
    private:

        void preContextSetup() {

            if (!helib_test::noPrint) {
                std::cout << "*** GTestThinboot";
                if (helib::isDryRun())
                    std::cout << " (dry run)";
                std::cout << ": p=" << p
                          << ", r=" << r
                          << ", bits=" << bits
                          << ", t=" << skHwt
                          << ", c=" << c
                          << ", m=" << m
                          << " mvec=" << mvec << ", gens=" << helib::vecToStr(gens) << ", ords=" << helib::vecToStr(ords)
                          << std::endl;
                std::cout << "Computing key-independent tables..." << std::flush;
            }
            helib::setTimersOn();
            helib::setDryRun(false); // Need to get a "real context" to test bootstrapping
            time = -NTL::GetTime();
        }

        void postContextSetup() {
            if (scale) {
                context.scale = scale;
            }

            context.zMStar.set_cM(c_m / 100.0);
        }

        static void setGlobals(int force_bsgs, int force_hoist, int chen_han) {
            helib::fhe_test_force_bsgs = force_bsgs;
            helib::fhe_test_force_hoist = force_hoist;
            helib::fhe_force_chen_han = chen_han;
        };

        void cleanupBootstrappingGlobals() {
            helib::fhe_test_force_bsgs = old_fhe_test_force_bsgs;
            helib::fhe_test_force_hoist = old_fhe_test_force_hoist;
            helib::fhe_force_chen_han = old_fhe_force_chen_han;
        }

        static void setSeedIfNeeded(const long seed) {
            if (seed)
                SetSeed(NTL::ZZ(seed));
        };

        static void checkPM(const long p, const long m) {
            helib::assertTrue(NTL::GCD(p, m) == 1, "GCD(p, m) == 1");
        }

    protected:
        const int old_fhe_test_force_bsgs;
        const int old_fhe_test_force_hoist;
        const int old_fhe_force_chen_han;
        const long p;
        const long r;
        const long c;
        const long bits;
        const long skHwt;
        const long nthreads;
        const long seed;
        const long useCache;
        const int force_bsgs;
        const int force_hoist;
        const int chen_han;
        const bool debug;
        const int scale;
        const long special_bits;
        const NTL::Vec<long> mvec;
        const std::vector<long> gens;
        const std::vector<long> ords;
        const int c_m; // = 100;
        const long iter;

        const long m, phim;
        double time;
        helib::Context context;

      std::string v_values_name;

        GTestThinboot() :
                old_fhe_test_force_bsgs(helib::fhe_test_force_bsgs),
                old_fhe_test_force_hoist(helib::fhe_test_force_hoist),
                old_fhe_force_chen_han(helib::fhe_force_chen_han),
                p((setGlobals(GetParam().force_bsgs, GetParam().force_hoist, GetParam().chen_han),
                   GetParam().p)),
                r(GetParam().r),
                c(GetParam().c),
                bits(GetParam().bits),
                skHwt(GetParam().skHwt),
                nthreads((NTL::SetNumThreads(GetParam().nthreads), GetParam().nthreads)),
                seed((setSeedIfNeeded(GetParam().seed), GetParam().seed)),
                useCache(GetParam().useCache),
                force_bsgs(GetParam().force_bsgs),
                force_hoist(GetParam().force_hoist),
                chen_han(GetParam().chen_han),
                debug(GetParam().debug),
                scale(GetParam().scale),
                special_bits(GetParam().special_bits),
                gens(helib::convert<std::vector<long>>(GetParam().global_gens)),
                ords(helib::convert<std::vector<long>>(GetParam().global_ords)),
                mvec(GetParam().global_mvec),
                c_m(GetParam().c_m),
                iter(GetParam().iter),
                m(helib::computeProd(mvec)),
                phim((checkPM(p, m), helib::phi_N(m))),
                time(0),
                context((preContextSetup(), m), p, r, gens, ords),
                v_values_name(GetParam().v_values_name)

        { postContextSetup(); }

        void TearDown() override
        {
          if(helib_test::verbose) {
            helib::printAllTimers();

#if (defined(__unix__) || defined(__unix) || defined(unix))
            struct rusage rusage;
            getrusage(RUSAGE_SELF, &rusage);
            if (!helib_test::noPrint)
              std::cout << "  rusage.ru_maxrss=" << rusage.ru_maxrss
                        << std::endl;
#endif

            if (helib::fhe_stats) {
              helib::print_stats(std::cout);
              std::cout << "mvec=" << mvec << ", ";
              print_anderson_darling();
              dump_v_values();
              std::cout << std::endl;
            }
          }

            cleanupBootstrappingGlobals();
            helib::cleanupGlobals();
        }

      void dump_v_values()
      {
        const std::vector<double> *v_values = helib::fetch_saved_values("v_values");
        if (v_values && v_values_name != "") {
          // write v_values to a file

          std::cerr << "writing v_values to " << v_values_name << std::endl;

          std::ofstream F;
          F.open(v_values_name.c_str());
          for (long i: helib::range(v_values->size()))
            F << (*v_values)[i] << std::endl;
        }
      }

      void anderson_darling(const std::vector<double>& X, double& AD, double& p_val)
      {
        long N = X.size();

        if (N < 2) {
          AD = 0;
          p_val = 1;
          return;
        }

        std::vector<double> Y(X);
        sort(Y.begin(), Y.end());

        // compute the sample mean
        double SM = 0;
        for (long i: helib::range(N)) SM += Y[i];
        SM /= N;

        // compute the sample variance
        double SV = 0;
        for (long i: helib::range(N)) SV += (Y[i]-SM)*(Y[i]-SM);
        SV /= (N-1);

        // replace Y[i] by CDF of Y[i]
        for (long i: helib::range(N))
          Y[i] = 0.5*(1 + erf((Y[i]-SM)/sqrt(2*SV)));

        double S = 0;
        for (long i: helib::range(N)) {
          S += (2*i+1)*(log(Y[i]) + log1p(-Y[N-1-i]));
        }
        AD = -N - S/N;

        AD *= (1 + 0.75/N + 2.25/N/N);
        // This adjustment and the p-values below come from:
        // R.B. D'Augostino and M.A. Stephens, Eds., 1986,
        // Goodness-of-Fit Techniques, Marcel Dekker.

        if (AD >= 0.6)
          p_val = exp(1.2937 - 5.709*(AD)+ 0.0186*helib::fsquare(AD));
        else if (AD > 0.34)
          p_val = exp(0.9177 - 4.279*(AD) - 1.38*helib::fsquare(AD));
        else if (AD > 0.2)
          p_val = 1 - exp(-8.318 + 42.796*(AD)- 59.938*helib::fsquare(AD));
        else
          p_val = 1 - exp(-13.436 + 101.14*(AD)- 223.73*helib::fsquare(AD));
      }

      void print_anderson_darling()
      {
        const std::vector<double> *v_values = helib::fetch_saved_values("v_values");
        if (v_values) {
          double AD, p_val;
          anderson_darling(*v_values, AD, p_val);
          std::cout << "AD=" << AD << ", p_val=" << p_val << std::endl;
        }
      }

    };

    TEST_P(GTestThinboot, correctlyPerformsThinboot) {
        helib::buildModChain(context, bits, c,
                             /*willBeBootstrappable=*/true,
                             /*skHwt=*/skHwt,
                             /*resolution=*/3,
                             /*bitsInSpecialPrimes=*/special_bits);

        if (!helib_test::noPrint) {
            std::cout << "security=" << context.securityLevel() << std::endl;
            std::cout << "# small primes = " << context.smallPrimes.card() << std::endl;
            std::cout << "# ctxt primes = " << context.ctxtPrimes.card() << std::endl;
            std::cout << "# bits in ctxt primes = "
                      << long(context.logOfProduct(context.ctxtPrimes) / log(2.0) + 0.5) << std::endl;
            std::cout << "# special primes = " << context.specialPrimes.card() << std::endl;
            std::cout << "# bits in special primes = "
                      << long(context.logOfProduct(context.specialPrimes) / log(2.0) + 0.5) << std::endl;
            std::cout << "scale=" << context.scale << std::endl;
        }

        context.makeBootstrappable(mvec,/*t=*/skHwt, useCache,/*alsoThick=*/false);
        // save time...disable some fat boot precomputation

        time += NTL::GetTime();

        //if (skHwt>0) context.rcData.skHwt = skHwt;
        if (!helib_test::noPrint) {
            std::cout << " done in " << time << " seconds" << std::endl;
            std::cout << "  e=" << context.rcData.e
                 << ", e'=" << context.rcData.ePrime
                 << ", t=" << context.rcData.skHwt
                 << std::endl << "  ";
            context.zMStar.printout();
        }
        helib::setDryRun(helib_test::dry); // Now we can set the dry-run flag if desired

        long p2r = context.alMod.getPPowR();

        if (!helib_test::noPrint) helib::fhe_stats = true;

        for (long numkey = 0; numkey < iter; numkey++) { // test with 3 keys
            if (helib::fhe_stats && numkey > 0 && numkey % 100 == 0) {
              helib::print_stats(std::cout);

            std::cout << "mvec=" << mvec << ", ";
            print_anderson_darling();
            dump_v_values();
          }

            if (!helib_test::noPrint) std::cerr << "*********** iter=" << (numkey + 1) << std::endl;

            time = -NTL::GetTime();
            if (!helib_test::noPrint) std::cout << "Generating keys, " << std::flush;
            helib::SecKey secretKey(context);
            secretKey.GenSecKey(skHwt);      // A Hamming-weight-64 secret key
            helib::addSome1DMatrices(secretKey); // compute key-switching matrices that we need
            helib::addFrbMatrices(secretKey);
            if (!helib_test::noPrint) std::cout << "computing key-dependent tables..." << std::flush;
            secretKey.genRecryptData();
            time += NTL::GetTime();
            if (!helib_test::noPrint) std::cout << " done in " << time << " seconds\n";

#ifdef DEBUG_PRINTOUT
            helib::dbgEa = (helib::EncryptedArray*) context.ea;
            helib::dbgKey = &secretKey;
#endif

            const helib::PubKey publicKey = secretKey;

            long d = context.zMStar.getOrdP();
            long phim = context.zMStar.getPhiM();
            long nslots = phim / d;

            // GG defines the plaintext space Z_p[X]/GG(X)
            NTL::ZZX GG;
            GG = context.alMod.getFactorsOverZZ()[0];
            helib::EncryptedArray ea(context, GG);

            if (debug) {
                helib::dbgKey = &secretKey;
                helib::dbgEa = &ea;
            }

            NTL::zz_p::init(p2r);
            NTL::Vec<NTL::zz_p> val0(NTL::INIT_SIZE, nslots);
            for (auto &x: val0)
                random(x);

            std::vector<NTL::ZZX> val1;
            val1.resize(nslots);
            for (long i = 0; i < nslots; i++) {
                val1[i] = NTL::conv<NTL::ZZX>(NTL::conv<NTL::ZZ>(rep(val0[i])));
            }

            helib::Ctxt c1(publicKey);
            ea.encrypt(c1, publicKey, val1);

          // Make some noise!
          // This ensures that we do not start the sequence of squarings
          // from a "fresh" ciphertext (which may not be representive).
          ea.rotate(c1, 1);
          ea.rotate(c1, -1);

          helib::Ctxt c2(c1);

            if (!helib_test::noPrint) helib::CheckCtxt(c2, "before squarings");

            long sqr_count = -1;
            helib::Ctxt next_c2(c2);
            do {
                c2 = next_c2;
                next_c2.multiplyBy(next_c2);
                sqr_count++;
            } while (next_c2.bitCapacity() >= 100);

            if (!helib_test::noPrint) {
              // compute minimal capacity before bootstrapping (rawModSwitch)
              long e = context.rcData.e;
              long q = NTL::power_long(p, e) + 1;
              double Bnd = context.boundForRecryption();
              double mfac = context.zMStar.getNormBnd();
              double min_bit_cap = log(  mfac*q / (p2r*Bnd*FHE_MIN_CAP_FRAC) )/log(2.0);

              std::cout << "min_bit_cap=" << min_bit_cap << std::endl;

              std::cout << "log2(modSwitchAddedNoiseBound)="
                   << log(c1.modSwitchAddedNoiseBound())/log(2.0) << std::endl;

                std::cout << "sqr_count=" << sqr_count << std::endl;
                if (sqr_count > 0) {
                    std::cout << "BITS-PER-LEVEL: "
                         << ((c1.bitCapacity() - c2.bitCapacity()) / double(sqr_count)) << std::endl;
                }
                helib::CheckCtxt(c2, "before recryption");
            }

            helib::resetAllTimers();

            {
                FHE_NTIMER_START(AAA_thinRecrypt);

                publicKey.thinReCrypt(c2);

            }

            if (!helib_test::noPrint) helib::CheckCtxt(c2, "after recryption");

            for (auto &x: val0) {
                for (long i: helib::range(sqr_count)) x = x * x;
            }

            for (long i = 0; i < nslots; i++) {
                val1[i] = NTL::conv<NTL::ZZX>(NTL::conv<NTL::ZZ>(rep(val0[i])));
            }

            std::vector<NTL::ZZX> val2;
            ea.decrypt(c2, secretKey, val2);

            EXPECT_EQ(val1, val2);
        }
    }

// LEGACY TEST DEFAULT PARAMETERS:
//long p=2;
//long r=1;
//long c=3;
//long bits=600;
//long t=64;
//long nthreads=1;
//long seed=0;
//long useCache=1;
//int c_m = 100;

    INSTANTIATE_TEST_SUITE_P(typicalParameters, GTestThinboot, ::testing::Values(
            //SLOW
            Parameters(2, 1, 3, 600, 64, 1, 0, 1, 100, 0, 0, 0, 0, 0, 0l, {1026, 249}, {30, -2}, {31, 41}, 1, ""),
            Parameters(17, 1, 3, 600, 64, 1, 0, 1, 100, 0, 0, 0, 0, 0, 0l, {556, 1037}, {6, 4}, {7, 5, 37}, 1, "")
            //FAST
            //Parameters(2, 1, 3, 600, 64, 1, 0, 1, 100, 0, 0, 0, 0, 0, {1026, 249}, {30, -2}, {31, 41}, 1, "")
    ));
} // namespace


#include <vector>
#include <stdio.h>
#include <assert.h>
#include <ctime>
#include <set>

#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>

#include "timer.h"
#include "vbyte.h"
#include "varintdecode.h"

static const int loops = 5;

template<typename Traits>
static void
run_compression_test(std::vector<typename Traits::type> &plain,
                std::vector<uint8_t> &z)
{
  size_t len = Traits::compress(&plain[0], &z[0], plain.size());
  z.resize(len);
  assert(len == Traits::compressed_size(&plain[0], plain.size()));
}

template<typename Traits>
static void
run_uncompression_test(const std::vector<typename Traits::type> &plain,
                std::vector<uint8_t> &z,
                std::vector<typename Traits::type> &out)
{
  Timer<boost::chrono::high_resolution_clock> t;
  for (int l = 0; l < loops; l++) {
    Traits::uncompress(&z[0], &out[0], plain.size());
    for (uint32_t j = 0; j < plain.size(); j++)
      assert(plain[j] == out[j]);
  }
  printf("    %s decode -> %f\n", Traits::name, t.seconds() / loops);
}

template<typename Traits>
static void
run_select_test(const std::vector<typename Traits::type> &plain,
                std::vector<uint8_t> &z)
{
  Timer<boost::chrono::high_resolution_clock> t;
  for (int l = 0; l < loops; l++) {
    for (uint32_t i = 0; i < plain.size(); i += 1 + plain.size() / 100) {
      uint32_t v = Traits::select(&z[0], z.size(), i);
      assert(plain[i] == v);
    }
  }
  printf("    %s select -> %f\n", Traits::name, t.seconds() / loops);
}

template<typename Traits>
static void
run_search_test(const std::vector<typename Traits::type> &plain,
                std::vector<uint8_t> &z)
{
  Timer<boost::chrono::high_resolution_clock> t;
  for (int l = 0; l < loops; l++) {
    for (size_t i = 0; i < plain.size(); i += 1 + plain.size() / 5000) {
      typename Traits::type found;
      size_t pos = Traits::search(&z[0], z.size(), plain[i], &found);
      assert(found == plain[i]);
      assert(i == pos);
    }
  }
  printf("    %s search -> %f\n", Traits::name, t.seconds() / loops);
}

template<typename Traits>
static void
run_append_test(std::vector<typename Traits::type> &plain,
                std::vector<uint8_t> &z)
{
  size_t zsize = z.size();

  z.reserve(z.size() + 1024);

  Timer<boost::chrono::high_resolution_clock> t;
  for (uint32_t i = 0; i < 100; i++) {
    typename Traits::type highest = plain[plain.size() - 1];
    typename Traits::type value = highest + 5;
    size_t size = Traits::append(z.data() + zsize, highest, value);
    zsize += size;
    plain.push_back(value);
  }
  printf("    %s append -> %f\n", Traits::name, t.seconds() / loops);

  // verify
  for (uint32_t i = 1; i <= 100; i++) {
    typename Traits::type sel = Traits::select(&z[0], zsize, plain.size() - i);
    assert(sel == plain[plain.size() - i]);
  }
}

template<typename Traits>
static void
run_tests(size_t length)
{
  std::vector<typename Traits::type> plain;
  std::vector<uint8_t> z(length * 10);
  std::vector<typename Traits::type> out(length);

  for (size_t i = 0; i < length; i++)
    plain.push_back(i * 7);

  // compress the data
  run_compression_test<Traits>(plain, z);

  // uncompress the data
  run_uncompression_test<Traits>(plain, z, out);

  // select values
  run_select_test<Traits>(plain, z);

  // search for keys
  run_search_test<Traits>(plain, z);

  // append keys. Will modify |plain| and |z|!
  run_append_test<Traits>(plain, z);
}

struct Sorted32Traits {
  typedef uint32_t type;
  static constexpr const char *name = "Sorted32";

  static size_t compress(const type *in, uint8_t *out, size_t length) {
    return vbyte_compress_sorted32(in, out, length);
  }

  static size_t compressed_size(const type *in, size_t length) {
    return vbyte_compressed_size_sorted32(in, length);
  }

  static size_t uncompress(const uint8_t *in, type *out, size_t length) {
    return vbyte_uncompress_sorted32(in, out, length);
  }

  static type select(const uint8_t *in, size_t length, size_t index) {
    return vbyte_select_sorted32(in, length, index);
  }

  static size_t search(const uint8_t *in, size_t length, type value,
                  type *result) {
    return vbyte_search_lower_bound_sorted32(in, length, value, result);
  }

  static size_t append(uint8_t *end, type highest, type value) {
    return vbyte_append_sorted64(end, highest, value);
  }
};

struct Sorted64Traits {
  typedef uint64_t type;
  static constexpr const char *name = "Sorted64";

  static size_t compress(const type *in, uint8_t *out, size_t length) {
    return vbyte_compress_sorted64(in, out, length);
  }

  static size_t compressed_size(const type *in, size_t length) {
    return vbyte_compressed_size_sorted64(in, length);
  }

  static size_t uncompress(const uint8_t *in, type *out, size_t length) {
    return vbyte_uncompress_sorted64(in, out, length);
  }

  static type select(const uint8_t *in, size_t length, size_t index) {
    return vbyte_select_sorted64(in, length, index);
  }

  static size_t search(const uint8_t *in, size_t length, type value,
                  type *result) {
    return vbyte_search_lower_bound_sorted64(in, length, value, result);
  }

  static size_t append(uint8_t *end, type highest, type value) {
    return vbyte_append_sorted64(end, highest, value);
  }
};

struct Unsorted32Traits {
  typedef uint32_t type;
  static constexpr const char *name = "Unsorted32";

  static size_t compress(const type *in, uint8_t *out, size_t length) {
    return vbyte_compress_unsorted32(in, out, length);
  }

  static size_t compressed_size(const type *in, size_t length) {
    return vbyte_compressed_size_unsorted32(in, length);
  }

  static size_t uncompress(const uint8_t *in, type *out, size_t length) {
    return vbyte_uncompress_unsorted32(in, out, length);
  }

  static type select(const uint8_t *in, size_t length, size_t index) {
    return vbyte_select_unsorted32(in, length, index);
  }

  static size_t search(const uint8_t *in, size_t length, type value,
                  type *result) {
    *result = value;
    return vbyte_search_unsorted32(in, length, value);
  }

  static size_t append(uint8_t *end, type, type value) {
    return vbyte_append_unsorted32(end, value);
  }
};

struct Unsorted64Traits {
  typedef uint64_t type;
  static constexpr const char *name = "Unsorted64";

  static size_t compress(const type *in, uint8_t *out, size_t length) {
    return vbyte_compress_unsorted64(in, out, length);
  }

  static size_t compressed_size(const type *in, size_t length) {
    return vbyte_compressed_size_unsorted64(in, length);
  }

  static size_t uncompress(const uint8_t *in, type *out, size_t length) {
    return vbyte_uncompress_unsorted64(in, out, length);
  }

  static type select(const uint8_t *in, size_t length, size_t index) {
    return vbyte_select_unsorted64(in, length, index);
  }

  static size_t search(const uint8_t *in, size_t length, type value,
                  type *result) {
    *result = value;
    return vbyte_search_unsorted64(in, length, value);
  }

  static size_t append(uint8_t *end, type, type value) {
    return vbyte_append_unsorted64(end, value);
  }
};

inline static void
test(size_t length)
{
  printf("%u, sorted, 32bit\n", (uint32_t)length);
  run_tests<Sorted32Traits>(length);

  printf("%u, sorted, 64bit\n", (uint32_t)length);
  run_tests<Sorted64Traits>(length);

  printf("%u, unsorted, 32bit\n", (uint32_t)length);
  run_tests<Unsorted32Traits>(length);

  printf("%u, unsorted, 64bit\n", (uint32_t)length);
  run_tests<Unsorted64Traits>(length);
}

int
main()
{
  uint32_t seed = std::time(0);
  printf("seed: %u\n", seed);

  test(1);
  test(2);
  test(10);
  test(16);
  test(33);
  test(42);
  test(100);
  test(128);
  test(256);
  test(333);
  test(1000);
  test(10000);
  test(20000);
  test(100000);
  test(1000000);
  test(10000000);
  return 0;
}


#pragma once

#include <vector>
#include <Eigen/Core>

#include "../geometry/LineSegment/linesegment.hh"

namespace bold
{
  struct LineJunction;

  enum class FieldSide
  {
    Unknown,
    Ours,
    Theirs
  };

  class FieldMap
  {
  public:
    static void initialise();

    // TODO return vectors of const objects so no user can modify them

    /// Returns the positions of all field lines, in the world frame.
    static std::vector<LineSegment3d> const& getFieldLines() { return d_fieldLines; }

    /// Returns the positions of all field line edges (two per field line), in the world frame.
    static std::vector<LineSegment3d> const& getFieldLineEdges() { return d_fieldLineEdges; }

    static std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> const& getFieldLineJunctions() { return d_fieldLineJunctions; }

    static std::vector<LineSegment3d> const& getCircleLines() { return d_circleLines; }

    /// Positions of the base of four goal posts, in the world frame.
    static std::vector<Eigen::Vector3d> const& getGoalPostPositions() { return d_goalPostPositions; }

    /// Positions of the base of our two goal posts (which we defend), in the world frame.
    static std::vector<Eigen::Vector3d> const& getOurGoalPostPositions() { return d_ourGoalPostPositions; }

    /// Positions of the base of our their goal posts (which we attack), in the world frame.
    static std::vector<Eigen::Vector3d> const& getTheirGoalPostPositions() { return d_theirGoalPostPositions; }

    static double getMaxDiagonalFieldDistance() { return d_maxDiagonalFieldDistance; }

    /// The long length of the field, from goal to goal.
    static double getFieldLengthX() { return d_fieldLengthX; }

    /// The narrow length of the field, from sideline to sideline.
    static double getFieldLengthY() { return d_fieldLengthY; }

    /// The minimum width of field surface found outside the outer field lines.
    static double getOuterMarginMinimum() { return d_outerMarginMinimum; }

    static double getCircleRadius() { return d_circleRadius; }

    /// The width between each of a pair of goal posts.
    /// Distance along the Y axis is measured between the midpoint Z vectors.
    static double getGoalY() { return d_goalY; };

    /// The shorter length of the goal (penalty) area.
    /// Perpendicular to goal line
    static double getGoalAreaLengthX() { return d_goalAreaLengthX; }

    /// The longer length of the goal (penalty) area.
    /// Larger than the distance between the goal posts.
    static double getGoalAreaLengthY() { return d_goalAreaLengthY; }

    static double getGoalPostDiameter() { return d_goalPostDiameter; }

    static double getBallDiameter() { return d_ballDiameter; }
    static double getBallRadius()   { return d_ballDiameter / 2.0; }

  private:
    FieldMap() = delete;

    static std::vector<LineSegment3d> d_fieldLines;
    static std::vector<LineSegment3d> d_fieldLineEdges;
    static std::vector<LineJunction, Eigen::aligned_allocator<LineJunction>> d_fieldLineJunctions;
    static std::vector<LineSegment3d> d_circleLines;
    static std::vector<Eigen::Vector3d> d_goalPostPositions;
    static std::vector<Eigen::Vector3d> d_ourGoalPostPositions;
    static std::vector<Eigen::Vector3d> d_theirGoalPostPositions;

    static double d_fieldLengthX;
    static double d_fieldLengthY;
    static double d_outerMarginMinimum;
    static double d_circleRadius;
    static double d_maxDiagonalFieldDistance;
    static double d_goalY;
    static double d_goalAreaLengthX;
    static double d_goalAreaLengthY;
    static double d_goalPostDiameter;
    static double d_ballDiameter;
  };
}


#ifndef TRANSIT_NAVIGATION_ALGORITHMS_TIMETABLE_DIJKSTRA_HPP_
#define TRANSIT_NAVIGATION_ALGORITHMS_TIMETABLE_DIJKSTRA_HPP_

#include "navigation/routing_algorithm.hpp"

#include "date/time.hpp"
#include "id/line.hpp"
#include "id/stop.hpp"
#include "tool/container/kary_heap.hpp"

#include <boost/optional.hpp>

namespace nepomuk
{

namespace search
{
class StopToLine;
}
namespace timetable
{
class TimeTable;
}

namespace navigation
{

class Route;

namespace algorithm
{

class TimeTableDijkstra : public RoutingAlgorithm
{
  public:
    using Base = RoutingAlgorithm;
    TimeTableDijkstra(timetable::TimeTable const &time_table,
                      search::StopToLine const &stop_to_line);

    // query a route between two stops
    boost::optional<Route> operator()(date::Time const departure,
                                      StopID const origin,
                                      StopID const destination) const override final;

    boost::optional<Route> operator()(date::Time const departure,
                                      std::vector<ADLeg> const &origins,
                                      std::vector<ADLeg> const &destinations) const override final;

  private:
    struct ReachedVia
    {
        StopID parent_stop;
        LineID on_line;
        date::Time parent_departure;
    };
    using FourHeap = tool::container::KAryHeap<StopID, date::Time, 4, ReachedVia>;

    // the unmodified timetable data to route on
    timetable::TimeTable const &time_table;

    // the look-up for lines from a given stop
    search::StopToLine const &stop_to_line;

    void relax_one(FourHeap &heap) const;

    std::vector<Base::PathEntry> extract_path(StopID current_stop, FourHeap const &heap) const;
};

} // namespace algorithm
} // namespace navigation
} // namespace nepomuk

#endif // TRANSIT_NAVIGATION_ALGORITHMS_TIMETABLE_DIJKSTRA_HPP_


/// @file   utility/zorder/template_math.hh
/// @brief  a place for some template loops to live
/// @author will sheffler

#ifndef INCLUDED_util_template_loop_HH
#define INCLUDED_util_template_loop_HH

#include <boost/static_assert.hpp>
#include <cstddef>

namespace scheme {
namespace util {

    template<int DIM>
    struct NESTED_FOR {
        // this is to make sure a compile errer happens if DIM not implemented
        BOOST_STATIC_ASSERT_MSG(DIM==123456789,"NESTED_FOR not implemented for requested DIM");
    };

    template<>
    struct NESTED_FOR<1> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }
        }
    };

    template<>
    struct NESTED_FOR<2> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}
        }
    };

    template<>
    struct NESTED_FOR<3> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}
        }
    };

    template<>
    struct NESTED_FOR<4> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}
        }
    };

    template<>
    struct NESTED_FOR<5> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}
        }
    };

    template<>
    struct NESTED_FOR<6> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[5] = lb[5]; i[5] <= ub[5]; ++i[5]){
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}}
        }
    };

    template<>
    struct NESTED_FOR<7> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[6] = lb[6]; i[6] <= ub[6]; ++i[6]){
            for(i[5] = lb[5]; i[5] <= ub[5]; ++i[5]){
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}}}
        }
    };

    template<>
    struct NESTED_FOR<8> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[7] = lb[7]; i[7] <= ub[7]; ++i[7]){
            for(i[6] = lb[6]; i[6] <= ub[6]; ++i[6]){
            for(i[5] = lb[5]; i[5] <= ub[5]; ++i[5]){
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}}}}
        }
    };

    template<>
    struct NESTED_FOR<9> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[8] = lb[8]; i[8] <= ub[8]; ++i[8]){
            for(i[7] = lb[7]; i[7] <= ub[7]; ++i[7]){
            for(i[6] = lb[6]; i[6] <= ub[6]; ++i[6]){
            for(i[5] = lb[5]; i[5] <= ub[5]; ++i[5]){
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}}}}}
        }
    };

    template<>
    struct NESTED_FOR<10> {
        template<class Functor, class Indices>
        NESTED_FOR(Indices const & lb, Indices const & ub, Functor & f){
            Indices i;
            for(i[9] = lb[9]; i[9] <= ub[9]; ++i[9]){
            for(i[8] = lb[8]; i[8] <= ub[8]; ++i[8]){
            for(i[7] = lb[7]; i[7] <= ub[7]; ++i[7]){
            for(i[6] = lb[6]; i[6] <= ub[6]; ++i[6]){
            for(i[5] = lb[5]; i[5] <= ub[5]; ++i[5]){
            for(i[4] = lb[4]; i[4] <= ub[4]; ++i[4]){
            for(i[3] = lb[3]; i[3] <= ub[3]; ++i[3]){
            for(i[2] = lb[2]; i[2] <= ub[2]; ++i[2]){
            for(i[1] = lb[1]; i[1] <= ub[1]; ++i[1]){
            for(i[0] = lb[0]; i[0] <= ub[0]; ++i[0]){
                f(i);
            }}}}}}}}}}
        }
    };

}
}

#endif // INCLUDED_util_template_math_HH


#define PY_ARRAY_UNIQUE_SYMBOL cve_ARRAY_API

#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>
#include <imgproc/imgproc.hpp>
#include <bgsegm/bgsegm.hpp>

namespace cve {

    using namespace boost::python;

/**
 * Example function. Basic inner matrix product using explicit matrix conversion.
 * @param left left-hand matrix operand (NdArray required)
 * @param right right-hand matrix operand (NdArray required)
 * @return an NdArray representing the dot-product of the left and right operands
 */
    PyObject *dot(PyObject *left, PyObject *right) {

        cv::Mat leftMat, rightMat;
        leftMat = pbcvt::fromNDArrayToMat(left);
        rightMat = pbcvt::fromNDArrayToMat(right);
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        // Check that the 2-D matrices can be legally multiplied.
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;
        PyObject * ret = pbcvt::fromMatToNDArray(result);
        return ret;
    }

//This example uses Mat directly, but we won't need to worry about the conversion
/**
 * Example function. Basic inner matrix product using implicit matrix conversion.
 * @param leftMat left-hand matrix operand
 * @param rightMat right-hand matrix operand
 * @return an NdArray representing the dot-product of the left and right operands
 */
    cv::Mat dot2(cv::Mat leftMat, cv::Mat rightMat) {
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;

        return result;
    }

#if (PY_VERSION_HEX >= 0x03000000)

        static void *init_ar() {
#else
        static void init_ar(){
#endif
        Py_Initialize();

        import_array();
        return NUMPY_IMPORT_ARRAY_RETVAL;
    }

    BOOST_PYTHON_MODULE (cve) {
        //using namespace XM;
        init_ar();

        //initialize converters
        to_python_converter<cv::Mat,
		        pbcvt::matToNDArrayBoostConverter>();
        pbcvt::matFromNDArrayBoostConverter();

        //expose module-level functions
        def("dot", dot);
        def("dot2", dot2);
        def("non_max_suppression", cve::non_max_suppression);
        class_<BackgroundSubtractorIMBS>("BackgroundSubtractorIMBS", "Independent Multimodal Background Subtraction, D. Bloisi and L. Locchi, 2012",
                                         init<optional<double, unsigned int,unsigned int,
                                                 double, unsigned int, unsigned int,
                                                 double, double, double, double,
                                                 double, double,
                                                 bool, bool>>(args("fps","fg_threshold", "association_threshold",
                                                             "sampling_interval", "min_bin_height", "num_samples",
                                                             "alpha", "beta", "tau_s", "tau_h",
                                                             "min_area", "persistence_period",
                                                             "use_morphological_filtering", "rebuild_model_continously"))
        ).def("apply", &BackgroundSubtractorIMBS::apply_pythonic);
    }

} //end namespace cve


/**
 * Copyright (c) 2016  Zubax Robotics OU  <info@zubax.com>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following
 *    disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *    products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <Eigen/Eigen>
#include <algorithm>
#include <utility>
#include <cassert>
#include <type_traits>
#include <cmath>
#include <cstdint>
#include <zubax_chibios/util/heapless.hpp>

namespace math
{

using Scalar = float;
using Const = const Scalar;

template <int Rows, int Cols>
using Matrix = Eigen::Matrix<Scalar, Rows, Cols>;

template <int Rows>
using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Rows>;

template <int Size>
using Vector = Matrix<Size, 1>;

constexpr inline Scalar convertKelvinToCelsius(Const kelvin)
{
    return kelvin - 273.15F;
}

constexpr inline Scalar convertCelsiusToKelvin(Const kelvin)
{
    return kelvin + 273.15F;
}

/**
 * Constants
 */
constexpr auto Pi  = Scalar(3.141592653589793);
constexpr auto Pi2 = Scalar(6.283185307179586);

/**
 * Constrains the angle within [0, Pi*2]
 */
inline Scalar normalizeAngle(Scalar x)
{
    if (x >= Pi2)
    {
        return x - Pi2;
    }
    else if (x < 0)
    {
        return x + Pi2;
    }
    else
    {
        return x;
    }
}

/**
 * Inclusive range of the form [min, max].
 */
template <typename T = Scalar>
struct Range
{
    T min = T(0);
    T max = T(0);

    constexpr Range() { }

    constexpr Range(const T min, const T max) :
        min(min),
        max(max)
    {
        assert(min < max);
    }

    constexpr T constrain(const T value) const
    {
        return std::min(std::max(value, min), max);
    }

    constexpr bool contains(const T value) const
    {
        return (value >= min) && (value <= max);
    }

    auto toString() const
    {
        return os::heapless::concatenate("[", min, ", ", max, "]");
    }
};

/**
 * Simple moving average of arbitrary depth.
 */
template <unsigned Depth, typename T>
class SimpleMovingAverageFilter
{
    T history_[Depth] = {};
    T sum_;
    unsigned next_index_ = 0;

public:
    SimpleMovingAverageFilter() :
        sum_(T() * Depth)
    {
        std::fill_n(std::begin(history_), Depth, T());
    }

    explicit SimpleMovingAverageFilter(const T& initial_value) :
        sum_(initial_value * Depth)
    {
        std::fill_n(std::begin(history_), Depth, initial_value);
    }

    void update(const T& value)
    {
        sum_ -= history_[next_index_];
        history_[next_index_] = value;
        sum_ += history_[next_index_];

        next_index_++;
        if (next_index_ >= Depth)
        {
            next_index_ = 0;
        }
    }

    void reset(const T& initial_value)
    {
        sum_ = initial_value * Depth;
        std::fill_n(std::begin(history_), Depth, initial_value);
    }

    T getValue() const
    {
        return sum_ / Depth;
    }
};

/**
 * Copmutes average over the entire collected dataset.
 * Useful for calibration and other batch measurements, e.g. motor identification.
 */
template <typename T = double>
class CumulativeAverageComputer
{
    std::uint32_t num_samples_ = 0;
    T accumulator_{};

    using FloatingPointScalar = typename std::conditional<std::is_floating_point<T>::value, T, Scalar>::type;

public:
    CumulativeAverageComputer() :
        accumulator_()
    { }

    explicit CumulativeAverageComputer(const T& init) :
        accumulator_(init)
    { }

    void addSample(const T& x)
    {
        num_samples_++;
        accumulator_ += x;
    }

    T getAverage() const
    {
        if (num_samples_ > 0)
        {
            return T(accumulator_ / FloatingPointScalar(num_samples_));
        }
        else
        {
            assert(false);
            return T();
        }
    }

    auto getNumSamples() const { return num_samples_; }
};

inline Vector<2> sincos(Scalar x)
{
    // Normally this should be replaced with a call to sincos(), but this is not a part of C++ standard library.
    // However, the compiler should be able to replace the two separate but localized calls to
    // sin()/cos() with one sincos().
    return { std::sin(x), std::cos(x) };
}

/**
 * Implementation details, do not use directly.
 */
namespace impl_
{

template <typename Scalar, int Size>
using RowVector = Eigen::Matrix<Scalar, 1, Size, Eigen::RowMajor>;

template <typename Scalar, int Size, typename Head>
inline void fillVector(RowVector<Scalar, Size>& vector,
                       int next_index,
                       Head head)
{
    vector[next_index] = static_cast<Scalar>(head);
}

template <typename Scalar, int Size, typename Head, typename... Tail>
inline void fillVector(RowVector<Scalar, Size>& vector,
                       int next_index,
                       Head head,
                       Tail... tail)
{
    vector[next_index] = static_cast<Scalar>(head);
    fillVector(vector, next_index + 1, tail...);
}

template <typename Scalar, int Rows, int Columns>
inline void fillMatrix(Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                       int next_row,
                       const impl_::RowVector<Scalar, Columns>& head)
{
    matrix.row(next_row) = head;
}

template <typename Scalar, int Rows, int Columns, typename... Tail>
inline void fillMatrix(Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                       int next_row,
                       const impl_::RowVector<Scalar, Columns>& head,
                       Tail... tail)
{
    matrix.row(next_row) = head;
    fillMatrix(matrix, next_row + 1, tail...);
}

template <typename Scalar, int DiagonalSize>
inline void fillDiagonalMatrix(Eigen::DiagonalMatrix<Scalar, DiagonalSize>& matrix,
                               int next_position,
                               Scalar head)
{
    matrix.diagonal()[next_position] = static_cast<Scalar>(head);
}

template <typename Scalar, int DiagonalSize, typename... Tail>
inline void fillDiagonalMatrix(Eigen::DiagonalMatrix<Scalar, DiagonalSize>& matrix,
                               int next_position,
                               Scalar head,
                               Tail... tail)
{
    matrix.diagonal()[next_position] = static_cast<Scalar>(head);
    fillDiagonalMatrix<Scalar, DiagonalSize>(matrix, next_position + 1, tail...);
}

} // namespace impl_

/**
 * Creates a row vector of arbitrary length, represented as a static row vector Eigen::Matrix<>.
 * Scalar type can be overriden.
 * See also @ref makeMatrix().
 */
template <typename Scalar = Scalar, typename... Tail>
inline impl_::RowVector<Scalar, sizeof...(Tail)>
makeRow(Tail... tail)
{
    impl_::RowVector<Scalar, sizeof...(Tail)> vector;
    impl_::fillVector(vector, 0, tail...);
    return vector;
}

/**
 * Creates a static matrix (Eigen::Matrix<>) of arbitrary size from a set of row vectors.
 * The matrix will use default layout, which is columnn-major for Eigen.
 * Note that row vectors of unequal size trigger a compile-time error.
 * Scalar type will be deduced automatically from the first row.
 * This function is totally type safe.
 * Usage:
 *      makeMatrix(makeRow(1, 2),
 *                 makeRow(3, 4))  // Produces a 2x2 matrix
 */
template <typename Scalar, int Columns, typename... Tail>
inline Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns>
makeMatrix(const impl_::RowVector<Scalar, Columns>& head,
           Tail... tail)
{
    Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns> matrix;
    matrix.setZero();
    impl_::fillMatrix(matrix, 0, head, tail...);
    return matrix;
}

/**
 * A helper like @ref makeMatrix() that creates diagonal matrix.
 * Size is dereved from the argument list.
 * Scalar type can be overriden.
 */
template <typename Scalar = Scalar, typename... Diagonal>
inline Eigen::DiagonalMatrix<Scalar, sizeof...(Diagonal)>
makeDiagonalMatrix(Diagonal... diag)
{
    Eigen::DiagonalMatrix<Scalar, sizeof...(Diagonal)> matrix;
    matrix.setZero();
    impl_::fillDiagonalMatrix<Scalar, sizeof...(Diagonal)>(matrix, 0, diag...);
    return matrix;
}

/**
 * Printing helpers
 * @{
 */
enum class StringRepresentation
{
    SingleLine,
    MultiLine
};

template <typename Scalar, int Rows, int Columns>
inline auto toString(const Eigen::Matrix<Scalar, Rows, Columns>& matrix,
                     const StringRepresentation representation = StringRepresentation::SingleLine)
{
    os::heapless::String<Rows * Columns * 20> s;

    for (int row = 0; row < Rows; row++)
    {
        if (row > 0)
        {
            s.append((representation == StringRepresentation::MultiLine) ? "\n" : "; ");
        }
        for (int column = 0; column < Columns; column++)
        {
            if (column > 0)
            {
                s.append(", ");
            }
            s.append(matrix(row, column));
        }
    }

    return s;
}
/**
 * @}
 */

}


#ifndef PATH_FINDER_HPP_DEF__
#define PATH_FINDER_HPP_DEF__
#include <boost/function.hpp>
#include "rexio/commons.h++"
#include <list>
#include <boost/scoped_array.hpp>
#include <boost/array.hpp>
#include <boost/noncopyable.hpp>

// Path Finding algorithm and more...
class MapData;

//! Class impelemts path finding algorithm for whole game
//! simple interface (only 7 functions including ctor and implicit
//! dtor) covers fairly complex structure.
class PathFinder: boost::noncopyable
{
public:

	// Path::begin() returns starting field of to-be-computed path
	// Path::end()-- returns goal (or at least as close as possible to it)
	typedef std::list<Scr::Position> PositionList ;
	typedef PositionList Path ;

	//! MUST RETURN TRUE IF CHARACTER CAN PASS FIELD
	typedef boost::function <bool (Scr::Position)> ObstacleDetector;
	//! returns movement cost for specific field
	typedef boost::function <int (Scr::Position)> CostCalculator;
protected:
	//! list of directions: defines only  constructor
	class Directions: public PositionList
	{
		/*
		/   128  1  2
		/      \ | /
		/  64--  x  --4
		/      / | \
		/    32  16 8  */
		static const unsigned char even_col_mask;
		static const unsigned char odd_col_mask ;

		static const
		Scr::Vector AvailDirections[] ;
	public:
//		PositionList LegalDirections;
		//! ctor fills list with all directions, that
		//! are reachable from p in map pMD
		//! \param p position
		//! \param pMD mapdata
		Directions (MapData * pMD, Scr::Position p);
	};
	//! for each element of map create record of data used to
	//! determine, if this location is good waypoint.
	class PFDataEntry
	{
	public:
		//! legal directions from here
		Directions D1;// directions legal from here

		//! L stands for length-of-path from starting point to here.
		typedef boost::array<unsigned,3> L;
		//! quality rating of this location (used when processing
		//! final result): when determinating best path locations are
		//! compared lexicographically by L1.
		L L1;//[0]length of path as number of tiles
		//[1]length of path as number of passable obstacles

		//!constructor
		//! \param  pMD - map data (used in constructor only, to
		//! compute D1 - not stored)
		//! \param  p - coordinates, this Entry refers to (used
		//! in constructor only, to compute D1 - not stored)
		//! \param L1 initializer as array
		PFDataEntry(MapData * pMD, Scr::Position p, int * l);
		//! as above, but initializer in form of L
		PFDataEntry(MapData * pMD, Scr::Position p, L l);
	};
	typedef std::auto_ptr<PFDataEntry> pPFDataEntry;

	// STL containers of auto_ptr's are bad idea, as they are ineligable
	// for any operations that require copying (including quick sort
	// algorithm ), but we don't even need resizing... so let's use dumb
	// dynamic array (in boost packaging to ensure correct free)

	//! Array of per-field metadata stored as auto_ptr's (so we
	//! allocate only 1 pointer per unused field.
	typedef
	boost::scoped_array<boost::scoped_array<pPFDataEntry> > PFData;
	// (as overhead of reference counted shared_ptr is not what we want)

	//! instance of PFData;
	PFData PFD1; // path finder data (used during path computation)
	//! "open" list: positions, from which we will start processing
	//! next time
	PositionList StartPoints;
	//! final result (when is ready)
	Path P1;
	//! mapdata, we refer to
	MapData * pMD1;
	//! starting position
	Scr::Position start;
	//! goal
	Scr::Position finish;
	//! obstacle detecting function (returns true if we can pass this
	//! location)
	ObstacleDetector OD1;
	//! function computing cost of movement to specific location
	CostCalculator CC1;
	//! length maximum (don't search longer paths)
	//! \note: this variable is modified by various functions, such as
	//! ComputePath or MarkPathRecursive, and there is no way of
	//! explicitly specifiing it.
	unsigned lm;
	//! Real "worker" function: fills PFD1 array until lm is matched
	//! of finish is found
	//! \param l length to this field
	void MarkPathRecursive(Scr::Position p, PFDataEntry::L l);

	//! strict weak ordering of points based on lex. comp. of
	//! PFDataEntries (for STL)
	//! \param p positions to compare
	bool BetterCoords(Scr::Position ,Scr::Position );
	//! return true, if found target
	void ComputePath();
#ifndef NDEBUG
	//! just calculated
	//! \note this feature is enabled only for debug builds: specific
	//! map  view can be triggered to show PF stats.
	PositionList Closed;
#endif
public:
	//! ONLY constructor (no cpy ctors or assignment ops!)
	//! \param pMD map data, we refer to
	//! \param s start
	//! \param f finish
	//! \param OD Obstacle detector function (see ObstacleDetector
	//! typedef spec for requirements, this function has tho meet)
	//! \param CC Cost calculator  function (see ObstacleDetector
	//! typedef spec for requirements, this function has tho meet)
	PathFinder( MapData * pMD,
				Scr::Position s,
				Scr::Position f,
				ObstacleDetector OD,
				CostCalculator CC);

	//! Recalculate path w/o changing positions (useful to inform
	//! PathFinder, that the tactical situation has changed:
	//! i.e. obstacle has been destroyed, and passage is easier)
	void Reset();

	//! recalculate for diffrent start and finish. Reset all data only
	//! if s differs from old start
	//! \param s start
	//! \param f finish
	void ConditionalReset(	Scr::Position s,
							Scr::Position f);

	//! Change start (this requires resetting all)
	//! \param s start
	void ChangeStart(Scr::Position s);
	//! Change finish w/o resetting data already calculated
	//! \param f finish
	void ChangeFinish(Scr::Position f);
	//! Fanfare!!! After all this complex operation we can just get
	//! our list of points!!
	Path & GetPath() {return P1;}
#ifndef NDEBUG
	//! Get list of fields visited during pf algorithm
	//! \note this function is enabled only for debug builds: specific
	//! map  view can be triggered to show PF stats.
	PositionList & GetClosed() {return Closed;}
	//! Get list of points, that were planned to visit, but finally
	//! ignored due to lm (that is path length limit).
	//! \note this function is enabled only for debug builds: specific
	//! map  view can be triggered to show PF stats.
	PositionList & GetOpen() {return StartPoints;}
#endif
}; /// PathFinder

//! Generate shared_ptr to new PathFinder objec
//! \param pMD map data, we refer to
//! \param s start
//! \param f finish
//! \param OD Obstacle detector function (see ObstacleDetector
//! typedef spec for requirements, this function has tho meet)
//! \param CC Cost calculator  function (see ObstacleDetector
//! typedef spec for requirements, this function has tho meet)
//! \note: opposite to default constructor this is templatized
//! function, that allows ANY compatible entity to be converted to
//! ObstacleDetector and CostCalculator.
template <typename __ObstacleDetector, typename __CostCalculator>
std::tr1::shared_ptr< PathFinder >
FindPath( MapData * pMD,
		  Scr::Position s,
		  Scr::Position f,
		  __ObstacleDetector OD,
		  __CostCalculator CC
	)
{
	return
		std::tr1::shared_ptr< PathFinder >
		(
			new PathFinder
			(pMD,s,f,PathFinder::ObstacleDetector(OD),
				PathFinder::CostCalculator(CC))
			);
} /// FindPath

#endif


//
// hash function from http://www.azillionmonkeys.com/qed/hash.html,
// written by Paul Hsieh.  Faster and apparently better than bob jenkins' hash.
//
// Note, there's a newer version of this available, which doesn't seem to work
// as well as this one for hashing Foyfi fingerprints.  If that ever becomes
// an issue, MurmurHash2 is probably a better bet.

#include <boost/cstdint.hpp>

namespace DACLIB {

#undef getshort
#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__)
#define getshort(d) (*((const unsigned short *) (d)))
#endif
#if defined(_MSC_VER) || defined(__BORLANDC__)
#define getshort(d) (*((const unsigned short *) (d)))
#endif

#if !defined(getshort)
#define getshort(d) ((((const unsigned char *) (d))[1] << 8UL)\
                     +((const unsigned char *) (d))[0])
#endif

unsigned int SuperFastHash (const char * data, int len) {
boost::uint32_t hash;
int i, rem;

    if (len <= 0 || !data) return 0;

    hash = (boost::uint32_t) len; /* Avoid 0 -> 0 trap */
    rem = len & 3;
    len >>= 2;

    /* Main loop */
    for (i=0; i < len; i++) {
        hash += getshort (data);
        data += 2*sizeof (char);
        hash ^= hash << 16;
        hash ^= getshort (data) << 11;
        data += 2*sizeof (char);
        hash += hash >> 11;
    }

    /* Handle end cases */
    switch (rem) {
        case 3: hash += getshort (data);
                hash ^= hash << 16;
                hash ^= data[2 * sizeof (char)] << 18;
                hash += hash >> 11;
                break;
        case 2: hash += getshort (data);
                hash ^= hash << 11;
                hash += hash >> 17;
                break;
        case 1: hash += *data;
                hash ^= hash << 10;
                hash += hash >> 1;
    }

    /* Force "avalanching" of final 127 bits */
    hash ^= hash << 3;
    hash += hash >> 5;
    hash ^= hash << 2;
    hash += hash >> 15;
    hash ^= hash << 10;

    return hash;
}

}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompnoneing file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_NONE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_SIMD_FUNCTION_NONE_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/detail/constant/butsign.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( none_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE bool operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bool(_mm256_testz_si256(a0, Allbits<A0>()));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( none_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::floating_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE bool operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0>;
      return bool(_mm256_testz_si256(bitwise_cast<iA0>(a0), Butsign<iA0>()));
    }
  };

} } }

#endif


//
// Created by Phalange on 2022/1/17.
// C'est La vie.Enjoy it! :D
//

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

namespace myslam {

    VisualOdometry::VisualOdometry()
            : state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0) {
        num_of_features_ = Config::get<int>("number_of_features");
        scale_factor_ = Config::get<double> ("scale_factor");
        level_pyramid_ = Config::get<int> ("level_pyramid");
        match_ratio_ = Config::get<float> ("match_ratio");
        max_num_lost_ = Config::get<float>("max_num_lost");
        min_inliers_ = Config::get<int> ("min_inliers");
        key_frame_min_rot = Config::get<double> ("keyframe_rotation");
        key_frame_min_trans = Config::get<double> ("keyframe_translation");
        orb_ = cv::ORB::create(num_of_features_,scale_factor_,level_pyramid_);

    }

    VisualOdometry::~VisualOdometry() {

    }

    bool VisualOdometry::addFrame(Frame::Ptr frame) {

        switch (state_) {
            case INITIALIZING:
            {
                state_ = OK;
                curr_ = ref_ = frame;
                map_->insertKeyFrame(frame);
                // extract features from first frame
                extractKeyPoints();
                computeDescriptors();
                // compute the 3d position of features in ref frame
                setRef3DPoints();
                break;
            }
            case OK:
            {
                curr_ = frame;
                extractKeyPoints();
                computeDescriptors();
                featureMatching();
                poseEstimationPnP();
                if(checkEstimatedPose() == true) // a good estimation
                {
                    curr_->T_c_w_ = T_c_r_estimated_ * ref_ -> T_c_w_;
                    ref_ = curr_;
                    setRef3DPoints();
                    num_lost_ = 0;
                    if(checkKeyFrame() == true)
                    {
                        addKeyFrame();
                    }
                }
                else
                {
                    num_lost_++;
                    if(num_lost_ > max_num_lost_){
                        state_ = LOST;
                    }
                    return false;
                }
                break;
            }
            case LOST:
            {
                cout <<"vo has lost." << endl;
                break;
            }

        }
        return true;
    }

    void VisualOdometry::extractKeyPoints() {
        orb_->detect(curr_->color_,keypoints_curr_);
    }
    void VisualOdometry::computeDescriptors() {
        orb_->compute( curr_->color_,keypoints_curr_,descriptors_curr_);
    }

    void VisualOdometry::featureMatching() {

        // match desp_ref and desp_curr,use OpenCV's brute force match
        vector<cv::DMatch> matches;
        cv::BFMatcher matcher (cv::NORM_HAMMING);
        matcher.match(descriptors_ref_,descriptors_curr_,matches);
        float min_dis = std::min_element(
                matches.begin(),matches.end(),[](const cv::DMatch& m1,const cv::DMatch& m2)
                {
                 return m1.distance < m2.distance;
                })->distance;

        feature_matches_.clear();
        for(cv::DMatch& m : matches)
        {
            if(m.distance < max<float> (min_dis * match_ratio_,30.0))
            {
                feature_matches_.push_back(m);

            }

        }
        cout <<"good matches:" << feature_matches_.size()<<endl;
    }

    void VisualOdometry::setRef3DPoints() {
        // select the features with depth measurements
        // think of cur as ref
        pts_3d_ref_.clear();
        descriptors_ref_ = Mat();
        for(size_t i=0;i<keypoints_curr_.size();i++){
            double d = ref_->findDepth(keypoints_curr_[i]);
            if( d > 0){
                Vector3d p_cam = ref_ ->camera_->pixel2camera(
                        Vector2d(keypoints_curr_[i].pt.x,keypoints_curr_[i].pt.y),d
                        );
                // vector3d transfrom to Point3f;
                pts_3d_ref_.push_back(cv::Point3f(p_cam(0,0),p_cam(1,0),p_cam(2,0)));

                descriptors_ref_.push_back(descriptors_curr_.row(i));
            }
        }
    }

    void VisualOdometry::poseEstimationPnP() {
        vector<cv::Point3f> pts3d;
        vector<cv::Point2f> pts2d;

        for(cv::DMatch m:feature_matches_){
            pts3d.push_back(pts_3d_ref_[m.queryIdx]);
            pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
        }

        Mat K = (cv::Mat_<double>(3,3) <<
                ref_->camera_->fx_,0,ref_->camera_->cx_,
                0,ref_->camera_->fy_,ref_->camera_->cy_,
                0,0,1);

        Mat rvec,tvec,inliers;
        cv::solvePnPRansac(pts3d,pts2d,K,Mat(),rvec,tvec,false,100,4.0,0.99,inliers);
        num_inliers_ = inliers.rows;
        cout<<"pnp inliers: " <<num_inliers_<<endl;
        T_c_r_estimated_ = Sophus::SE3(
                Sophus::SO3(rvec.at<double>(0,0),rvec.at<double>(1,0),rvec.at<double>(2,0)),
                Eigen::Vector3d(tvec.at<double>(0,0),tvec.at<double>(1,0),tvec.at<double>(2,0))
                );

        }

    bool VisualOdometry::checkEstimatedPose() {

        // check if the estimated pose is good
        if(num_inliers_ < min_inliers_){
           cout<<"reject because inlier is too small:" << num_inliers_<<endl;
            return false;
        }
        // if the motion is too large,it is probably wrong
        Sophus::Vector6d d = T_c_r_estimated_.log(); // lie algebra
        if(d.norm() > 5.0)
        {
            cout <<"reject because motion is too large: " << d.norm()<<endl;
            return false;
        }

        return true;
    }

    bool VisualOdometry::checkKeyFrame() {

        Sophus::Vector6d d = T_c_r_estimated_.log();
        Vector3d trans = d.head<3>();
        Vector3d rot = d.tail<3>();
        if(rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
            return true;
        return false;
    }

    void VisualOdometry::addKeyFrame() {
        cout <<"adding a key-frame"<<endl;
        map_->insertKeyFrame(curr_);
    }

}


#pragma once

#include <rosbag/view.h>
#include <QApplication>

#include <opencv2/core.hpp>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>

#include <pcl_ros/point_cloud.h>

#include <Eigen/Dense>

#include <map>
#include <memory>

#include <point_cloud_object_detection_and_recognition/pcodar_types.hpp>

#include <mil_msgs/PerceptionObjectArray.h>

// namespace pcod
// {
// namespace label_tool
// {

struct course_object
{
  mil_msgs::PerceptionObject p_object;
  std::vector<Eigen::Vector4d> bounding_box_points;
  bool visualize = true;
  cv::Scalar color;
};

using id_to_labeled_object = std::map<uint16_t, course_object>;

class label_model
{
public:
  label_model(int argc, char* argv[], std::shared_ptr<id_to_labeled_object> object_map_ptr);
  bool has_next_image();
  cv::Mat get_next_image();
  void generate_bag();
  void restart();
  void populate_map();

private:
  std::shared_ptr<ros::NodeHandle> nh_;
  std::shared_ptr<rosbag::View> v_;

  std::string input_bag_name_;
  std::string output_bag_name_;

  Eigen::Matrix<double, 3, 4> intrinsics_;
  Eigen::Affine3d e_baselink_to_stereoright_;
  Eigen::Affine3d e_velodyne_to_baselink_;

  rosbag::Bag bag_;

  pcodar::point_cloud mega_cloud_;

  rosbag::View::iterator bag_iter_;
  nav_msgs::Odometry::ConstPtr last_odom_;
  sensor_msgs::ImageConstPtr last_image_;

  std::shared_ptr<id_to_labeled_object> object_map_ptr_;
};

// }  // namespace label_tool
// }  // namespace pcod


#include <iostream>
#include <cmath>
#include <vector>

#include <uv.h>
#include <node.h>
#include <node_buffer.h>

#include <Eigen/Dense>

#include "search.h"

using namespace v8;

Handle<Value> Search(const Arguments& args) {
    HandleScope scope;

    Local<String> rows = String::New("rows");
    Local<String> cols = String::New("cols");
    Local<String> data = String::New("data");
    Local<String> channels = String::New("channels");

    // unwrap arguments
    Handle<Object> matrix1 = Handle<Object>::Cast(args[0]);
    Handle<Object> matrix2 = Handle<Object>::Cast(args[1]);

    const unsigned int colorTolerance = args[2]->IsNumber() ? args[2]->Int32Value() : 0;
    const unsigned int pixelTolerance = args[3]->IsNumber() ? args[3]->Int32Value() : 0;

    Persistent<Function> callback = Persistent<Function>::New(Local<Function>::Cast(args[4]));

    // check for required matrix properties
    if ( ! matrix1->Has(rows) || ! matrix1->Has(cols) || ! matrix1->Has(channels) || ! matrix1->Has(data)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'imgMatrix'")));
    }

    if ( ! matrix2->Has(rows) || ! matrix2->Has(cols) || ! matrix2->Has(channels) || ! matrix2->Has(data)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'tplMatrix'")));
    }

    // unwrap matrices
    const unsigned int m1Rows = matrix1->Get(rows)->Uint32Value();
    const unsigned int m1Cols = matrix1->Get(cols)->Uint32Value();
    const unsigned int m1Channels = matrix1->Get(channels)->Uint32Value();

    const unsigned int m2Rows = matrix2->Get(rows)->Uint32Value();
    const unsigned int m2Cols = matrix2->Get(cols)->Uint32Value();
    const unsigned int m2Channels = matrix2->Get(channels)->Uint32Value();

    // channel count validation
    if (m1Channels < 1 || m2Channels < 1 || m1Channels > 4 || m2Channels > 4) {
        return ThrowException(Exception::TypeError(String::New("Bad number of channels")));
    }

    if (abs((int) m2Channels - (int) m1Channels) > 1) {
        return ThrowException(Exception::TypeError(String::New("Channel mismatch")));
    }

    // unwrap matrix.data
    Handle<Object> m1Data = Handle<Object>::Cast(matrix1->Get(data));
    Handle<Object> m2Data = Handle<Object>::Cast(matrix2->Get(data));

    // TODO: consider removal of channels property
    // declared and actual channel count validation
    if (m1Channels != m1Data->Get(String::New("length"))->Uint32Value()) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'imgMatrix'")));
    }

    if (m2Channels != m2Data->Get(String::New("length"))->Uint32Value()) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'tplMatrix'")));
    }

    // unwrap matrix.data channels
    Handle<Object> m1K, m1R, m1G, m1B, m1A;
    Handle<Object> m2K, m2R, m2G, m2B, m2A;

    // TODO: check matrix data type
    if (m1Channels == 1 || m1Channels == 2) {
        m1K = Handle<Object>::Cast(m1Data->Get(0));
    } else if (m1Channels == 3 || m1Channels == 4) {
        m1R = Handle<Object>::Cast(m1Data->Get(0));
        m1G = Handle<Object>::Cast(m1Data->Get(1));
        m1B = Handle<Object>::Cast(m1Data->Get(2));
    }

    if (m1Channels == 2) {
        m1A = Handle<Object>::Cast(m1Data->Get(1));
    } else if (m1Channels == 4) {
        m1A = Handle<Object>::Cast(m1Data->Get(3));
    }

    if (m2Channels == 1 || m2Channels == 2) {
        m2K = Handle<Object>::Cast(m2Data->Get(0));
    } else if (m2Channels == 3 || m2Channels == 4) {
        m2R = Handle<Object>::Cast(m2Data->Get(0));
        m2G = Handle<Object>::Cast(m2Data->Get(1));
        m2B = Handle<Object>::Cast(m2Data->Get(2));
    }

    if (m2Channels == 2) {
        m2A = Handle<Object>::Cast(m2Data->Get(1));
    } else if (m2Channels == 4) {
        m2A = Handle<Object>::Cast(m2Data->Get(3));
    }

    // unwrap channel buffers
    size_t m1KL, m1RL, m1GL, m1BL, m1AL;
    char *m1KD, *m1RD, *m1GD, *m1BD, *m1AD;
    float *m1KDi, *m1RDi, *m1GDi, *m1BDi, *m1ADi;

    m1KL = m1RL = m1GL = m1BL = m1AL = 0;
    m1KDi = m1RDi = m1GDi = m1BDi = m1ADi = 0;

    size_t m2KL, m2RL, m2GL, m2BL, m2AL;
    char *m2KD, *m2RD, *m2GD, *m2BD, *m2AD;
    float *m2KDi, *m2RDi, *m2GDi, *m2BDi, *m2ADi;

    m2KL = m2RL = m2GL = m2BL = m2AL = 0;
    m2KDi = m2RDi = m2GDi = m2BDi = m2ADi = 0;

    if (m2Channels == 1 || m2Channels == 2) {
        m1KL = node::Buffer::Length(m1K) * sizeof(float);
        m1KD = node::Buffer::Data(m1K);
        m1KDi = (float *) malloc(m1KL);
        memcpy(m1KDi, &m1KD[0], m1KL);

        m2KL = node::Buffer::Length(m2K) * sizeof(float);
        m2KD = node::Buffer::Data(m2K);
        m2KDi = (float *) malloc(m2KL);
        memcpy(m2KDi, &m2KD[0], m2KL);
    }

    if (m2Channels == 3 || m2Channels == 4) {
        m1RL = node::Buffer::Length(m1R) * sizeof(float);
        m1RD = node::Buffer::Data(m1R);
        m1RDi = (float *) malloc(m1RL);
        memcpy(m1RDi, &m1RD[0], m1RL);

        m1GL = node::Buffer::Length(m1G) * sizeof(float);
        m1GD = node::Buffer::Data(m1G);
        m1GDi = (float *) malloc(m1GL);
        memcpy(m1GDi, &m1GD[0], m1GL);

        m1BL = node::Buffer::Length(m1B) * sizeof(float);
        m1BD = node::Buffer::Data(m1B);
        m1BDi = (float *) malloc(m1BL);
        memcpy(m1BDi, &m1BD[0], m1BL);

        m2RL = node::Buffer::Length(m2R) * sizeof(float);
        m2RD = node::Buffer::Data(m2R);
        m2RDi = (float *) malloc(m2RL);
        memcpy(m2RDi, &m2RD[0], m2RL);

        m2GL = node::Buffer::Length(m2G) * sizeof(float);
        m2GD = node::Buffer::Data(m2G);
        m2GDi = (float *) malloc(m2GL);
        memcpy(m2GDi, &m2GD[0], m2GL);

        m2BL = node::Buffer::Length(m2B) * sizeof(float);
        m2BD = node::Buffer::Data(m2B);
        m2BDi = (float *) malloc(m2BL);
        memcpy(m2BDi, &m2BD[0], m2BL);
    }

    if (m1Channels == 2 || m1Channels == 4) {
        m1AL = node::Buffer::Length(m1A) * sizeof(float);
        m1AD = (char*) node::Buffer::Data(m1A);
        m1ADi = (float *) malloc(m1AL);
        memcpy(m1ADi, &m1AD[0], m1AL);
    }

    if (m2Channels == 2 || m2Channels == 4) {
        m2AL = node::Buffer::Length(m2A) * sizeof(float);
        m2AD = (char*) node::Buffer::Data(m2A);
        m2ADi = (float *) malloc(m2AL);
        memcpy(m2ADi, &m2AD[0], m2AL);
    }

    // validate channel buffer lengths
    if (m1Channels == 2 && m1KL != m1AL) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'imgMatrix.data'")));
    } else if (m1Channels == 3 && (m1RL != m1GL || m1RL != m1BL)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'imgMatrix.data'")));
    } else if (m1Channels == 4 && (m1RL != m1GL || m1RL != m1BL || m1RL != m1AL)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'imgMatrix.data'")));
    }

    if (m2Channels == 2 && m2KL != m2AL) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'tplMatrix.data'")));
    } else if (m2Channels == 3 && (m2RL != m2GL || m2RL != m2BL)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'tplMatrix.data'")));
    } else if (m2Channels == 4 && (m2RL != m2GL || m2RL != m2BL || m2RL != m2AL)) {
        return ThrowException(Exception::TypeError(String::New("Bad argument 'tplMatrix.data'")));
    }

    Cargo *m1 = new Cargo;
    m1->rows = m1Rows;
    m1->cols = m1Cols;
    m1->channels = m1Channels;
    m1->k = m1KDi;
    m1->r = m1RDi;
    m1->g = m1GDi;
    m1->b = m1BDi;
    m1->a = m1ADi;

    Cargo *m2 = new Cargo;
    m2->rows = m2Rows;
    m2->cols = m2Cols;
    m2->channels = m2Channels;
    m2->k = m2KDi;
    m2->r = m2RDi;
    m2->g = m2GDi;
    m2->b = m2BDi;
    m2->a = m2ADi;

    AsyncBaton *baton = new AsyncBaton;
    baton->request.data = baton;
    baton->callback = callback;
    baton->m1 = m1;
    baton->m2 = m2;
    baton->colorTolerance = colorTolerance;
    baton->pixelTolerance = pixelTolerance;

    uv_queue_work(uv_default_loop(), &baton->request, searchDo, (uv_after_work_cb) searchAfter);

    return Undefined();
}

void searchDo(uv_work_t *request) {
    AsyncBaton *baton = static_cast<AsyncBaton*>(request->data);
    Cargo *m1D = static_cast<Cargo*>(baton->m1);
    Cargo *m2D = static_cast<Cargo*>(baton->m2);

    Matrix m1 = {
        m1D->rows,
        m1D->cols,
        m1D->channels,
        MatrixChannel(m1D->k, m1D->rows, m1D->cols),
        MatrixChannel(m1D->r, m1D->rows, m1D->cols),
        MatrixChannel(m1D->g, m1D->rows, m1D->cols),
        MatrixChannel(m1D->b, m1D->rows, m1D->cols),
        MatrixChannel(m1D->a, m1D->rows, m1D->cols)
    };

    Matrix m2 = {
        m2D->rows,
        m2D->cols,
        m2D->channels,
        MatrixChannel(m2D->k, m2D->rows, m2D->cols),
        MatrixChannel(m2D->r, m2D->rows, m2D->cols),
        MatrixChannel(m2D->g, m2D->rows, m2D->cols),
        MatrixChannel(m2D->b, m2D->rows, m2D->cols),
        MatrixChannel(m2D->a, m2D->rows, m2D->cols)
    };

    std::vector<Match> result = search(m1, m2, baton->colorTolerance, baton->pixelTolerance);
    baton->result = result;
}

void searchAfter(uv_work_t *request) {
    AsyncBaton *baton = static_cast<AsyncBaton*>(request->data);

    Local<Array> out = Array::New((int) baton->result.size());
    Local<Object> match;

    Local<String> row = String::New("row");
    Local<String> col = String::New("col");
    Local<String> accuracy = String::New("accuracy");

    int i = 0;
    for (std::vector<Match>::iterator it = baton->result.begin(); it != baton->result.end(); it++) {
        match = Object::New();
        match->Set(row, Number::New(it->row));
        match->Set(col, Number::New(it->col));
        match->Set(accuracy, Number::New(it->accuracy));

        out->Set(i++, match);
    }

    Handle<Value> argv[] = { Null(), out };
    baton->callback->Call(Context::GetCurrent()->Global(), 2, argv);

    delete baton;
    baton = NULL;
}

std::vector<Match> search(Matrix &m1, Matrix &m2, unsigned int colorTolerance, unsigned int pixelTolerance) {
    Eigen::RowVectorXf devK, devR, devG, devB, devA;
    Eigen::RowVectorXf dev = Eigen::RowVectorXf::Zero(m2.cols);

    if (m1.channels < 3) {
        devK = stdDev(m2.k);
        dev += devK;
    } else {
        devR = stdDev(m2.r);
        devG = stdDev(m2.g);
        devB = stdDev(m2.b);
        dev += devR + devG + devB;
    }

    Eigen::RowVectorXf::Index maxCol;
    dev.maxCoeff(&maxCol);
    const unsigned int dx = (const unsigned int) maxCol;

    float* dataM1;
    float* dataM2;

    if (m1.channels < 3) {
        dataM1 = &m1.k(0);
        dataM2 = &m2.k(0);
    } else {
        if (devR.sum() > devG.sum()) {
            dataM1 = &m1.r(0);
            dataM2 = &m2.r(0);
        } else if (devG.sum() > devB.sum()) {
            dataM1 = &m1.g(0);
            dataM2 = &m2.g(0);
        } else {
            dataM1 = &m1.b(0);
            dataM2 = &m2.b(0);
        }
    }

    MatrixChannel stubM1(dataM1, m1.rows, m1.cols);
    MatrixChannel stubM2(dataM2, m2.rows, m2.cols);

    Eigen::VectorXf stub = stubM2.block(0, dx, m2.rows, 1);
    Eigen::ArrayXf stubDiff;
    Eigen::ArrayXXf matDiff;

    unsigned int r = 0;
    unsigned int c = dx;
    const unsigned int mr = m1.rows - m2.rows;
    const unsigned int mc = m1.cols - m2.cols + c;

    // TODO: adjust point colot tolerance according to alpha channel
    // unsigned int pointColorTolerance = 0;
    unsigned int pixelMiss = 0;
    float accuracy = 0;

    std::vector<Match> out;
    do {
        do {
            stubDiff = (stubM1.block(r, c, m2.rows, 1) - stub).array().abs();
            pixelMiss = (unsigned int) (stubDiff > (float) colorTolerance).count();
            if (pixelMiss > pixelTolerance) continue;

            if (m1.channels < 3) {
                matDiff  = (m1.k.block(r, c - dx, m2.rows, m2.cols) - m2.k).array().abs();
            } else {
                matDiff  = (m1.r.block(r, c - dx, m2.rows, m2.cols) - m2.r).array().abs();
                matDiff += (m1.g.block(r, c - dx, m2.rows, m2.cols) - m2.g).array().abs();
                matDiff += (m1.b.block(r, c - dx, m2.rows, m2.cols) - m2.b).array().abs();
            }

            pixelMiss = (unsigned int) (matDiff > (float) colorTolerance).count();

            if (pixelMiss <= pixelTolerance) {
                accuracy = matDiff.maxCoeff();
                accuracy = (accuracy > 0) ? (matDiff / accuracy).sum() : 0;

                Match res = {
                    r,
                    (c - dx),
                    accuracy
                };
                out.push_back(res);
            }
        } while (++c <= mc);
        c = dx;
    } while (++r <= mr);

    return out;
}

Eigen::RowVectorXf stdDev(MatrixChannel &m) {
    const unsigned int N = (unsigned int) m.rows();
    return ((m.rowwise() - (m.colwise().sum() / (float) N)).array().square().colwise().sum() / (float) N).array().sqrt();
}

void Init(Handle<Object> exports) {
    exports->Set(String::NewSymbol("search"), FunctionTemplate::New(Search)->GetFunction());
}

NODE_MODULE(search, Init)


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

// Copyright (c) 2008-2012 Barend Gehrels, 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_STRATEGY_PROJECT_INVERSE_TRANSFORMER_HPP
#define BOOST_GEOMETRY_STRATEGY_PROJECT_INVERSE_TRANSFORMER_HPP

#include <boost/shared_ptr.hpp>

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/extensions/gis/projections/factory.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>

namespace boost { namespace geometry { namespace projections
{

/*!
    \brief Transformation strategy to do transform using a Map Projection
    \ingroup transform
    \tparam Cartesian first point type
    \tparam LatLong second point type
 */
template <typename Cartesian, typename LatLong>
struct project_inverse_transformer
{
    typedef boost::shared_ptr<projection<LatLong, Cartesian> > projection_ptr;

    projection_ptr m_prj;

    /// Constructor using a shared-pointer-to-projection_ptr
    inline project_inverse_transformer(projection_ptr& prj)
        : m_prj(prj)
    {}

    /// Constructor using a string
    inline project_inverse_transformer(std::string const& par)
    {
        factory<LatLong, Cartesian, parameters> fac;
        m_prj.reset(fac.create_new(init(par)));
    }

    /// Constructor using Parameters
    template <typename Parameters>
    inline project_inverse_transformer(Parameters const& par)
    {
        factory<LatLong, Cartesian, Parameters> fac;
        m_prj.reset(fac.create_new(par));
    }

    /// Transform operator
    inline bool apply(Cartesian const& p1, LatLong& p2) const
    {
        // Latlong (LL -> XY) will be projected, rest will be copied.
        // So first copy third or higher dimensions
        geometry::detail::conversion::point_to_point<Cartesian, LatLong, 2,
                geometry::dimension<Cartesian>::value> ::apply(p1, p2);
        return m_prj->inverse(p1, p2);
    }

};

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_STRATEGY_PROJECT_INVERSE_TRANSFORMER_HPP