/*  Copyright (C) 2017 NEC Laboratories America, Inc. ("NECLA"). All rights reserved.
 *
 * This source code is licensed under the license found in the LICENSE file in
 * the root directory of this source tree. An additional grant of patent rights
 * can be found in the PATENTS file in the same directory.
 */
/** \file
 * For every example in a test set, use Multi-Class projections
 * to determine {possible class labels}.
 */

#include "parameter-args.h"
#include "mclinearClass.h"
#include "linearModel.h"
#include "mcxydata.h"
#include "mcfilter.h"
#include <boost/program_options.hpp>
#include <iostream>

using namespace std;
namespace po=boost::program_options;
using namespace po;

#define PRINT_PERF(PERF) do {out<<#PERF << ": ";  for (int i = 0; i < perfs.size(); i++) {out << perfs[i].PERF << " ";} out<<endl;} while(0)
void outputPerfs(ostream& out, string const& modelfile, string const& datafile, string const& filterfile, vector<int> const& nProj, vector<PerfStruct> const& perfs)
{
  out << "---------------------------------" << endl;
  out << "Model: " << modelfile << endl;
  out << "Data: " << datafile << endl;
  out << "Filter: "<< filterfile << endl;

  assert (nProj.size() == perfs.size());

  out << "nProj: ";
  for (int i = 0; i < nProj.size(); i++)
    {
      out << nProj[i] << " ";
    }
  out << endl;

  PRINT_PERF(prcFeasible);
  PRINT_PERF(Prec1);
  PRINT_PERF(Prec5);
  PRINT_PERF(Prec10);
  PRINT_PERF(Top1);
  PRINT_PERF(Top5);
  PRINT_PERF(Top10);
  PRINT_PERF(MacroPrecision);
  PRINT_PERF(MacroRecall);
  PRINT_PERF(MacroF1);
  PRINT_PERF(MacroF1_2);
  PRINT_PERF(MicroPrecision);
  PRINT_PERF(MicroRecall);
  PRINT_PERF(MicroF1);
  out << "---------------------------------" << endl;
}
#undef PRINT_PERF

int main(int argc, char**argv){

  po::variables_map vm;

  vector<PerfStruct> perfs;
  PerfStruct nofilter_perfs;
  bool nofilter_ok = false;
  int verbose = 1;

  po::options_description desc;
  try {
    po::options_description general("General Options");

    general.add_options()
      ("verbose,v",value<int>()->default_value(1),"Verbosity level")
#ifdef _OPENMP
      ("nthreads", value<int>()->default_value(0),"Number of threads to use. 0 means all available")
#endif
      ("help,h","This help")
      ;

    desc.add(general).add(MCxyDataArgs::getDesc()).add(MCprojectorArgs::getDesc()).add(MCclassifierArgs::getDesc());
    po::parsed_options parsed
      = po::command_line_parser( argc, argv )
      .options( desc )
      .run();
    po::store( parsed, vm );

    if( vm.count("help") ) {
      cout<<desc<<endl;
      exit(0);
    }

    po::notify(vm); // at this point, raise any exceptions for 'required' args

  }catch(std::exception const& e){
    cerr << "Error: "<<e.what()<<endl<<endl;
    cerr << desc << endl;
    exit(-1);
  }catch(...){
    cerr<<"Command-line parsing exception of unknown type!"<<endl;
    cerr << desc << endl;
    exit(-2);
  }

  if (vm.count("verbose"))
    {
      verbose = vm["verbose"].as<int>();
    }

#ifdef _OPENMP
  if (vm.count("nthreads"))
    {
      int nthreads = vm["nthreads"].as<int>();
      if (nthreads > 0)
	{
	  omp_set_num_threads(nthreads);
	}
    }
#endif

  MCxyDataArgs dataargs(vm);
  MCprojectorArgs filterargs(vm);
  MCclassifierArgs modelargs(vm);

  // get the stats for data normalization
  bool center = 0;
  std::string normfile;

  if (dataargs.normData.size() > 0)
    {
      normfile = dataargs.normData;
    }
  else
    {
      normfile = dataargs.xFiles[0]; // use the first file to calculate the stats for data normalization and removing rare features
    }

  Eigen::VectorXd mean;
  Eigen::VectorXd sdev;
  vector<size_t> feature_map;
  vector<size_t> reverse_feature_map;
  vector<size_t> label_map;
  vector<size_t> reverse_label_map;

  { // get statistics for normalization and for removal of rare features.
    MCxyData data;

    if (dataargs.rmRareF > 0 || dataargs.rmRareL > 0 || dataargs.xnorm )
      {
	data.read(normfile);
      }
    if (dataargs.rmRareF > 0)
      {
	data.removeRareFeatures(feature_map, reverse_feature_map, dataargs.rmRareF, false);
      }

    if (dataargs.rmRareL > 0)
      {
	data.removeRareLabels(label_map, reverse_label_map, dataargs.rmRareL, false);
      }

    if (dataargs.xnorm)
      {
	switch (dataargs.center)
	  {
	  case -1:
	    center = data.denseOk;
	    break;
	  case 0:
	  center = false;
	  break;
	  case 1:
	    center = true;
	    break;
	  default :
	    throw runtime_error("Unrecongnized argument center = "+to_string(dataargs.center));
	  }
	data.xstdnormal(mean, sdev, true, center, false);
      }
  }

  for (vector<string>::iterator mit = modelargs.modelFiles.begin(); mit != modelargs.modelFiles.end(); ++mit)
    {
      // prepare the classifier

      shared_ptr<linearModel> model = make_shared<linearModel>();
      model->read(*mit);
      // done with changing the model. Make it const
      shared_ptr<const linearModel> const_model = static_pointer_cast<const linearModel>(model);
      model = nullptr;  // only use the const version from now on to make sure the model does not change.

      for (int dta = 0; dta < dataargs.xFiles.size(); ++dta)
	{
	  // prepare the data
	  string xfile = dataargs.xFiles[dta];
	  string yfile;
	  if (dataargs.yFiles.size() == 0)
	    {
	      // no yfile
	      yfile = "";
	    }
	  else if (dataargs.yFiles.size() == 1)
	    {
	      // only one label file for all datasets
	      yfile = dataargs.yFiles[0];
	    }
	  else if (dataargs.yFiles.size() == dataargs.yFiles.size())
	    {
	      yfile = dataargs.yFiles[dta];
	    }
	  else
	    {
	      throw runtime_error("xFiles and yFiles do not match");
	    }
	  shared_ptr<MCxyData> data = make_shared<MCxyData>(verbose);
	  data->read(xfile, yfile);
	  if (dataargs.rmRareF > 0)
	    {
	      data->removeRareFeatures(feature_map, reverse_feature_map, dataargs.rmRareF, true);
	    }
	  if (dataargs.rmRareL > 0)
	    {
	      data->removeRareLabels(label_map, reverse_label_map, dataargs.rmRareL, true);
	    }

	  if (dataargs.xnorm)
	    {
	      data->xstdnormal(mean, sdev, true, center, true);
	    }
	  if (dataargs.xunit)
	    {
	      data->xunitnormal();
	    }
	  data->xscale(dataargs.xscale);

	  // done with modifying the data. Make it const.
	  shared_ptr<const MCxyData> const_data = static_pointer_cast<const MCxyData>(data);
	  data = nullptr;  // only use the const version from now on to make sure the data does not change.

	  if (filterargs.lfFiles.size() == 0)
	    {
	      filterargs.lfFiles.push_back("");
	    }
	  for (int l=0; l<filterargs.lfFiles.size(); l++)
	    {
	      // prepare filter.
	      shared_ptr<MCfilter> lf = make_shared<MCfilter>();
	      if (filterargs.lfFiles[l].size())
		{
		  lf->read(filterargs.lfFiles[l]);
		}

	      // replace -2 and -1 nProj arguments with the correct values.
	      std::vector<int> nproj;

	      if (std::find(filterargs.nProj.begin(), filterargs.nProj.end(), -2) != filterargs.nProj.end())
		{
		  nproj.clear();
		  for (int i = 0; i <= lf->nFilters(); i++)
		    {
		      nproj.push_back(i);
		    }
		}
	      else
		{
		  for (vector<int>::iterator npit = filterargs.nProj.begin(); npit != filterargs.nProj.end();++npit)
		    if ( *npit == -1 )
		      {
			nproj.push_back(lf->nFilters());
		      }
		    else
		      {
			nproj.push_back(*npit);
		      }
		}
	      int nlogtime = filterargs.nlogtime;
	      if (nlogtime < 0) nlogtime = *std::max_element(nproj.begin(),nproj.end());
	      if (nlogtime > 0)
		{
		  lf->init_logtime(nlogtime);
		}

	      //done with modifying the label filter. Make it const
	      shared_ptr<const MCfilter> const_lf = static_pointer_cast<const MCfilter>(lf);
	      // we have the model, data and filter. We can predict/evaluate models using a linear classifier
	      MClinearClassifier classifier(const_data, const_model, const_lf, verbose);
	      // set keep_top, keep_thresh
	      classifier.setPruneParams(modelargs.keep_thresh, modelargs.keep_top);

	      // set the nubmer of filters to apply
	      for (vector<int>::iterator npit=nproj.begin(); npit != nproj.end(); ++npit)
		{
		  int np = *npit;
		  if (np == 0 && nofilter_ok)
		    {
		      //the results for 0 projections or no filters are the same regardelss of the label filter used.
		      // So used saved results if available and save them if not
		      perfs.push_back(nofilter_perfs);
		    }
		  else
		    {

		      if (np > const_lf->nFilters())
			{
			  throw runtime_error(to_string(np) + " filters requested, but only " + to_string(const_lf->nFilters()) + " exist");
			}

		      classifier.nProj(np);
		      classifier.predict();
		      perfs.push_back(classifier.evaluate(modelargs.threshold, modelargs.min_labels));
		      if (np == 0)
			{
			  //the results for 0 projections or no filters are the same regardelss of the label filter used.
			  // And they are expensive co calculate. So save reslults for late use
			  nofilter_perfs = perfs.back();
			  nofilter_ok = true;
			}
		    }
		}

	      // Done. Output the performances
	      outputPerfs(cout, *mit, dataargs.xFiles[dta], filterargs.lfFiles[l], nproj, perfs);
	      perfs.clear();
	    }
	  nofilter_ok = false; // data or model has changed, so result with no filter are not valid any more
	}
    }
}


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

#include <string>
#include <iostream>
#include <iomanip>
#include <vector>
#include <algorithm>

#ifdef _MSC_VER
// disable stupid diamond-inheritance warnings (the compiler does not see that there is only a single implementation for each of the functions)
#pragma warning(disable: 4250)
#endif

#include <Util/Test.hpp>
#include <Util/AlignedBlock.hpp>
#include <Util/TestInfo.hpp>
#include <Util/ErrorInfo.hpp>
#include <Util/VFunc.hpp>
#include <Util/Euclidean.hpp>
#include <Util/Intrinsics.hpp>
#include <Util/ComputeNumRuns.hpp>
#include <Util/ExpandTest.hpp>
#include <Util/TestCase.hpp>
#include <Util/Output.hpp>

#include <Copy/CopyTest.hpp>

#include <XOR/XOR_scalar.hpp>
#include <XOR/XOR_simd.hpp>

#include <AN/AN_scalar.hpp>
#include <AN/AN_simd.hpp>

#include <Hamming/Hamming_scalar.hpp>
#include <Hamming/Hamming_simd.hpp>

#ifdef TESTCRC
#include <CRC/CRC_scalar.hpp>
#endif

using namespace coding_benchmark;

#ifdef TEST32
#include <boost/multiprecision/cpp_int.hpp>

using uint128_t = boost::multiprecision::uint128_t;
#endif

int checkArgs(
        int argc,
        char* argv[],
        size_t & AUser1) {
    if (argc == 1) {
        return 0;
    }
    if (argc != 2) {
        printUsage(argv);
        return 1;
    }
    char* endPtr = nullptr;
    AUser1 = strtoull(argv[1], &endPtr, 0);
    if (endPtr == argv[1] || AUser1 == 0 || (AUser1 & 1) == 0) {
        std::cerr << "Error: A is not a valid positive, non-zero, odd integer!\n";
        printUsage(argv);
        return 2;
    }
#if defined(TEST8)
    if (AUser1 >= (1ull << 8)) {
        std::cerr << "Error: Given A is too large! Must be at most 2^8-1\n";
        printUsage(argv);
        return 3;
    }
#elif defined(TEST16)
    if (AUser1 >= (1ull << 16)) {
        std::cerr << "Error: Given A is too large! Must be at most 2^16-1\n";
        printUsage(argv);
        return 3;
    }
#elif defined(TEST32)
    if (AUser1 >= (1ull << 32)) {
        std::cerr << "Error: Given A is too large! Must be at most 2^32-1\n";
        printUsage(argv);
        return 3;
    }
#else
#error "You must define (-D) exactly one of TEST8, TEST16, or TEST32"
#endif
    return 0;
}

int main(
        int argc,
        char* argv[]) {
    const constexpr size_t numElements = 1000001ull;
    const constexpr size_t iterations = 10000ull;
    const constexpr size_t UNROLL_LO = 1ull;
    const constexpr size_t UNROLL_HI = 1024ull;

    std::cout << "# numElements = " << numElements << '\n';
    std::cout << "# iterations = " << iterations << '\n';
    std::cout << "# UNROLL_LO = " << UNROLL_LO << '\n';
    std::cout << "# UNROLL_HI = " << UNROLL_HI << '\n';

    size_t AUser = 64311ull;
    int result = checkArgs(argc, argv, AUser);
    if (result != 0) {
        return result;
    }
    _ReadWriteBarrier();

    std::vector<std::vector<TestInfos>> vecTestInfos;

    TestConfiguration testConfig(iterations, numElements);
    // The following is for the reproducibility of the SIGMOD 2018 "AHEAD" paper, to reduce the amount of tests
    testConfig.disableAll();
    testConfig.enableCheck = true;
    testConfig.enableDecode = true;

#ifdef TEST8
    {
        uint16_t AUserInv = static_cast<uint16_t>(ext_euclidean<uint32_t>(static_cast<uint32_t>(AUser), 16));
        std::cout << "# A=" << AUser << " A^-1=" << AUserInv << std::endl;

        DataGenerationConfiguration dataGenConfig8(5, 3, 2, 0); /* TODO we do not check for overflows or underflows ANYWHERE, yet :-(*/
        AlignedBlock bufRawdata8(numElements * sizeof(uint8_t), 64);
        AlignedBlock bufEncoded8(3 * numElements * sizeof(uint8_t), 64); // Coding may generate thrice (since CRC) as much encoded output data as raw input data
        AlignedBlock bufResult8(3 * numElements * sizeof(uint8_t), 64);// Coding may generate thrice (since CRC) as much encoded result data as raw input data (or the same amount as encoded data)

        WarmUp<CopyTest8, UNROLL_LO, UNROLL_HI>("CopyTest8", "Copy", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8);

        std::clog << "# 8-bit Baseline (memcpy / memcmp) test:" << std::endl;
        TestCase<CopyTest8, UNROLL_LO, UNROLL_HI>("CopyTest8", "Copy", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos);
        size_t refIdx = vecTestInfos.size() - 1;

        // 8-bit data sequential tests
        std::clog << "# 8-bit scalar tests:" << std::endl;
        TestCase<XOR_scalar_8_8, UNROLL_LO, UNROLL_HI>("XOR_scalar_8_8", "XOR Scalar", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_scalar_8_16_u_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_8_16_u_divmod", "AN Scalar U DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_scalar_8_16_s_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_8_16_s_divmod", "AN Scalar S DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_scalar_8_16_u_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_8_16_u_inv", "AN Scalar U Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_scalar_8_16_s_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_8_16_s_inv", "AN Scalar S Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<Hamming_scalar_8, UNROLL_LO, UNROLL_HI>("Hamming_scalar_8", "Hamming Scalar", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
#ifdef TESTCRC
        TestCase<CRC16_scalar_8, UNROLL_LO, UNROLL_HI>("CRC16_scalar_8", "CRC16 Scalar", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
#endif

#ifdef __SSE4_2__
        // 8-bit data vectorized tests
        std::clog << "# 8-bit SSE4.2 tests:" << std::endl;
        TestCase<XOR_sse42_8x8_8x8, UNROLL_LO, UNROLL_HI>("XOR_sse42_16x8_16x8", "XOR SSE4.2", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_sse42_8_16_u_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_8_16_u_divmod", "AN SSE4.2 U DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_sse42_8_16_s_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_8_16_s_divmod", "AN SSE4.2 S DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_sse42_8_16_u_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_8_16_u_inv", "AN SSE4.2 U Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_sse42_8_16_s_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_8_16_s_inv", "AN SSE4.2 S Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<Hamming_sse42_8, UNROLL_LO, UNROLL_HI>("Hamming_sse42_8", "Hamming SSE4.2", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
#endif

#ifdef __AVX2__
        std::clog << "# 8-bit AVX2 tests:" << std::endl;
        TestCase<XOR_avx2_8x8_8x8, UNROLL_LO, UNROLL_HI>("XOR_avx2_32x8_32x8", "XOR AVX2", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_avx2_8_16_u_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_8_16_u_divmod", "AN AVX2 U DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_avx2_8_16_s_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_8_16_s_divmod", "AN AVX2 S DivMod", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_avx2_8_16_u_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_8_16_u_inv", "AN AVX2 U Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<AN_avx2_8_16_s_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_8_16_s_inv", "AN AVX2 S Inv", bufRawdata8, bufEncoded8, bufResult8, AUser, AUserInv, testConfig, dataGenConfig8, vecTestInfos, refIdx);
        TestCase<Hamming_avx2_8, UNROLL_LO, UNROLL_HI>("Hamming_avx2_8", "Hamming AVX2", bufRawdata8, bufEncoded8, bufResult8, testConfig, dataGenConfig8, vecTestInfos, refIdx);
#endif
    }
#endif /* TEST8 */

#ifdef TEST16
    {
        uint32_t AUserInv = static_cast<uint32_t>(ext_euclidean<uint64_t>(AUser, 32));
        std::cout << "# A=" << AUser << " A^-1=" << AUserInv << std::endl;

        DataGenerationConfiguration dataGenConfig16(11, 5, 4, 0); /* TODO we do not check for overflows or underflows ANYWHERE, yet :-(*/
        AlignedBlock bufRawdata16(numElements * sizeof(uint16_t), 64);
        AlignedBlock bufEncoded16(3 * numElements * sizeof(uint16_t), 64); // Coding may generate thrice (since CRC) as much encoded output data as raw input data
        AlignedBlock bufResult16(3 * numElements * sizeof(uint16_t), 64);// Coding may generate thrice (since CRC) as much encoded result data as raw input data (or the same amount as encoded data)

        WarmUp<CopyTest16, UNROLL_LO, UNROLL_HI>("CopyTest16", "Copy", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16);

        std::clog << "# 16-bit Baseline (memcpy / memcmp) test:" << std::endl;
        TestCase<CopyTest16, UNROLL_LO, UNROLL_HI>("CopyTest16", "Copy", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos);
        size_t refIdx = vecTestInfos.size() - 1;

        // 16-bit data sequential tests
        std::clog << "# 16-bit scalar tests:" << std::endl;
        TestCase<XOR_scalar_16_16, UNROLL_LO, UNROLL_HI>("XOR_scalar_16_16", "XOR Scalar", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_scalar_16_32_u_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_16_32_u_divmod", "AN Scalar U DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_scalar_16_32_s_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_16_32_s_divmod", "AN Scalar S DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_scalar_16_32_u_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_16_32_u_inv", "AN Scalar U Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_scalar_16_32_s_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_16_32_s_inv", "AN Scalar S Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<Hamming_scalar_16, UNROLL_LO, UNROLL_HI>("Hamming_scalar_16", "Hamming Scalar", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
#ifdef TESTCRC
        TestCase<CRC32_scalar_16, UNROLL_LO, UNROLL_HI>("CRC32_scalar_16", "CRC32 Scalar", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
#endif

#ifdef __SSE4_2__
        // 16-bit data vectorized tests
        std::clog << "# 16-bit SSE4.2 tests:" << std::endl;
        TestCase<XOR_sse42_8x16_8x16, UNROLL_LO, UNROLL_HI>("XOR_sse42_8x16_8x16", "XOR SSE4.2", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_sse42_16_32_u_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_16_32_u_divmod", "AN SSE4.2 U DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_sse42_16_32_s_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_16_32_s_divmod", "AN SSE4.2 S DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_sse42_16_32_u_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_16_32_u_inv", "AN SSE4.2 U Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_sse42_16_32_s_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_16_32_s_inv", "AN SSE4.2 S Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<Hamming_sse42_16, UNROLL_LO, UNROLL_HI>("Hamming_sse42_16", "Hamming SSE4.2", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
#endif

#ifdef __AVX2__
        std::clog << "# 16-bit AVX2 tests:" << std::endl;
        TestCase<XOR_avx2_16x16_16x16, UNROLL_LO, UNROLL_HI>("XOR_avx2_16x16_16x16", "XOR AVX2", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_avx2_16_32_u_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_16_32_u_divmod", "AN AVX2 U DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_avx2_16_32_s_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_16_32_s_divmod", "AN AVX2 S DivMod", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_avx2_16_32_u_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_16_32_u_inv", "AN AVX2 U Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<AN_avx2_16_32_s_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_16_32_s_inv", "AN AVX2 S Inv", bufRawdata16, bufEncoded16, bufResult16, AUser, AUserInv, testConfig, dataGenConfig16, vecTestInfos, refIdx);
        TestCase<Hamming_avx2_16, UNROLL_LO, UNROLL_HI>("Hamming_avx2_16", "Hamming AVX2", bufRawdata16, bufEncoded16, bufResult16, testConfig, dataGenConfig16, vecTestInfos, refIdx);
#endif
    }
#endif /* TEST16 */

#ifdef TEST32
    {
        uint64_t AUserInv = static_cast<uint64_t>(ext_euclidean<uint128_t>(static_cast<uint128_t>(AUser), 64));
        std::cout << "# A=" << AUser << " A^-1=" << AUserInv << std::endl;

        DataGenerationConfiguration dataGenConfig32(26, 6, 5, 0); /* TODO we do not check for overflows or underflows ANYWHERE, yet :-(*/
        AlignedBlock bufRawdata32(numElements * sizeof(uint32_t), 64);
        AlignedBlock bufEncoded32(3 * numElements * sizeof(uint32_t), 64); // Coding may generate twice as much encoded output data as raw input data
        AlignedBlock bufResult32(3 * numElements * sizeof(uint32_t), 64);// Coding may generate twice as much encoded result data as raw input data (or the same amount as encoded data)

        WarmUp<CopyTest32, UNROLL_LO, UNROLL_HI>("CopyTest32", "Copy", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32);

        std::clog << "# 32-bit Baseline (memcpy / memcmp) test:" << std::endl;
        TestCase<CopyTest32, UNROLL_LO, UNROLL_HI>("CopyTest32", "Copy", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos);
        size_t refIdx = vecTestInfos.size() - 1;

        std::clog << "# 32-bit Scalar tests:" << std::endl;
        // 32-bit data sequential tests
        TestCase<XOR_scalar_32_32, UNROLL_LO, UNROLL_HI>("XOR_scalar_32_32", "XOR Scalar", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_scalar_32_64_u_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_32_64_u_divmod", "AN Scalar U DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_scalar_32_64_s_divmod, UNROLL_LO, UNROLL_HI>("AN_scalar_32_64_s_divmod", "AN Scalar S DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_scalar_32_64_u_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_32_64_u_inv", "AN Scalar U Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_scalar_32_64_s_inv, UNROLL_LO, UNROLL_HI>("AN_scalar_32_64_s_inv", "AN Scalar S Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<Hamming_scalar_32, UNROLL_LO, UNROLL_HI>("Hamming_scalar_32", "Hamming Scalar", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
#ifdef TESTCRC
        TestCase<CRC32_scalar_32, UNROLL_LO, UNROLL_HI>("CRC32_scalar_32", "CRC32 Scalar", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
#endif

#ifdef __SSE4_2__
        std::clog << "# 32-bit SSE4.2 tests:" << std::endl;
        // 32-bit data vectorized tests
        TestCase<XOR_sse42_4x32_4x32, UNROLL_LO, UNROLL_HI>("XOR_sse42_4x32_4x32", "XOR SSE4.2", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_sse42_32_64_u_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_32_64_u_divmod", "AN SSE4.2 U DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_sse42_32_64_s_divmod, UNROLL_LO, UNROLL_HI>("AN_sse42_32_64_s_divmod", "AN SSE4.2 S DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_sse42_32_64_u_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_32_64_u_inv", "AN SSE4.2 U Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_sse42_32_64_s_inv, UNROLL_LO, UNROLL_HI>("AN_sse42_32_64_s_inv", "AN SSE4.2 S Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<Hamming_sse42_32, UNROLL_LO, UNROLL_HI>("Hamming_sse42_32", "Hamming SSE4.2", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
#endif

#ifdef __AVX2__
        std::clog << "# 32-bit AVX2 tests:" << std::endl;
        TestCase<XOR_avx2_8x32_8x32, UNROLL_LO, UNROLL_HI>("XOR_avx2_8x32_8x32", "XOR AVX2", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_avx2_32_64_u_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_32_64_u_divmod", "AN AVX2 U DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_avx2_32_64_s_divmod, UNROLL_LO, UNROLL_HI>("AN_avx2_32_64_s_divmod", "AN AVX2 S DivMod", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_avx2_32_64_u_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_32_64_u_inv", "AN AVX2 U Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<AN_avx2_32_64_s_inv, UNROLL_LO, UNROLL_HI>("AN_avx2_32_64_s_inv", "AN AVX2 S Inv", bufRawdata32, bufEncoded32, bufResult32, AUser, AUserInv, testConfig, dataGenConfig32, vecTestInfos, refIdx);
        TestCase<Hamming_avx2_32, UNROLL_LO, UNROLL_HI>("Hamming_avx2_32", "Hamming AVX2", bufRawdata32, bufEncoded32, bufResult32, testConfig, dataGenConfig32, vecTestInfos, refIdx);
#endif
    }
#endif /* TEST32 */

    printResults<false>(testConfig, vecTestInfos);
    std::cout << "\n\n";
    printResults<true>(testConfig, vecTestInfos);
}


#include "pymccp.h"
#include <iostream>
#include <boost/cstdint.hpp>

using namespace std;

int * pymcc_classify(double *x, double *y, double *z, int32_t n, double scaleDomain2Spacing, double curvatureThreshold) {

  uint8_t *classification = mcc_classify(x, y, z, n, scaleDomain2Spacing, curvatureThreshold);
  int *intClassification = new int[n];
  for(int i = 0; i < n; i++) {
	  intClassification[i] = (int)classification[i];
  }

  return intClassification;

}

double * pymcc_pass(double *x, double *y, double *z, int32_t n, double scaleDomainSpacing) {

  return mcc_pass(x, y, z, n, scaleDomainSpacing);

}


// Copyright 2021 Apex.AI, Inc.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_
#define STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_

#include <state_estimation/noise_model/noise_interface.hpp>
#include <state_estimation/visibility_control.hpp>

#include <Eigen/Core>

#include <algorithm>
#include <array>
#include <chrono>
#include <type_traits>
#include <vector>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      This class contains the logic for a uniform noise model.
///
/// @tparam     StateT  State vector type for which this noise model is created.
///
template<typename StateT>
class STATE_ESTIMATION_PUBLIC UniformNoise : public NoiseInterface<UniformNoise<StateT>>
{
  using Scalar = typename StateT::Scalar;
  using Matrix = typename StateT::Matrix;
  template<typename OtherScalarT>
  using VarianceVector = Eigen::Matrix<OtherScalarT, StateT::size(), 1>;

public:
  /// A convenience typedef for the state for which this noise model is to be used.
  using State = StateT;

  ///
  /// @brief      Create the noise model from an existing covariance matrix.
  ///
  /// @param[in]  covariance  The covariance
  ///
  explicit UniformNoise(const Matrix & covariance) noexcept
  : m_covariance{covariance} {}

  ///
  /// @brief      Create the noise model from an array
  ///
  /// @param[in]  variances     The values for variances, i.e., values for σ (not squared)
  ///
  /// @tparam     OtherScalarT  Some scalar type.
  ///
  template<typename OtherScalarT>
  explicit UniformNoise(
    const std::array<OtherScalarT, State::size()> & variances) noexcept
  : m_covariance{
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal()} {}

  ///
  /// @brief      Create the noise model from a vector
  ///
  /// @param[in]  variances     The values for variances, i.e., values for σ (not squared)
  ///
  /// @throws     std::runtime_error  if the vector is of a wrong size.
  ///
  /// @tparam     OtherScalarT  Some scalar type.
  ///
  template<typename OtherScalarT>
  explicit UniformNoise(const std::vector<OtherScalarT> & variances)
  {
    if (variances.size() != static_cast<std::size_t>(State::size())) {
      throw std::runtime_error(
              "There must be " + std::to_string(State::size()) +
              " variances for initializing the uniform noise model, but " +
              std::to_string(variances.size()) + " provided");
    }
    m_covariance =
      Eigen::Map<const VarianceVector<OtherScalarT>>(variances.data())
      .template cast<Scalar>().array().square().matrix().asDiagonal();
  }

  ///
  /// @brief      Create noise model from variances directly.
  ///
  /// @param[in]  variance    The first variance, i.e., value for σ (not squared)
  /// @param[in]  variances   The variances for all the other variables
  ///
  /// @tparam     VarianceTs  Types for other variances.
  ///
  template<typename ... VarianceTs>
  explicit UniformNoise(const Scalar variance, const VarianceTs ... variances)
  : UniformNoise{std::array<Scalar, State::size()> {variance, variances ...}}
  {
    static_assert(
      sizeof...(VarianceTs) + 1 == State::size(),
      "Wrong number of variances passed into the UniformNoise constructor");
  }

protected:
  /// Allow the interface to access the protected and private members to visually encapsulate the
  /// implementation.
  friend NoiseInterface<UniformNoise<StateT>>;

  ///
  /// @brief      A CRTP-called covariance getter.
  ///
  /// @return     A covariance of the noise process over a given time span.
  ///
  Matrix crtp_covariance(const std::chrono::nanoseconds & dt) const noexcept
  {
    return m_covariance * std::chrono::duration_cast<std::chrono::duration<Scalar>>(dt).count();
  }

private:
  /// Store the covariance matrix internally.
  Matrix m_covariance{};
};

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION__NOISE_MODEL__UNIFORM_NOISE_HPP_


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BUNDLE_INDICE_CONTAINER_HPP__
#define __OPTIMISATION2_BUNDLE_INDICE_CONTAINER_HPP__

#include <cassert>
#include <vector>
#include <map>
#include <set>
#include <algorithm>

#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/ttt/traits/typed_indice.hpp>
#include <libv/lma/color/console.hpp>
#include <utility>
#include <boost/fusion/include/pair.hpp>

//! SparseIndiceContainer

/**
 * Le container d'indice SIC<A,B> lie les paramètres A avec les paramètres B :
 * SIC<A,B> contient pour chaque instance de A, la liste des B associés.
 * Le container std::map est utilisé uniquement pour accélérer le remplissage avec add()
 */

template<class A, class B> class SIC
{
  public:
    typedef A first_type;
    typedef B second_type;
    typedef ttt::Indice<A> IndiceA;
    typedef ttt::Indice<B> IndiceB;
    typedef ttt::Indice<boost::fusion::pair<B,A> > SparseAB;

    static std::string name() { return " SIC<" + ttt::name<A>() + "," + ttt::name<B>() + ">" ; }

    SIC(){/*std::cout << name() << std::endl;*/}
    IndiceA first() const { return IndiceA(0); }
    IndiceA size() const { return IndiceA(vaz.size()); }

    SparseAB first(const IndiceA&) const { return SparseAB(0); }
    SparseAB size(const IndiceA& ind) const { assert(ind<vaz.size());return SparseAB(vaz[ind()].size()); }

    const IndiceB& operator()(const IndiceA& inda, const SparseAB& indab) const
    {
      assert(inda<vaz.size());
      assert(indab<vaz[inda()].size());
      return vaz[inda()][indab()];
    }

    void set_max(const IndiceA& max_a, const IndiceB& max_b)
    {
      vazset.resize(max_a());
      maxb = max_b;
    }

    void add(const IndiceA& inda, const IndiceB& indb)
    {
      assert(inda() < int(vazset.size()));
      vazset[inda()].insert(indb);
    }

    SparseAB get(const IndiceA& a, const IndiceB& b)
    {
      assert( size_t(a()) < map.size() );
      assert( map[a()].find(b) != map[a()].end() );
      return map[a()][b];
    }

    void update()
    {
      vaz.resize(vazset.size());
      map.resize(vazset.size());

      for(size_t i = 0 ; i < vaz.size() ; ++i)
        for(auto& s : vazset[i])
        {
          map[i][s] = vaz[i].size();
          vaz[i].push_back(s);
        }
      std::vector<std::set<IndiceB>>().swap(vazset);
    }

    std::ostream& disp(std::ostream& o) const
    {
      o << color.blue() << std::endl << "SIC<" << ttt::name<A>() << "," << ttt::name<B>() << ">" << color.reset() << " :\n";
      if (size()==0) std::cout << " empty " << std::endl;
      for(auto i = first(); i != size() ; ++i)
        for(auto j = first(i); j != size(i) ; ++j)
          o << i << "," << j << "->" << this->operator()(i,j) << ";";
      return o;
    }

    IndiceB maxb;

  private:

    std::vector<std::vector<IndiceB>> vaz;
    std::vector<std::set<IndiceB>> vazset;
    std::vector<std::map<IndiceB,int>> map;
};

/**
 * Le container SIC2 est utilisé uniquement pour l'algo de ExplicitSchur
 * pour accéder aux indices éparses de SIC<A,B>
 */

template<class A, class B> class SIC2
{
  public:
    typedef A first_type;
    typedef B second_type;
    typedef ttt::Indice<A> IndiceA;
    typedef ttt::Indice<B> IndiceB;
    typedef ttt::Indice<boost::fusion::pair<B,A> > SparseAB;
    typedef ttt::Indice<boost::fusion::pair<A,B> > ReverseBA;

    static std::string name() { return " SIC2<" + ttt::name<A>() + "," + ttt::name<B>() + ">" ; }

    SIC2(){/*std::cout << name() << std::endl;*/}

    SparseAB add(const IndiceA& inda, const IndiceB&, const ReverseBA& rba)
    {
      size_t i(inda());
      if (i >= vaz.size())
      {
        vaz.resize(i+1);
        vaz.back().push_back(rba);
        return 0;
      }

      vaz[i].push_back(rba);
      return vaz[i].size() - 1;
    }

    ReverseBA reverse(const IndiceA& inda, const SparseAB& indab) const
    {
      assert(inda<vaz.size());assert(indab<vaz[inda()].size());
      return vaz[inda()][indab()];
    }

  private:
    std::vector<std::vector<ReverseBA>> vaz;
};

namespace ttt
{
  template<class A, class Z> struct Name<SIC<A,Z>> { static std::string name() { return "SIC<" + ttt::name<A>() + "," + ttt::name<Z>() + ">"; } };
}

template<class A, class Z> std::ostream& operator<<(std::ostream& o, const SIC<A,Z>& va)
{
  return va.disp(o);
  o << ttt::name<SIC<A,Z>>() << std::endl; //"SIC<" << ttt::name<A>() << "," << ttt::name<Z>() << ">\n";
  if (va.size()==0) std::cout << " empty " << std::endl;
  for(auto i = va.first() ; i < va.size() ; ++i)
  {
//     if (va.size(i)!=0)
      o << " [ ";
    for(auto j = va.first(i) ; j < va.size(i) ; ++j)
      o << va(i,j) << " ";
//     if (va.size(i)!=0)
      o << "] ";
    o << std::endl;
  }
  return o;
}

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(Deserializer)

struct EqualFixture : public ParserFlatbuffersSerializeFixture
{
    explicit EqualFixture(const std::string & inputShape1,
                          const std::string & inputShape2,
                          const std::string & outputShape,
                          const std::string & dataType)
    {
        m_JsonString = R"(
            {
                inputIds: [0, 1],
                outputIds: [3],
                layers: [
                    {
                        layer_type: "InputLayer",
                        layer: {
                            base: {
                                layerBindingId: 0,
                                base: {
                                    index: 0,
                                    layerName: "InputLayer1",
                                    layerType: "Input",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [{
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + inputShape1 + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                    }],
                                },
                            }
                        },
                    },
                    {
                        layer_type: "InputLayer",
                        layer: {
                            base: {
                                layerBindingId: 1,
                                base: {
                                      index:1,
                                      layerName: "InputLayer2",
                                      layerType: "Input",
                                      inputSlots: [{
                                          index: 0,
                                          connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                      }],
                                      outputSlots: [{
                                          index: 0,
                                          tensorInfo: {
                                              dimensions: )" + inputShape2 + R"(,
                                              dataType: )" + dataType + R"(
                                          },
                                      }],
                                },
                            }
                        },
                    },
                    {
                        layer_type: "EqualLayer",
                        layer: {
                            base: {
                                 index:2,
                                 layerName: "EqualLayer",
                                 layerType: "Equal",
                                 inputSlots: [{
                                     index: 0,
                                     connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                 },
                                 {
                                     index: 1,
                                     connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                                 }],
                                 outputSlots: [{
                                     index: 0,
                                     tensorInfo: {
                                         dimensions: )" + outputShape + R"(,
                                         dataType: Boolean
                                     },
                                 }],
                            }
                        },
                    },
                    {
                        layer_type: "OutputLayer",
                        layer: {
                            base:{
                                layerBindingId: 0,
                                base: {
                                    index: 3,
                                    layerName: "OutputLayer",
                                    layerType: "Output",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:2, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [{
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + outputShape + R"(,
                                            dataType: Boolean
                                        },
                                    }],
                                }
                            }
                        },
                    }
                ]
            }
        )";
        Setup();
    }
};

struct SimpleEqualFixture : EqualFixture
{
    SimpleEqualFixture() : EqualFixture("[ 2, 2, 2, 1 ]",
                                        "[ 2, 2, 2, 1 ]",
                                        "[ 2, 2, 2, 1 ]",
                                        "QuantisedAsymm8") {}
};

BOOST_FIXTURE_TEST_CASE(EqualQuantisedAsymm8, SimpleEqualFixture)
{
  RunTest<4, armnn::DataType::QuantisedAsymm8, armnn::DataType::Boolean>(
      0,
      {{"InputLayer1", { 0, 1, 2, 3, 4, 5, 6, 7 }},
       {"InputLayer2", { 0, 0, 0, 3, 0, 0, 6, 7 }}},
      {{"OutputLayer", { 1, 0, 0, 1, 0, 0, 1, 1 }}});
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#ifndef SFMTAVXEQ_HPP
#define SFMTAVXEQ_HPP
/**
 * @file SFMTAVXeq.hpp
 */
#include "devavxprng.h"
#include "AlgorithmSIMDEquidistribution.hpp"
//#include <MTToolBox/AlgorithmEquidistribution.hpp>
//#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
//#include <MTToolBox/period.hpp>
//#include <NTL/GF2X.h>
#include "Annihilate.hpp"
#include "EQOptions.hpp"

namespace MTToolBox {

    template<typename U, typename G, typename P, int bitWidth>
    int sfmtavx_equidistribution(EQOptions<P> opt)
    {
        using namespace std;
        G sf(opt.params);
        cout << sf.getParamString() << endl;
        U wseed;
        wseed.u64[0] = opt.seed;
        sf.seed(wseed);
        Annihilate<G, U> annihilate;
        if (!annihilate.anni(sf)) {
            return -1;
        }
        //cout << "annihilate end" << endl;
        int delta32 = 0;
        int delta64 = 0;
        bool lsb = false;
        const char * lsb_str = "";
#if 0
        if (opt.reverse) {
            sf.set_reverse_bit();
            lsb = true;
            cout << "Equidistribution from LSB" << endl;
            lsb_str = " from LSB";
        }
#endif
        SIMDInfo info;
        info.bitSize = bitWidth;
        info.fastMode = false;
        sf.reset_reverse_bit();
        info.bitMode = 64;
        info.elementNo = bitWidth / 64;
        if (opt.verbose) {
            cout << "64bit dimension of equidistribution at v-bit accuracy k(v)"
                 << lsb_str << endl;
        }
        for (int v = 1; v <= 64; v++) {
            int veq = calc_SIMD_equidist<U, G>(v, sf, info,
                                               opt.params.mexp, lsb);
            int d = opt.params.mexp / v - veq;
            if (opt.verbose) {
                cout << "k(" << dec << v << ") = " << dec << veq;
                cout << "\td(" << dec << v << ") = " << dec << d << endl;
            }
            delta64 += d;
        }
        info.bitMode = 32;
        info.elementNo = bitWidth / 32;
        if (opt.verbose) {
            cout << "32bit dimension of equidistribution at v-bit accuracy k(v)"
                 << lsb_str << endl;
        }
        for (int v = 1; v <= 32; v++) {
            int veq = calc_SIMD_equidist<U, G>
                (v, sf, info, opt.params.mexp, lsb);
            int d = opt.params.mexp / v - veq;
            if (opt.verbose) {
                cout << "k(" << dec << v << ") = " << dec << veq;
                cout << "\td(" << dec << v << ") = " << dec << d << endl;
            }
            delta32 += d;
        }
        cout << sf.getParamString();
        cout << dec << delta32 << "," << delta64 << endl;
        return 0;
    }
}
#endif // SFMTAVXEQ_HPP


#include <algorithm>
#include <iomanip>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <coruja/container/vector.hpp>
#include <coruja/support/boost_signals2.hpp>
#include "timer.hpp"

using namespace coruja;

template<typename F>
float run(F&& f, std::size_t arg,
                std::string label,
                std::size_t reps = 1,
                std::size_t iter = 10000)
{
    using namespace boost;
    using namespace boost::accumulators;

    std::vector<float> rtimes;
    rtimes.reserve(reps);
    for(std::size_t r(0);r < reps;++r)
    {
        std::vector<std::size_t> times;
        times.reserve(iter);
        std::size_t sum{0};
        for(std::size_t i(0); i<iter; ++i)
            times.emplace_back(f(arg));
        accumulator_set<double, stats<tag::variance> > acc;
        acc = std::for_each(times.begin(), times.end(), acc);
        rtimes.emplace_back(mean(acc));
    }
    accumulator_set<double, stats<tag::variance> > acc;
    acc = std::for_each(rtimes.begin(), rtimes.end(), acc);
    auto _mean = mean(acc);
    std::cout << label << "/" << arg << "\t"
              << "\tmean=" << std::fixed << std::setprecision(2) << _mean
              << "\tst=" << sqrt(variance(acc))
              << std::endl;
    return _mean;
}

template<typename V>
inline std::size_t test(V& v, std::size_t arg)
{
    v.reserve(arg);
    timer<std::chrono::nanoseconds> t;
    for(std::size_t i(0);i<arg;++i)
        v.push_back(i);
    return t.stop();
}

const std::size_t reps = 5;

float wsignals(std::size_t arg) {
    return run([](std::size_t arg) {
        std::vector<std::size_t> v;
        return test(v, arg);
        }, arg, "wsignals", reps);
}

float csignals(std::size_t arg) {
    return run([](std::size_t arg) {
        vector<std::size_t> v;
        return test(v, arg);
        }, arg, "csignals", reps);
}

float bsignals(std::size_t arg) {
    return run([](std::size_t arg) {
        vector<std::size_t, std::allocator<std::size_t>,
               std::vector, void, boost_signals2> v;
        return test(v, arg);
        }, arg, "bsignals", reps, 100);
}

int main() {
    std::initializer_list<std::size_t> il{5, 1000, 10000, 100000};
    for(auto n : il)
    {
        wsignals(n);
        auto v1 = csignals(n);
        auto v2 = bsignals(n);
        std::cout << "\t\tdiff=" << std::fixed << std::setprecision(2)
                  << (v2-v1)/v1 << std::endl;
    }
}


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

    @copyright 2016 NumScale SAS

  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_FUNCTION_TOINT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TOINT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object converts its parameter to integer by truncation.

    @par Header <boost/simd/function/toint.hpp>

    @par Notes

    @c toint cast a floating value to the signed integer value of the same bit size.

    This is done by C casting for scalars and corresponding intrinsic in simd (if available).

    Peculiarly,  that implies that the behaviour of this function on invalid entries is
    not defined and quite unpredictable.

    (For instance it is quite frequent that the test: `toint(Inf<double>()) ==  toint(1.0/0.0)`
    will return @ref False whilst the test: `Inf<double>() == 1.0/0.0` returns @ref True !)

    If you intend to use @ref Nan and @ref Inf entries,  consider using
    saturated_(toint) instead or its alias @ref ifix.

    @par Decorators

    - saturated_ (See @ref group-decorator) as stated above ensures good behaviour on
      limiting values.

    @see touint, tofloat

    @par Example:

      @snippet toint.cpp toint

    @par Possible output:

      @snippet toint.txt toint
  **/
  as_integer_t<Value> toint(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/toint.hpp>
#include <boost/simd/function/simd/toint.hpp>

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 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_COMMON_SCALAR_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IS_SIMD_LOGICAL_HPP_INCLUDED

#include <boost/simd/function/scalar/bitwise_cast.hpp>
#include <boost/simd/function/scalar/genmask.hpp>
#include <boost/simd/logical.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( is_simd_logical_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE logical<A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      return a0 ==  genmask(a0);
    }
  };
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( is_simd_logical_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE logical<A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      return is_simd_logical(bitwise_cast<i_t>(a0)).value();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_simd_logical_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<logical_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 ) const BOOST_NOEXCEPT
    {
      return {true};
    }
  };
} } }

#endif


//
// Created by jestjest on 11/11/17.
//

#ifndef SIMPLE_KADEMLIA_NODEINFO_HPP
#define SIMPLE_KADEMLIA_NODEINFO_HPP

#include <aux/picosha2.hpp>
#include <arpa/inet.h>
#include <boost/multiprecision/cpp_int.hpp>

namespace kdml {

    namespace mp = boost::multiprecision;

    struct NodeInfo {

        uint32_t ipAddr{};
        unsigned short port{0};
        mp::uint256_t id{};

        NodeInfo() = default;

        NodeInfo(const std::string& ip, unsigned short p)
                : port(p) {

            struct sockaddr_in sa;
            inet_pton(AF_INET, ip.c_str(), &(sa.sin_addr));
            ipAddr = sa.sin_addr.s_addr;

            std::string ipPort = ip + std::to_string(p);
            std::vector<unsigned char> hash(32);
            picosha2::hash256(ipPort.begin(), ipPort.end(), hash.begin(), hash.end());
            mp::import_bits(id, hash.begin(), hash.end());
        }

        template<class Archive>
        void save(Archive& ar) const {
            ar(ipAddr, port);
        }

        template<class Archive>
        void load(Archive& ar) {
            ar(ipAddr, port);
            std::string ip = getIpAddr() + std::to_string(port);
            std::vector<unsigned char> hash(32);
            picosha2::hash256(ip.begin(), ip.end(), hash.begin(), hash.end());
            mp::import_bits(id, hash.begin(), hash.end());
        }

        friend std::ostream& operator<<(std::ostream& os, const NodeInfo& node) {
            os << "(id: " << node.id
               << ", IP: " << node.getIpAddr()
               << ":" << node.port << ")";
            return os;
        }

        std::string getIpAddr() const {
            char str[INET_ADDRSTRLEN];
            struct sockaddr_in sa;
            sa.sin_addr.s_addr = ipAddr;
            inet_ntop(AF_INET, &(sa.sin_addr), str, INET_ADDRSTRLEN);
            std::string addrStr(str);
            return addrStr;
        }
    };
}

#endif //SIMPLE_KADEMLIA_NODEINFO_HPP


/*
 * @Description: IMU-lidar fusion for localization workflow
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */
#ifndef LIDAR_LOCALIZATION_FILTERING_KITTI_FILTERING_HPP_
#define LIDAR_LOCALIZATION_FILTERING_KITTI_FILTERING_HPP_

#include <deque>
#include <unordered_map>

#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include "lidar_localization/sensor_data/cloud_data.hpp"
#include "lidar_localization/sensor_data/imu_data.hpp"
#include "lidar_localization/sensor_data/pos_vel_data.hpp"
#include "lidar_localization/sensor_data/pose_data.hpp"

#include "lidar_localization/models/cloud_filter/box_filter.hpp"
#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"

#include "lidar_localization/models/scan_context_manager/scan_context_manager.hpp"

#include "lidar_localization/models/registration/registration_interface.hpp"

#include "lidar_localization/models/kalman_filter/kalman_filter.hpp"

namespace lidar_localization {

class KITTIFiltering {
public:
  KITTIFiltering();

  bool Init(const CloudData &init_scan, const Eigen::Vector3f &init_vel,
            const IMUData &init_imu_data);

  bool Init(const Eigen::Matrix4f &init_pose, const Eigen::Vector3f &init_vel,
            const IMUData &init_imu_data);

  bool Update(const IMUData &imu_data);
  bool Correct(const IMUData &imu_data, const CloudData &cloud_data,
               const PosVelData &pos_vel_data, Eigen::Matrix4f &cloud_pose);

  // getters:
  bool HasInited() const { return has_inited_; }
  bool HasNewGlobalMap() const { return has_new_global_map_; }
  bool HasNewLocalMap() const { return has_new_local_map_; }

  void GetGlobalMap(CloudData::CLOUD_PTR &global_map);
  CloudData::CLOUD_PTR &GetLocalMap() { return local_map_ptr_; }
  CloudData::CLOUD_PTR &GetCurrentScan() { return current_scan_ptr_; }

  double GetTime(void) { return kalman_filter_ptr_->GetTime(); }
  Eigen::Matrix4f GetPose(void) { return current_pose_; }
  Eigen::Vector3f GetVel(void) { return current_vel_; }
  void GetOdometry(Eigen::Matrix4f &pose, Eigen::Vector3f &vel);

private:
  bool InitWithConfig(void);
  // a. filter initializer:
  bool InitFilter(std::string filter_user,
                  std::shared_ptr<CloudFilterInterface> &filter_ptr,
                  const YAML::Node &config_node);
  bool InitLocalMapSegmenter(const YAML::Node &config_node);
  bool InitFilters(const YAML::Node &config_node);
  // b. map initializer:
  bool InitGlobalMap(const YAML::Node &config_node);
  // c. scan context manager initializer:
  bool InitScanContextManager(const YAML::Node &config_node);
  // d. frontend initializer:
  bool
  InitRegistration(std::shared_ptr<RegistrationInterface> &registration_ptr,
                   const YAML::Node &config_node);
  // e. IMU-lidar fusion initializer:
  bool InitFusion(const YAML::Node &config_node);

  // local map setter:
  bool ResetLocalMap(float x, float y, float z);

  // init pose setter:
  bool SetInitScan(const CloudData &init_scan);
  bool SetInitGNSS(const Eigen::Matrix4f &init_pose);
  bool SetInitPose(const Eigen::Matrix4f &init_pose);

private:
  std::string map_path_ = "";
  std::string scan_context_path_ = "";

  std::string loop_closure_method_ = "";

  // a. global map:
  std::shared_ptr<CloudFilterInterface> global_map_filter_ptr_;
  // b. local map:
  std::shared_ptr<BoxFilter> local_map_segmenter_ptr_;
  std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;
  // c. current scan:
  std::shared_ptr<CloudFilterInterface> current_scan_filter_ptr_;

  // scan context manager:
  std::shared_ptr<ScanContextManager> scan_context_manager_ptr_;
  // frontend:
  std::shared_ptr<RegistrationInterface> registration_ptr_;
  // IMU-lidar Kalman filter:
  struct {
    std::string FUSION_METHOD;

    std::unordered_map<std::string, KalmanFilter::MeasurementType>
        FUSION_STRATEGY_ID;
    KalmanFilter::MeasurementType FUSION_STRATEGY;
  } CONFIG;
  std::shared_ptr<KalmanFilter> kalman_filter_ptr_;
  KalmanFilter::Measurement current_measurement_;

  CloudData::CLOUD_PTR global_map_ptr_;
  CloudData::CLOUD_PTR local_map_ptr_;
  CloudData::CLOUD_PTR current_scan_ptr_;

  Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity();
  Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
  Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity();
  Eigen::Vector3f current_vel_ = Eigen::Vector3f::Zero();

  bool has_inited_ = false;
  bool has_new_global_map_ = false;
  bool has_new_local_map_ = false;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_FILTERING_KITTI_FILTERING_HPP_


#include <vector>

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>
#include <fstream>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/ultinous/projective_matrix_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe
{
namespace ultinous
{

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::LayerSetUp(const vector<Blob<Dtype> *> &bottom,
                                              const vector<Blob<Dtype> *> &top)
{

  CHECK(bottom[0]->channels() == 9);

  m_base_scale = this->layer_param_.proj_matrix_param().base_scale();
  m_base_f = this->layer_param_.proj_matrix_param().base_f();
  m_base_tz = this->layer_param_.proj_matrix_param().base_tz();

  m_min_scale = this->layer_param_.proj_matrix_param().min_scale();
  m_max_scale = this->layer_param_.proj_matrix_param().max_scale();
  m_min_f = this->layer_param_.proj_matrix_param().min_f();
  m_max_f = this->layer_param_.proj_matrix_param().max_f();
  m_min_alpha = this->layer_param_.proj_matrix_param().min_alpha();
  m_max_alpha = this->layer_param_.proj_matrix_param().max_alpha();
  m_min_beta = this->layer_param_.proj_matrix_param().min_beta();
  m_max_beta = this->layer_param_.proj_matrix_param().max_beta();
  m_min_gamma = this->layer_param_.proj_matrix_param().min_gamma();
  m_max_gamma = this->layer_param_.proj_matrix_param().max_gamma();

  m_min_tx = this->layer_param_.proj_matrix_param().min_tx();
  m_max_tx = this->layer_param_.proj_matrix_param().max_tx();
  m_min_ty = this->layer_param_.proj_matrix_param().min_ty();
  m_max_ty = this->layer_param_.proj_matrix_param().max_ty();
  m_min_tz = this->layer_param_.proj_matrix_param().min_tz();
  m_max_tz = this->layer_param_.proj_matrix_param().max_tz();

  m_bias_scale = this->layer_param_.proj_matrix_param().bias_scale();
  m_bias_U = this->layer_param_.proj_matrix_param().bias_u();

  m_max_diff = this->layer_param_.proj_matrix_param().max_diff();
  m_boundary_violation_step = 0.1;
  m_iter = 0;

  if (this->blobs_.size() > 0)
  {
    LOG(INFO) << "Skipping parameter initialization";
  }
  else
  {
    this->blobs_.resize(1);
    vector<int> sz;
    sz.push_back(bottom[0]->channels());

    this->blobs_[0].reset(new Blob<Dtype>(sz));
    caffe_set(this->blobs_[0]->count(), Dtype(0),
              this->blobs_[0]->mutable_cpu_data());
  }

  std::vector<int> top_shape(2);
  top_shape[0] = bottom[0]->num();
  top_shape[1] = 9;
  top[0]->Reshape(top_shape);
}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Reshape(const vector<Blob<Dtype> *> &bottom,
                                           const vector<Blob<Dtype> *> &top)
{
  std::vector<int> top_shape(2);
  top_shape[0] = bottom[0]->num();
  top_shape[1] = 9;
  top[0]->Reshape(top_shape);
}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Forward_cpu(const vector<Blob<Dtype> *> &bottom,
                                               const vector<Blob<Dtype> *> &top)
{
  ++m_iter;
  if (this->phase_ == TRAIN && (m_iter % 100) == 0)
  {
    Dtype const *bias = this->blobs_[0]->cpu_data();
    std::cout << "Bias:";
    for (int c = 0; c < 9; ++c)
      std::cout << " " << bias[c];
    std::cout << std::endl;
  }

/**
  Original matrix:
  H = [ cos(gamma)*cos(beta)*S*f  (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*S*f  U*f;
      sin(gamma)*cos(beta)*S*f  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*S*f   V*f;
    -sin(beta)*S             cos(beta)*sin(alpha)*S                                     W+f];
*/

/** Non-uniform scaling:
  Original matrix:
  H = [ cos(gamma)*cos(beta)*Sx*f  (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx*f  U*f*Sx;
      sin(gamma)*cos(beta)*Sy*f  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy*f   V*f*Sy;
    -sin(beta)             cos(beta)*sin(alpha)                                     W+f];
*/

  for (int n = 0; n < bottom[0]->num(); ++n)
  {
    Dtype const *bottom_data = bottom[0]->cpu_data() + 9 * n;

    Dtype Sx = m_base_scale + bottom_data[0];
    Dtype Sy = m_base_scale + bottom_data[1];
    Dtype f = m_base_f + bottom_data[2];
    Dtype alpha = bottom_data[3];
    Dtype beta = bottom_data[4];
    Dtype gamma = bottom_data[5];
    Dtype U = bottom_data[6];
    Dtype V = bottom_data[7];
    Dtype W = m_base_tz + bottom_data[8];

    Dtype const *bias = this->blobs_[0]->cpu_data();
    if (m_bias_scale)
    {
      Sx += bias[0];
      Sy += bias[1];
    }
    if (m_bias_U)
    {
      U += bias[6];
    }

    Dtype *top_data = top[0]->mutable_cpu_data() + 9 * n;
    top_data[0] = cos(gamma)*cos(beta)*Sx*f;
    top_data[1] = (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx*f;
    top_data[2] = U*f*Sx;

    top_data[3] = sin(gamma)*cos(beta)*Sy*f;
    top_data[4] = (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy*f;
    top_data[5] = V*f*Sy;

    top_data[6] = -sin(beta);
    top_data[7] = cos(beta)*sin(alpha);
    top_data[8] = W+f;
  }

}

template<typename Dtype>
void ProjectiveMatrixLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype> *> &top,
                                                const vector<bool> &propagate_down,
                                                const vector<Blob<Dtype> *> &bottom)
{

  if (!propagate_down[0]) return;

  for (int n = 0; n < bottom[0]->num(); ++n)
  {
    Dtype const *bottom_data = bottom[0]->cpu_data() + 9 * n;

    Dtype Sx = m_base_scale + bottom_data[0];
    Dtype Sy = m_base_scale + bottom_data[1];
    Dtype f = m_base_f + bottom_data[2];
    Dtype alpha = bottom_data[3];
    Dtype beta = bottom_data[4];
    Dtype gamma = bottom_data[5];
    Dtype U = bottom_data[6];
    Dtype V = bottom_data[7];
    Dtype W = m_base_tz + bottom_data[8];

    Dtype const *bias = this->blobs_[0]->cpu_data();
    if (m_bias_scale)
    {
      Sx += bias[0];
      Sy += bias[1];
    }
    if (m_bias_U)
    {
      U += bias[6];
    }

    Dtype *bottom_diff = bottom[0]->mutable_cpu_diff() + 9 * n;
    Dtype const *top_diff = top[0]->cpu_diff() + 9 * n;

    if (Sx < m_min_scale)
      bottom_diff[0] = (Sx - m_min_scale) * m_boundary_violation_step;
    else if (Sx > m_max_scale)
      bottom_diff[0] = (Sx - m_max_scale) * m_boundary_violation_step;
    else
      bottom_diff[0] = top_diff[0] * (cos(gamma)*cos(beta)*f)
                       +top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*f)
                      + top_diff[2] * U*f;

    if (Sy < m_min_scale)
      bottom_diff[1] = (Sy - m_min_scale) * m_boundary_violation_step;
    else if (Sy > m_max_scale)
      bottom_diff[1] = (Sy - m_max_scale) * m_boundary_violation_step;
    else
      bottom_diff[1] = top_diff[3] * ( sin(gamma)*cos(beta)*f )
                       +top_diff[4] * (  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*f )
                      + top_diff[5] * V*f;

    if (f < m_min_f)
      bottom_diff[2] = (f - m_min_f) * m_boundary_violation_step;
    else if (f > m_max_f)
      bottom_diff[2] = (f - m_max_f) * m_boundary_violation_step;
    else
      bottom_diff[2] =  top_diff[0] * (cos(gamma)*cos(beta)*Sx)
                        +top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sx)
                         +top_diff[2] * ( U * Sx )
                        +top_diff[3] * ( sin(gamma)*cos(beta)*Sy )
                        +top_diff[4] * (  (cos(gamma)*cos(alpha)+sin(gamma)*sin(beta)*sin(alpha))*Sy )
                        +top_diff[5] * ( V * Sy)
                        +top_diff[8] * ( 1 );

    if (alpha < m_min_alpha)
      bottom_diff[3] = (alpha - m_min_alpha) * m_boundary_violation_step;
    else if (alpha > m_max_alpha)
      bottom_diff[3] = (alpha - m_max_alpha) * m_boundary_violation_step;
    else
      bottom_diff[3] = top_diff[1] * ( (-sin(gamma)*-sin(alpha)+cos(gamma)*sin(beta)*cos(alpha))*Sx*f )
                      + top_diff[4] * ((cos(gamma)*-sin(alpha)+sin(gamma)*sin(beta)*cos(alpha))*Sy*f)
                      + top_diff[7] * (cos(beta)*cos(alpha));

    if (beta < m_min_beta)
      bottom_diff[4] = (beta - m_min_beta) * m_boundary_violation_step;
    else if (beta > m_max_beta)
      bottom_diff[4] = (beta - m_max_beta) * m_boundary_violation_step;
    else
      bottom_diff[4] = top_diff[0] * (cos(gamma)*-sin(beta)*Sx*f)
                  + top_diff[1] * ( (-sin(gamma)*cos(alpha)+cos(gamma)*cos(beta)*sin(alpha))*Sx*f)
              + top_diff[3] * (sin(gamma)*-sin(beta)*Sy*f)
            + top_diff[4] * ( (cos(gamma)*cos(alpha)+sin(gamma)*cos(beta)*sin(alpha))*Sy*f)
            + top_diff[6] * (-cos(beta))
            + top_diff[7] * (-sin(beta)*sin(alpha));

    if (gamma < m_min_gamma)
      bottom_diff[5] = (gamma - m_min_gamma) * m_boundary_violation_step;
    else if (gamma > m_max_gamma)
      bottom_diff[5] = (gamma - m_max_gamma) * m_boundary_violation_step;
    else
      bottom_diff[5] = top_diff[0] * (-sin(gamma)*cos(beta)*Sx*f)
              + top_diff[1] * ((-cos(gamma)*cos(alpha)+-sin(gamma)*sin(beta)*sin(alpha))*Sx*f)
                + top_diff[3] * (cos(gamma)*cos(beta)*Sy*f)
                  + top_diff[4] * ((-sin(gamma)*cos(alpha)+cos(gamma)*sin(beta)*sin(alpha))*Sy*f);

    if (U < m_min_tx)
      bottom_diff[6] = (U - m_min_tx) * m_boundary_violation_step;
    else if (U > m_max_tx)
      bottom_diff[6] = (U - m_max_tx) * m_boundary_violation_step;
    else
      bottom_diff[6] = top_diff[2]*f*Sx;

    if (V < m_min_ty)
      bottom_diff[7] = (V - m_min_ty) * m_boundary_violation_step;
    else if (V > m_max_ty)
      bottom_diff[7] = (V - m_max_ty) * m_boundary_violation_step;
    else
      bottom_diff[7] = top_diff[5]*f*Sy;

    if (W < m_min_tz)
      bottom_diff[8] = (W - m_min_tz) * m_boundary_violation_step;
    else if (W > m_max_tz)
      bottom_diff[8] = (W - m_max_tz) * m_boundary_violation_step;
    else
      bottom_diff[8] = top_diff[8] * (1);

    if (m_max_diff != 0)
      for (int i = 0; i < 9; i++)
        bottom_diff[i] = std::max(-m_max_diff, std::min(m_max_diff, bottom_diff[i]));

    // Bias - only on specific params
    Dtype *bias_diff = this->blobs_[0]->mutable_cpu_diff();
    if (m_bias_scale)
    {
      bias_diff[0] += bottom_diff[0]; // scaleX
      bias_diff[1] += bottom_diff[1]; // scaleY
    }
    if (m_bias_U)
    {
      bias_diff[6] += bottom_diff[6]; // // U
    }

  }
}

#ifdef CPU_ONLY
//STUB_GPU(ProjectiveMatrixLayer);
#endif

INSTANTIATE_CLASS(ProjectiveMatrixLayer);

REGISTER_LAYER_CLASS(ProjectiveMatrix);

}  // namespace ultinous
}  // namespace caffe


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_UTILITY_AS_INDEX_HPP_INCLUDED
#define NT2_CORE_UTILITY_AS_INDEX_HPP_INCLUDED

#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/sdk/meta/as_unsigned.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/value_at.hpp>
#include <boost/fusion/include/size.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/eval_if.hpp>
#include <cstddef>

namespace nt2 { namespace result_of
{
  template<class Seq>
  struct sequence_value
   : boost::dispatch::meta::
     strip< typename boost::mpl::
            eval_if_c< boost::fusion::result_of::size<Seq>::type::value == 0
                     , boost::mpl::identity<std::size_t>
                     , boost::fusion::result_of::value_at_c<Seq, 0>
                     >::type
          >
  {
  };

  template<class Size, class Position>
  struct as_index
  {
    typedef typename sequence_value<Position>::type               typeS;
    typedef typename meta::as_unsigned<typeS>::type               type;

    BOOST_DISPATCH_FORCE_INLINE type
    operator()(const Size& size, const Position& pos) const
    {
      typedef typename boost::fusion::result_of::size<Position>::type dims;
      return eval ( size, pos
                  , boost::mpl::int_<0>()
                  , boost::mpl::int_<dims::value-1>()
                  );
    }

    template<class Idx, class Sz>
    BOOST_DISPATCH_FORCE_INLINE type
    eval(const Size& s, const Position& p, const Idx&, const Sz& sz) const
    {
      return  splat<type>( boost::fusion::at_c<Idx::value>(p) )
            + splat<type>( boost::fusion::at_c<Idx::value>(s) )
            * eval(s,p,boost::mpl::int_<Idx::value+1>(),sz);
    }

    template<class Sz>
    BOOST_DISPATCH_FORCE_INLINE type
    eval(const Size& s,const Position& p,const boost::mpl::int_<0>&,const Sz& sz) const
    {
      return  splat<type>( boost::fusion::at_c<0>(p) )
            + splat<type>( boost::fusion::at_c<0>(s) )
            * eval(s,p,boost::mpl::int_<1>(),sz);
    }

    template<class Sz>
    BOOST_DISPATCH_FORCE_INLINE type
    eval(const Size&, const Position& p, const Sz&, const Sz&) const
    {
      return splat<type>( boost::fusion::at_c<Sz::value>(p) );
    }

    BOOST_DISPATCH_FORCE_INLINE type
    eval( const Size&, const Position& p
        , const boost::mpl::int_<0>&, const boost::mpl::int_<0>&
        ) const
    {
      return splat<type>( boost::fusion::at_c<0>(p) );
    }

    BOOST_DISPATCH_FORCE_INLINE type
    eval( const Size&, const Position&
        , const boost::mpl::int_<0>&, const boost::mpl::int_<-1>&
        ) const
    {
      return Zero<type>();
    }
  };
} }

namespace nt2
{
  template<class Size,class Position>
  typename result_of::as_index<Size,Position>::type
  as_index(Size const& sz, Position const& p)
  {
    return result_of::as_index<Size,Position>()(sz,p);
  }
}

#endif


//==================================================================================================
/*!
  @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_COMMON_SIMD_FUNCTION_GROUP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_GROUP_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/deinterleave_first.hpp>
#include <boost/simd/function/simd/deinterleave_second.hpp>
#include <boost/simd/function/simd/extract.hpp>
#include <boost/simd/function/simd/slice.hpp>
#include <boost/dispatch/meta/downgrade.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/mpl/not.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(group_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     using base_t = bd::downgrade_t<typename A0::value_type>; //I smell a problem here
      static const std::size_t M = 2*bs::cardinal_of<base_t>::value;
      using result =  pack<base_t, M>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return eval(a0, boost::is_same<A0, result>());
      }
      BOOST_FORCEINLINE
      result eval(A0 const& a0, std::false_type const&) const
      {
        return eval2(a0, brigand::bool_<(A0::static_size>=4)>());
      }
      BOOST_FORCEINLINE
      result eval(A0 const& a0, std::true_type const&) const
      {
        return a0;
      }
      BOOST_FORCEINLINE
      result eval2(A0 const& a0, std::true_type const&) const
      {
        pack<base_t, bs::cardinal_of<A0>::value/2> a00, a01;
        bs::slice(a0,a00,a01);
        return bs::group(a00,a01);
      }
      BOOST_FORCEINLINE
      result eval2(A0 const& a0, std::false_type const&) const
      {
        return { static_cast<base_t>( a0[0] )
               , static_cast<base_t>( a0[1])
               };
      }
   };

   BOOST_DISPATCH_OVERLOAD(group_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      using result = bd::downgrade_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        static const size_t size = bs::cardinal_of<A0>::value;
        using sR = bd::scalar_of_t<result>;
        result that;
        for(size_t i = 0; i != size; ++i)
          that[i]      = static_cast<sR>(a0[i]);
        for(size_t i = 0; i != size; ++i)
          that[i+size] = static_cast<sR>(a1[i]);
        return that;
      }
   };

   BOOST_DISPATCH_OVERLOAD(group_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      using result = bd::downgrade_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        #ifdef BOOST_LITTLE_ENDIAN
        return deinterleave_first(bitwise_cast<result>(a0), bitwise_cast<result>(a1));
        #else
        return deinterleave_second(bitwise_cast<result>(a0), bitwise_cast<result>(a1));
        #endif
      }
   };

} } }
#endif


// ------------------------------------------------------------------------
//  Copyright (C)
//  ETH Zurich - Switzerland
//
//  Kevis-Kokitsi Maninis <kmaninis@vision.ee.ethz.ch>
//  Jordi Pont-Tuset <jponttuset@vision.ee.ethz.ch>
//  July 2016
// ------------------------------------------------------------------------
// This file is part of the COB package presented in:
//    K.K. Maninis, J. Pont-Tuset, P. Arbelaez and L. Van Gool
//    Convolutional Oriented Boundaries
//    European Conference on Computer Vision (ECCV), 2016
// Please consider citing the paper if you use this code.
// ------------------------------------------------------------------------
#include "mex.h"
#include "ucm2hier.hpp"
#include "part2cont.hpp"
#include "ucm_fill_gaps.hpp"
#include "mex_helpers.hpp"

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace std;

template<class T>
std::pair<T,T> get_sorted_pair(T n1, T n2)
{
    if (n1<n2)
        return std::make_pair(n1,n2);
    else
        return std::make_pair(n2,n1);
}

void mexFunction( int nlhs, mxArray *plhs[],
        		  int nrhs, const mxArray*prhs[] )
{
    if(nrhs!=6)
        mexErrMsgTxt("There should be 6 input parameters");

    /* Input parameters */
    auto ucm2              =     eigen_map<double>(prhs[0]);
    auto lp_ucm            = eigen_map<label_type>(prhs[1]);
    std::size_t nr_ucm     =           mxGetScalar(prhs[2]);
    auto lp_project        = eigen_map<label_type>(prhs[3]);
    std::size_t nr_project =           mxGetScalar(prhs[4]);
    std::size_t mode       =           mxGetScalar(prhs[5]);

    /* Check sizes */
    if (ucm2.rows() != 2*lp_ucm.rows()+1 || ucm2.rows() != 2*lp_project.rows()+1)
        mexErrMsgTxt("UCM and partitions sizes not coherent");

    /* Project the superpixels of lp_project to lp_ucm via maximum overlap */
    std::vector<std::vector<std::size_t> > sup_count(nr_project,std::vector<std::size_t>(nr_ucm,0));
    for(std::size_t xx=0; xx<lp_ucm.rows(); ++xx)
        for(std::size_t yy=0; yy<lp_ucm.cols(); ++yy)
            sup_count[lp_project(xx,yy)-1][lp_ucm(xx,yy)-1]++;

    /* Map between the labels of lp_project to lp_ucm */
    std::vector<label_type> sup_map(nr_project,0);
    for(std::size_t ii=0; ii<nr_project; ++ii)
        for(std::size_t jj=1; jj<nr_ucm; ++jj)
            if (sup_count[ii][sup_map[ii]]<sup_count[ii][jj])
                sup_map[ii] = jj;

    /* Relabel lp_project */
    Eigen::Array<label_type,Eigen::Dynamic,Eigen::Dynamic> lp_proj_rel(lp_project.rows(),lp_project.cols());
    for(std::size_t xx=0; xx<lp_project.rows(); ++xx)
        for(std::size_t yy=0; yy<lp_project.cols(); ++yy)
            lp_proj_rel(xx,yy) = sup_map[lp_project(xx,yy)-1]+1;

// for(std::size_t xx=0; xx<lp_project.rows(); ++xx)
// {
//     for(std::size_t yy=0; yy<lp_project.cols(); ++yy)
//        printf("%d\t",lp_proj_rel(xx,yy));
//     printf("\n");
// }

    /* Extract the contour positions of lp_project */
    contour_container conts = part2cont(lp_proj_rel);
// printf("Conts:\n");
// conts.print();

    /* Get the neighbors of each region */
    std::map<label_type,std::set<label_type> > neighbors;
    for(const auto& elem: conts)
    {
        neighbors[elem.first.first].insert(elem.first.second);
        neighbors[elem.first.second].insert(elem.first.first);
    }

// printf("Neighbors:\n");
// for(const auto& elem: neighbors)
// {
//     printf("%d: ",elem.first);
//     for(const auto& elem2: elem.second)
//         printf("%d,  ",elem2);
//     printf("\n");
//
// }

//     /* Invert the map (set of labels from lp_ucm that overlap with each label) */
//     std::vector<std::list<label_type> > inv_sup_map(nr_ucm);
//     for(std::size_t ii=1; ii<nr_project; ++ii)
//         inv_sup_map[sup_map[ii]].push_back(ii);

    /* Get the hierarchy out of the ucm */
    merging_sequence ms = ucm2hier(ucm2, lp_ucm);
// printf("Merging sequence:\n");
// ms.print();

    /* Get the maximum ucm threshold */
    double max_ucm_th = *(std::max_element(ms.start_ths.begin(), ms.start_ths.end()));
    double step_th = max_ucm_th/100.;
    double curr_ucm_th = step_th;

    /* Allocation ucm2_project */
    plhs[0] = mxCreateDoubleMatrix(ucm2.rows(),ucm2.cols(),mxREAL);
    auto ucm2_project = eigen_map<double>(plhs[0]);

    /* Redo the merging sequence on lp_project and get mark the ucm contours */
    label_type new_region = ms.n_regs;
    for(std::size_t ii=0; ii<ms.parent_labels.size(); ++ii)
    {
        /* Sweep all children */
        std::size_t num_children_done = 0;
        for(auto it = ms.children_labels[ii].begin(); it!=ms.children_labels[ii].end(); ++it, ++num_children_done)
        {

            auto itb = it; ++itb;
            if (itb==ms.children_labels[ii].end())
                break;

            /* Two labels being merged */
            label_type lab1;
            if (num_children_done==0)
                lab1 = *it;
            else
                lab1 = new_region;
            label_type lab2 = *itb;

            /* Is it the last we'll do? */
            label_type parent_label;
            auto itc = itb; ++itc;
            if (itc==ms.children_labels[ii].end())
                parent_label = ms.parent_labels[ii];
            else
            {
                new_region++;
                parent_label = new_region;
            }

            /* Get the threshold value to apply (quantized or not) */
            double threshold_value;
            if (mode==1)
                threshold_value = ms.start_ths[ms.n_leaves+ii];
            else if (mode==2)
            {
                while(curr_ucm_th+step_th < ms.start_ths[ms.n_leaves+ii])
                    curr_ucm_th += step_th;

                threshold_value = curr_ucm_th;
            }
            else
                mexErrMsgTxt("Unknown mode");

            auto it2 = conts.find(get_sorted_pair(lab1,lab2));
            if (it2==conts.end())
            {
//     printf("Not found (%d,%d) - %0.3f\n",lab1,lab2,ms.start_ths[ms.n_leaves+ii]);
            }
            else
            {
//     printf("Found (%d,%d) - %0.3f\n",lab1,lab2,ms.start_ths[ms.n_leaves+ii]);
                for (const cont_elem& ct:it2->second)
                    ucm2_project(ct.x,ct.y) = threshold_value;
    // printf("Next step\n");

                /* Erase merged neighbors */
                conts.erase(it2);
                neighbors[lab1].erase(lab2);
                neighbors[lab2].erase(lab1);
    // printf("Erased\n");

            }

            /* Redo the neighbors with the parent */
            for (label_type neigh:neighbors[lab1])
            {
//     printf("    - Scanning (%d,%d)\n",get_sorted_pair(lab1,neigh).first,get_sorted_pair(lab1,neigh).second);
                auto it3 = conts.find(get_sorted_pair(lab1,neigh));
                for (const cont_elem& ct2: it3->second)
                    conts[get_sorted_pair(parent_label,neigh)].push_back(ct2);
                conts.erase(get_sorted_pair(lab1,neigh));
                neighbors[neigh].erase(lab1);
                neighbors[parent_label].insert(neigh);
                neighbors[neigh].insert(parent_label);
//     printf("    - Adding (%d,%d)\n",neigh,parent_label);

            }
            for (label_type neigh:neighbors[lab2])
            {
//     printf("    - Scanning (%d,%d)\n",get_sorted_pair(lab2,neigh).first,get_sorted_pair(lab2,neigh).second);
                auto it3 = conts.find(get_sorted_pair(lab2,neigh));
                for (const cont_elem& ct2: it3->second)
                    conts[get_sorted_pair(parent_label,neigh)].push_back(ct2);
                conts.erase(get_sorted_pair(lab2,neigh));
                neighbors[neigh].erase(lab2);
                neighbors[parent_label].insert(neigh);
                neighbors[neigh].insert(parent_label);

//     printf("    - Adding (%d,%d)\n",neigh,parent_label);

            }

//             /* Erase merged entries in the neighbors */
//             neighbors.erase(lab1);
//             neighbors.erase(lab2);

// printf("Conts:\n");
// conts.print();
//
// printf("Neighbors:\n");
// for(const auto& elem: neighbors)
// {
//     printf("%d: ",elem.first);
//     for(const auto& elem2: elem.second)
//         printf("%d,  ",elem2);
//     printf("\n");
// }
        }
// for(std::size_t xx=0; xx<ucm2_project.rows(); ++xx)
// {
//     for(std::size_t yy=0; yy<ucm2_project.cols(); ++yy)
//        printf("%0.1f\t",ucm2_project(xx,yy));
//     printf("\n");
// }

    }

    /* Connect the gaps in the UCM */
    ucm_fill_gaps(ucm2_project);
}


/*
Copyright 2014 Alberto Crivellaro, Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
alberto.crivellaro@epfl.ch

terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA
 */

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <Eigen/Core>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <list>
#include <vector>
#include<stdio.h>
#include <chrono>

#include "mtf/ThirdParty/DFT/HomographyEstimation.hpp"

using namespace std;
using namespace cv;
using namespace std::chrono;

#define B_VERBOSE_TRACKING 0
#define CAM_HEIGHT (480)
#define CAM_WIDTH (640)

void CalibrateVideoStream(VideoCapture capture);

int main(int ac, char ** av)
{
	if (ac != 2)
	{
		cerr <<" Usage : ./homographyDemo <videoName>"<<endl;
		cerr <<" <videoName> can be the video file name or a camera ID (0, 1, etc.) for live demo. "<<endl;
		return 1;
	}

	cout<< " ***********************   Starting demo ... ************************************"<<endl;
	cout<< "press 1 : intensities "<<endl;
	cout<< "press 2 : gradient magnitude"<<endl;
	cout<< "press 3 : descriptor fields"<<endl;
	cout<< "press g : resets tracking"<<endl;
	cout<< "press space : pause tracking"<<endl;
	cout<< " *********************************************************************************"<<endl;

	std::string arg = av[1];
	VideoCapture capture(arg);
	if (!capture.isOpened())
		capture.open(atoi(arg.c_str()));
	if (!capture.isOpened()) {
		cerr << "Failed to open a video device or video file!\n" << endl;
		return 1;
	}
	CalibrateVideoStream(capture);

	return 0;
}

void CalibrateVideoStream(VideoCapture capture)
{

	string window_name = "ISMAR Demo 2014";

	namedWindow(window_name, CV_WINDOW_KEEPRATIO);

	OptimizationType optimizationType = descriptorFields;
	string method = string("Using Our DESCRIPTOR FIELDS!");

	bool bResetOptimization(false);
	OptimizationParameters optimizationParametersDescrFields = ReadOptimizationParametersFromXML("parameter/parametersDescriptorFieldsOptimization.yml");
	OptimizationParameters optimizationParametersSSD = ReadOptimizationParametersFromXML("parameter/parametersSSDOptimization.yml");
	OptimizationParameters optimizationParametersGradientModule = ReadOptimizationParametersFromXML("parameter/parametersGradientMagnitudeOptimization.yml");

	OptimizationParameters* optimizationParameters(&optimizationParametersDescrFields);

	cout<<endl<<endl<<*optimizationParameters<<endl;

	LucasKanade optimization;
	AlignmentResults results;

	Mat templ, image, firstFrame, currentFrameRGB;

//--------------------init traking of libalbe start here
	AcquireVGAGrayscaleImage(capture, templ);

	firstFrame = templ.clone();
	StructOfArray2di pixelsOnTemplate;
	if(optimizationParameters->bAdaptativeChoiceOfPoints)
		pixelsOnTemplate = CreateAnisotropicGridOfControlPoints(templ, optimizationParameters->nControlPointsOnEdge,
		optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
	else
		pixelsOnTemplate = CreateGridOfControlPoints(templ, optimizationParameters->nControlPointsOnEdge,
		optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);

	StructOfArray2di pixelsOnTemplateWarped = pixelsOnTemplate;

	vector<float> parameters(8,0);
	vector<float> parametersInitialGuess(8,0);

	StructOfArray2di warpedPixels(4);
	StructOfArray2di  panelCorners = GetRectangleCornersForAugmentation(optimizationParameters, templ.cols, templ.rows);
//--------------------init traking of libalbe stop here

	Matx33f homographyMatrix;
	high_resolution_clock::time_point startTime, endTime;

	float elapsedTime(0.), tim;
	bool bKeepTracking(true);

	int nFrames = 0;

	while(bKeepTracking)
	{
		AcquireVGAGrayscaleImage(capture, image, currentFrameRGB);

		if (image.empty())
			break;

		startTime = high_resolution_clock::now();

		switch (optimizationType)
		{
		case descriptorFields:
			results = optimization.DescriptorFieldsCalibration(pixelsOnTemplateWarped,
				templ, image, parametersInitialGuess,
				*optimizationParameters); //parameters are updated with the current guess!
			break;
		case gradientModule:
			results = optimization.GradientModuleCalibration(pixelsOnTemplateWarped,
				templ, image, parametersInitialGuess,
				*optimizationParameters); //parameters are updated with the current guess!
			break;
		case intensity:
			results = optimization.SSDCalibration(pixelsOnTemplateWarped,
				templ, image, parametersInitialGuess,
				*optimizationParameters);//parameters are updated with the current guess!
			break;
		default:
			break;
		}

		Homography::CompositionalParametersUpdateWithCheck(parameters, parametersInitialGuess);

		for (int iPoint = 0; iPoint< panelCorners.size(); ++iPoint)
			Homography::ComputeWarpedPixels(panelCorners[iPoint].x,panelCorners[iPoint].y, warpedPixels.x[iPoint], warpedPixels.y[iPoint], parameters);

		Mat ctrlPtImg;
		if (B_VERBOSE_TRACKING)
		{
			WriteResultsOnImage(image, results, pixelsOnTemplateWarped.size(), optimizationType);
			// Draw the control points
			ctrlPtImg = Mat(templ.rows, templ.cols,CV_8UC3,Scalar(0,0,0));
			for (int idxPt = 0; idxPt < pixelsOnTemplateWarped.size(); ++idxPt)
				circle(ctrlPtImg, pixelsOnTemplateWarped[idxPt], 3, Scalar(255,255,255) );
		}

		AugmentFrameWithQuadrilater(warpedPixels, currentFrameRGB);

		Mat frameToDisplay;
		if (ctrlPtImg.rows > 0)
		{
			hconcat(currentFrameRGB, ctrlPtImg, frameToDisplay);
		}else
			frameToDisplay = currentFrameRGB;//quick copy

		imshow(window_name, frameToDisplay);

		endTime = high_resolution_clock::now();
		tim = duration_cast<duration<double> >(endTime - startTime).count();
		elapsedTime +=tim;
		nFrames++;

		char key = (char)waitKey(1);
		switch (key)
		{
		case 27:
			bKeepTracking = false;
			break;
		case ' ':
			key = (char)waitKey(0);
			break;
		case '1':
			optimizationType = intensity;
			optimizationParameters = &optimizationParametersSSD;
			method = string("Using intensity!");
			cout<<method<<endl;
			bResetOptimization = true;
			break;
		case '2':
			optimizationType = gradientModule;
			optimizationParameters = &optimizationParametersGradientModule;
			method = string("Using gradient magnitude!");
			cout<< method <<endl;
			bResetOptimization = true;
			break;
		case '3':
			optimizationType = descriptorFields;
			optimizationParameters = &optimizationParametersDescrFields;
			method = string("Using Our DESCRIPTOR FIELDS!");
			cout<< method <<endl;
			bResetOptimization = true;
			break;
		case 'g':
			bResetOptimization = true;
			break;
		default:
			;
		}

		if (bResetOptimization)
		{
			bResetOptimization = false;
			cout<<"reset template!"<<endl;
			fill(parameters.begin(), parameters.end(), 0.);
			fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);

			AcquireVGAGrayscaleImage(capture, templ);

			firstFrame = templ.clone();
			if(optimizationParameters->bAdaptativeChoiceOfPoints)
				pixelsOnTemplate = CreateAnisotropicGridOfControlPoints(firstFrame, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);
			else
				pixelsOnTemplate = CreateGridOfControlPoints(firstFrame, optimizationParameters->nControlPointsOnEdge, optimizationParameters->borderThicknessHorizontal, optimizationParameters->borderThicknessVertical);

			pixelsOnTemplateWarped = pixelsOnTemplate;
		}

		homographyMatrix = Homography::GetUnscaledMatrix(parameters);
		warpPerspective(firstFrame, templ, homographyMatrix, templ.size(), INTER_LINEAR, BORDER_CONSTANT);
		WarpGridOfControlPoints(pixelsOnTemplate, pixelsOnTemplateWarped, parameters, templ.cols, templ.rows);
		fill(parametersInitialGuess.begin(), parametersInitialGuess.end(), 0.);
	}

	cout << "Calibrated "<<nFrames<< " frames"<<endl;
	cout << "average elapsedTime = "<< elapsedTime/nFrames<<endl;
}


#include <dlib/graph_utils.h>
#include "classify.h"

int classify(
	const std::vector<descriptor>& samples,
	const std::unordered_map<int, int>& cats,
	const descriptor& test_sample,
	float tolerance
) {
	if (samples.size() == 0)
		return -1;

	std::vector<std::pair<int, float>> distances;
	distances.reserve(samples.size());
	auto dist_func = dlib::squared_euclidean_distance();
	int idx = 0;
	for (const auto& sample : samples) {
		float dist = dist_func(sample, test_sample);
		if (dist < tolerance)
			continue;
		distances.push_back({idx, dist});
		idx++;
	}

	if (distances.size() == 0)
		return -1;

	std::sort(
		distances.begin(), distances.end(),
		[](const auto a, const auto b) { return a.second < b.second; }
	);

	int len = std::min((int)distances.size(), 10);
	std::unordered_map<int, std::pair<int, float>> hits_by_cat;
	for (int i = 0; i < len; i++) {
		int idx = distances[i].first;
		float dist = distances[i].second;
		auto cat = cats.find(idx);
		if (cat == cats.end())
			continue;
		int cat_idx = cat->second;
		auto hit = hits_by_cat.find(cat_idx);
		if (hit == hits_by_cat.end()) {
			// printf("1 hit for %d (%d: %f)\n", cat_idx, idx, dist);
			hits_by_cat[cat_idx] = {1, dist};
		} else {
			// printf("+1 hit for %d (%d: %f)\n", cat_idx, idx, dist);
			hits_by_cat[cat_idx].first++;
		}
	}

	auto hit = std::max_element(
		hits_by_cat.begin(), hits_by_cat.end(),
		[](const auto a, const auto b) {
			auto hits1 = a.second.first;
			auto hits2 = b.second.first;
			auto dist1 = a.second.second;
			auto dist2 = b.second.second;
			if (hits1 == hits2) return dist1 > dist2;
			return hits1 < hits2;
		}
	);
	// printf("Found cat with max hits: %d\n", hit->first); fflush(stdout);
	return hit->first;
}