/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once
#ifndef SOLVER_GUROBI_UTILS_HPP
#define SOLVER_GUROBI_UTILS_HPP

#include "gurobi_c++.h"
#include <sstream>
#include <Eigen/Dense>
#include <type_traits>
// using namespace std;

inline double MinPositiveElement(std::vector<double> v)
{
  std::sort(v.begin(), v.end());  // sorted in ascending order
  double min_value = 0;
  for (int i = 0; i < v.size(); i++)
  {
    if (v[i] > 0)
    {
      min_value = v[i];
      break;
    }
  }
  return min_value;
}

template <typename T>
GRBQuadExpr GetNorm2(const std::vector<T>& x)  // Return the squared norm of a vector
{
  GRBQuadExpr result = 0;
  for (int i = 0; i < x.size(); i++)
  {
    result = result + x[i] * x[i];
  }
  return result;
}

template <typename T>
std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<T>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}

/*std::vector<GRBLinExpr> MatrixMultiply(const std::vector<std::vector<double>>& A, const std::vector<GRBLinExpr>& x)
{
  std::vector<GRBLinExpr> result;

  for (int i = 0; i < A.size(); i++)
  {
    GRBLinExpr lin_exp = 0;
    for (int m = 0; m < x.size(); m++)
    {
      lin_exp = lin_exp + A[i][m] * x[m];
    }
    result.push_back(lin_exp);
  }
  return result;
}*/

template <typename T>  // Overload + to sum Elementwise std::vectors
std::vector<T> operator+(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::plus<T>());
  return result;
}

template <typename T>  // Overload - to substract Elementwise std::vectors
std::vector<T> operator-(const std::vector<T>& a, const std::vector<T>& b)
{
  assert(a.size() == b.size());

  std::vector<T> result;
  result.reserve(a.size());

  std::transform(a.begin(), a.end(), b.begin(), std::back_inserter(result), std::minus<T>());
  return result;
}

template <typename T>
std::vector<GRBLinExpr> operator-(const std::vector<T>& x, const std::vector<double>& b)
{
  std::vector<GRBLinExpr> result;
  for (int i = 0; i < x.size(); i++)
  {
    GRBLinExpr tmp = x[i] - b[i];
    result.push_back(tmp);
  }
  return result;
}

template <typename T>
std::vector<T> eigenVector2std(const Eigen::Matrix<T, -1, 1>& x)
{
  std::vector<T> result = 0;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

template <typename T>
std::vector<std::vector<T>> eigenMatrix2std(const Eigen::Matrix<T, -1, -1>& x)
{
  std::vector<std::vector<T>> result;

  for (int i = 0; i < x.rows(); i++)
  {
    std::vector<T> row;
    for (int j = 0; j < x.cols(); j++)
    {
      row.push_back(x(i, j));
    }
    result.push_back(row);
  }
  return result;
}

template <typename T>
std::vector<T> GetColumn(std::vector<std::vector<T>> x, int column)
{
  std::vector<T> result;

  for (int i = 0; i < x.size(); i++)
  {
    result.push_back(x[i][column]);
  }
  return result;
}

#endif


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

#include <fstream>
#include <ctime>

#include <helib/helib.h>
#include <helib/ArgMap.h>
#include <helib/debugging.h>

#include <NTL/BasicThreadPool.h>

#include "common.h"

struct CmdLineOpts
{
  std::string paramFileName;
  std::string outputPrefixPath;
  std::string scheme = "BGV";
  std::string bootstrappable = "NONE"; // NONE | THIN | FAT
  bool noSKM = false;
  bool frobSKM = false;
  bool infoFile = false;
};

// Captures parameters of both BGV and CKKS
struct ParamsFileOpts
{
  long m = 0;
  long p = 0;
  long r = 0;
  long c = 0;
  long Qbits = 0;
  long scale = 4;
  long c_m = 100;
  NTL::Vec<long> mvec;
  NTL::Vec<long> gens;
  NTL::Vec<long> ords;
};

// Write context.printout to file.out
void printoutToStream(const helib::Context& context,
                      std::ostream& out,
                      bool noSKM,
                      bool frobSKM,
                      bool bootstrappable)
{
  if (!noSKM || bootstrappable)
    out << "Key switching matrices created.\n";
  if (frobSKM || bootstrappable)
    out << "Frobenius matrices created.\n";
  if (bootstrappable)
    out << "Recrypt data created.\n";

  // write the algebra info
  context.printout(out);
}

// sk is child of pk in HElib.
void writeKeyToFile(std::string& pathPrefix,
                    helib::Context& context,
                    helib::SecKey& secretKey,
                    bool pkNotSk)
{
  std::string path = pathPrefix + (pkNotSk ? ".pk" : ".sk");
  std::ofstream keysFile(path, std::ios::binary);
  if (!keysFile.is_open()) {
    std::runtime_error("Cannot write keys to file at '" + path);
  }

  // write the context
  helib::writeContextBaseBinary(keysFile, context);
  helib::writeContextBinary(keysFile, context);

  // write the keys
  if (pkNotSk)
    helib::writePubKeyBinary(keysFile, secretKey);
  else
    helib::writeSecKeyBinary(keysFile, secretKey);
}

int main(int argc, char* argv[])
{
  CmdLineOpts cmdLineOpts;

  // clang-format off
  helib::ArgMap()
        .toggle()
         .arg("--no-skm", cmdLineOpts.noSKM,
               "disable switch-key matrices.", nullptr)
         .arg("--frob-skm", cmdLineOpts.frobSKM,
               "generate Frobenius switch-key matrices.", nullptr)
         .arg("--info-file", cmdLineOpts.infoFile,
               "print algebra info to file.", nullptr)
        .separator(helib::ArgMap::Separator::WHITESPACE)
        .named()
          .arg("--scheme", cmdLineOpts.scheme,
               "choose scheme BGV | CKKS.")
          .arg("-o", cmdLineOpts.outputPrefixPath,
               "choose an output prefix path.", nullptr)
          .arg("--bootstrap", cmdLineOpts.bootstrappable,
               "choose boostrapping option NONE | THIN | THICK.")
        .required()
        .positional()
          .arg("<params-file>", cmdLineOpts.paramFileName,
               "the parameters file.", nullptr)
        .parse(argc, argv);
  // clang-format on

  ParamsFileOpts paramsOpts;

  try {
    // clang-format off
    helib::ArgMap()
          .arg("p", paramsOpts.p, "require p.", "")
          .arg("m", paramsOpts.m, "require m.", "")
          .arg("r", paramsOpts.r, "require r.", "")
          .arg("c", paramsOpts.c, "require c.", "")
          .arg("Qbits", paramsOpts.Qbits, "require Q bits.", "")
          .optional()
            .arg("scale", paramsOpts.scale, "require scale for CKKS")
            .arg("c_m", paramsOpts.c_m, "require c_m for bootstrapping.", "")
            .arg("mvec", paramsOpts.mvec, "require mvec for bootstrapping.", "")
            .arg("gens", paramsOpts.gens, "require gens for bootstrapping.", "")
            .arg("ords", paramsOpts.ords, "require ords for bootstrapping.", "")
          .parse(cmdLineOpts.paramFileName);
    // clang-format on
  } catch (const helib::RuntimeError& e) {
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  // Create the FHE context
  long p;
  if (cmdLineOpts.scheme.empty() || cmdLineOpts.scheme == "BGV") {
    if (paramsOpts.p < 2) {
      std::cerr << "BGV invalid plaintext modulus. "
                   "In BGV it must be a prime number greater than 1."
                << std::endl;
      return EXIT_FAILURE;
    }
    p = paramsOpts.p;
  } else if (cmdLineOpts.scheme == "CKKS") {
    if (paramsOpts.p != -1) {
      std::cerr << "CKKS invalid plaintext modulus. "
                   "In CKKS it must be set to -1."
                << std::endl;
      return EXIT_FAILURE;
    }
    p = -1;
    if (cmdLineOpts.bootstrappable != "NONE") {
      std::cerr << "CKKS does not currently support bootstrapping."
                << std::endl;
      return EXIT_FAILURE;
    }
  } else {
    std::cerr << "Unrecognized scheme '" << cmdLineOpts.scheme << "'."
              << std::endl;
    return EXIT_FAILURE;
  }

  if (cmdLineOpts.noSKM && cmdLineOpts.frobSKM) {
    std::cerr << "Frobenius matrices reqires switch-key matrices to be "
                 "generated."
              << std::endl;
    return EXIT_FAILURE;
  }

  if (cmdLineOpts.bootstrappable != "NONE" &&
      cmdLineOpts.bootstrappable != "THIN" &&
      cmdLineOpts.bootstrappable != "THICK") {
    std::cerr << "Bad boostrap option: " << cmdLineOpts.bootstrappable
              << ".  Allowed options are NONE, THIN, THICK." << std::endl;
    return EXIT_FAILURE;
  }

  if (cmdLineOpts.bootstrappable != "NONE") {
    if (cmdLineOpts.noSKM) {
      std::cerr << "Cannot generate bootstrappable context without switch-key "
                   "and frobenius matrices."
                << std::endl;
      return EXIT_FAILURE;
    }
    if (paramsOpts.mvec.length() == 0) {
      std::cerr << "Missing mvec parameter for bootstrapping in "
                << cmdLineOpts.paramFileName << "." << std::endl;
      return EXIT_FAILURE;
    }
    if (paramsOpts.gens.length() == 0) {
      std::cerr << "Missing gens parameter for bootstrapping in "
                << cmdLineOpts.paramFileName << "." << std::endl;
      return EXIT_FAILURE;
    }
    if (paramsOpts.ords.length() == 0) {
      std::cerr << "Missing ords parameter for bootstrapping in "
                << cmdLineOpts.paramFileName << "." << std::endl;
      return EXIT_FAILURE;
    }
  }

  try {
    helib::Context context(paramsOpts.m,
                           p,
                           paramsOpts.r,
                           helib::convert<std::vector<long>>(paramsOpts.gens),
                           helib::convert<std::vector<long>>(paramsOpts.ords));
    if (cmdLineOpts.bootstrappable == "NONE") {
      helib::buildModChain(context, paramsOpts.Qbits, paramsOpts.c);
    } else {
      context.zMStar.set_cM(paramsOpts.c_m / 100.0);
      helib::buildModChain(context,
                           paramsOpts.Qbits,
                           paramsOpts.c,
                           /*willBeBootstrappable=*/true);
      if (cmdLineOpts.bootstrappable == "THICK")
        context.makeBootstrappable(paramsOpts.mvec,
                                   /*skWht=*/0,
                                   /*build_cache=*/false,
                                   /*alsoThick=*/true);
      else if (cmdLineOpts.bootstrappable == "THIN")
        context.makeBootstrappable(paramsOpts.mvec,
                                   /*skWht=*/0,
                                   /*build_cache=*/false,
                                   /*alsoThick=*/false);
    }

    if (p == -1)
      context.scale = paramsOpts.scale;

    // and a new secret/public key
    helib::SecKey secretKey(context);
    secretKey.GenSecKey(); // A +-1/0 secret key

    // compute key-switching matrices
    if (!cmdLineOpts.noSKM || cmdLineOpts.bootstrappable != "NONE") {
      helib::addSome1DMatrices(secretKey);
      if (cmdLineOpts.frobSKM || cmdLineOpts.bootstrappable != "NONE") {
        helib::addFrbMatrices(secretKey);
      }
    }

    if (cmdLineOpts.bootstrappable != "NONE") {
      secretKey.genRecryptData();
    }

    // If not set by user, returns params file name with truncated UTC
    if (cmdLineOpts.outputPrefixPath.empty()) {
      cmdLineOpts.outputPrefixPath =
          stripExtension(cmdLineOpts.paramFileName) +
          std::to_string(std::time(nullptr) % 100000);
      std::cout << "File prefix: " << cmdLineOpts.outputPrefixPath << std::endl;
    }

    // Printout important info
    if (cmdLineOpts.infoFile) {
      // outputPrefixPath should be set further up main.
      std::string path = cmdLineOpts.outputPrefixPath + ".info";
      std::ofstream out(path);
      if (!out.is_open()) {
        throw std::runtime_error("Cannot write keys to file at '" + path +
                                 "'.");
      }
      printoutToStream(context,
                       out,
                       cmdLineOpts.noSKM,
                       cmdLineOpts.frobSKM,
                       cmdLineOpts.bootstrappable != "NONE");
    } else {
      printoutToStream(context,
                       std::cout,
                       cmdLineOpts.noSKM,
                       cmdLineOpts.frobSKM,
                       cmdLineOpts.bootstrappable != "NONE");
    }

    NTL::SetNumThreads(2);

    NTL_EXEC_INDEX(2, skOrPk)
    writeKeyToFile(cmdLineOpts.outputPrefixPath, context, secretKey, skOrPk);
    NTL_EXEC_INDEX_END

  } catch (const std::invalid_argument& e) {
    std::cerr << "Exit due to invalid argument thrown:\n"
              << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const helib::IOError& e) {
    std::cerr << "Exit due to IOError thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::runtime_error& e) {
    std::cerr << "Exit due to runtime error thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::logic_error& e) {
    std::cerr << "Exit due to logic error thrown:\n" << e.what() << std::endl;
    return EXIT_FAILURE;
  } catch (const std::exception& e) {
    std::cerr << "Exit due to unknown exception thrown:\n"
              << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2014          LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014          NumScale SAS
//
//          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
//==============================================================================
#include <boost/numeric/odeint.hpp>
#include <nt2/table.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/ones.hpp>

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_nt2_copy

#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/numeric/odeint/external/nt2/nt2_norm_inf.hpp>

#include <boost/mpl/list.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef boost::mpl::list< float , double > fp_types;

BOOST_AUTO_TEST_SUITE( nt2_norm_inf )

BOOST_AUTO_TEST_CASE_TEMPLATE( test_norm_inf, T, fp_types )
{
  nt2::table<T> x = nt2::ones(10,1, nt2::meta::as_<T>() );
  x(4) = 55;

  nt2::table<T> y = nt2::zeros(8,8, nt2::meta::as_<T>() );
  y(6,4) = -42;

  BOOST_CHECK_SMALL(vector_space_norm_inf<nt2::table<T> >()(x) - T(55), T(1e-10));
  BOOST_CHECK_SMALL(vector_space_norm_inf<nt2::table<T> >()(y) - T(42), T(1e-10));
}

BOOST_AUTO_TEST_SUITE_END()


// Authors: Frerk Saxen and Philipp Werner (Frerk.Saxen@ovgu.de, Philipp.Werner@ovgu.de)
// License: BSD 2-Clause "Simplified" License (see LICENSE file in root directory)

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/opencv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/videoio/videoio.hpp>
#include <fstream>
#include <string>
#include <chrono>
#include "misc.hpp"

//using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

template <long num_filters, typename SUBNET> using con5d = con<num_filters, 5, 5, 2, 2, SUBNET>;
template <long num_filters, typename SUBNET> using con5 = con<num_filters, 5, 5, 1, 1, SUBNET>;

template <typename SUBNET> using downsampler = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16, SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5 = relu<affine<con5<45, SUBNET>>>;

using net_type = loss_mmod<con<1, 9, 9, 1, 1, rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

void detectFace(const std::string& exdata_dir, const std::string& train_or_val_or_test)
{
	std::chrono::steady_clock::time_point time_start = std::chrono::steady_clock::now();

	std::string filename_list_filename = exdata_dir + train_or_val_or_test + "_filenames.txt";
	std::string filename_face_detection = exdata_dir + train_or_val_or_test + "_facedet.txt";
	std::string net_filename = exdata_dir + "mmod_human_face_detector.dat";

	std::vector<std::string> filename_list;
	misc::read_filename_list(filename_list_filename, filename_list);

	net_type net;
	deserialize(net_filename) >> net;
	long nrVid = 0;
	long nrError = 0;

	// Open detection filename
	std::ofstream detFile(filename_face_detection);
	CV_Assert(detFile.is_open());

	for(const auto& filename : filename_list)
	{
		try
		{
		    std::chrono::steady_clock::time_point time_end = std::chrono::steady_clock::now();
		    int seconds_expired = std::chrono::duration_cast<std::chrono::seconds>(time_end - time_start).count();
		    int minutes_left = nrVid == 0 ? 0 : seconds_expired * (static_cast<double>(filename_list.size() - nrVid) / (double)nrVid / 60.0);

		    // Open video
		    std::cout << rpad(cast_to_string(nrVid+1),3) << "/" << filename_list.size() << ". Approx. " << rpad(cast_to_string(minutes_left), 3) << " minutes left. " << "Processing video: " << filename;
		    cv::VideoCapture vid(filename);
		    CV_Assert(vid.isOpened());

		    cv::Mat cvBGRImg;
		    dlib::matrix<dlib::rgb_pixel> dlibRGBImg;
		    std::vector<dlib::matrix<dlib::rgb_pixel>> images;
		    dlib::rectangle det;
		    long frameCnt = 0;
		    bool vid_read = vid.read(cvBGRImg);
		    while (vid_read || !images.empty())
		    {
			    if (vid_read)
			    {
				    // Copy image to correct format
				    dlib::assign_image(dlibRGBImg, dlib::cv_image<dlib::bgr_pixel>(cvBGRImg));
				    images.push_back(std::move(dlibRGBImg));
				    vid_read = vid.read(cvBGRImg);

				    if (images.size() < 15)
					    continue;
			    }

			    // Get detections
			    auto detectionList = net(images);

			    for (auto&& dets : detectionList)
			    {
				    if (dets.empty())
				    {
					    det = dlib::rectangle();
				    }
				    else
				    {
					    // Sort detection scores in ascending order
					    std::sort(dets.begin(), dets.end(), [](const mmod_rect& left, const mmod_rect& right) {return(right.detection_confidence < left.detection_confidence); });
					    // The first detection has the highest score.
					    det = dets.at(0).rect;
				    }

				    // Write detection to file (x, y, width, height)
				    detFile << nrVid << "," << frameCnt << "," << det.left() << "," << det.top() << "," << det.width() << "," << det.height() << std::endl;

    // 				win.clear_overlay();
    // 				win.set_image(dlibBGRImg);
    // 				win.add_overlay(det);

				    frameCnt++;
			    }
			    images.clear();
		    }
		    std::cout << " with " << frameCnt << "frames. Done." << std::endl;
		    ++nrVid;
		}
		catch(const std::exception& e)
		{
		    std::cout << "\n-----------------------------------------------------------------------\n";
		    std::cout << "Error at vid nr: " << nrVid << std::endl;
		    std::cout << e.what() << std::endl;
		    std::cout << "-----------------------------------------------------------------------\n\n";
		    ++nrVid;
		    ++nrError;
		}
		catch(...)
		{
		    std::cout << "\n-----------------------------------------------------------------------\n";
		    std::cout << "Error at vid nr: " << nrVid << std::endl;
		    std::cout << "-----------------------------------------------------------------------\n\n";
		    ++nrVid;
		    ++nrError;
		}
	}

	std::cout << "Finished. Number of Errors: " << nrError << std::endl;
	detFile.close();
}


/*
 * fs_lwe_sign.cpp
 *
 *  Created on: 16.10.2011
 *      Author: Wladimir Paulsen
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vector>
#include <algorithm>

#include <openssl/sha.h>
#include <openssl/evp.h>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/ZZ_pE.h>
#include <NTL/ZZ_pX.h>

#include "aux.h"
#include "crypto_sign.h"
#include "fs_lwe_sign.h"
#include "crypto_sign.h"
#include "sizes.h"

NTL_CLIENT

/**
 * Look-up table for random oracle algorithm.
 */

NTL::ZZ L1_elements[] = {
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,0),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,1),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,2),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,3),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,4),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,5),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,6),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,7),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,8),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,9),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,10),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,11),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,12),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,13),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,14),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,15),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,16),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,17),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,18),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,19),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,20),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,21),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,22),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,23),
	count_elements_up_to_L1(PUBLIC_PARAMETER_N,24)
};
/*
// how many bits in higher byte needed to get a random degree, if degree of reduction polynomial > 256
const unsigned char BITS_HIGH_BYTE = log2(PUBLIC_PARAMETER_N) - 8;
// what is optimal size for splitting char array, so that there is no need for bit shift over chars
const unsigned char BLOCKSIZE = (8 - 8%(BITS_HIGH_BYTE+1))/(BITS_HIGH_BYTE+1);
// how many bytes are needed to get a random number in range of Dc
const unsigned int const_BYTES_NUMBER = ceil(ceil(log2(to_double(L1_elements[PUBLIC_PARAMETER_K])))/8);
// how many bytes are needed to get a random number in range of Dc
const unsigned int const_BITS_NUMBER = int(ceil(log2(to_double(L1_elements[PUBLIC_PARAMETER_K])))) % 8;

*/

/**
 * Generate secret key for FS_LWE-Schema.
 * @param PublicParam 	Public security parameter.
 * @param secret_key	Pointer to array of polynomials, where to store generated secret key.
 * @return 				0 if successful, else -1.
 */
int generate_sk(PublicParameter PublicParam, ZZ_pE *secret_key){

	if(PublicParam.m < 1||PublicParam.sigma < 1||secret_key == NULL){
		return -1;
	}

	for(int i = 0; i < PublicParam.m; i++){
		secret_key[i] = random_element_R_with_Linf(to_ZZ(PublicParam.sigma+1));
	}
	return EXIT_SUCCESS;
}

/**
 *	Generate random hash function for FS_LWE-Schema.
 * @param random_hash_function	Pointer to array of polynomials, where to store generated random hash polynomials.
 * @param number_of_ZZ_pE		Number of polynomials to generate.
 * @return	0 if successful, else -1.
 */
int generate_random_hash_function(ZZ_pE *random_hash_function, int number_of_ZZ_pE){
	if(number_of_ZZ_pE <1 || random_hash_function == NULL){
		return -1;
	}

	for(int i = 0; i < number_of_ZZ_pE; i++){
		random_hash_function[i] = random_ZZ_pE();
	}
	return EXIT_SUCCESS;
}

/**
 *	Convert deterministically a char-array into an element of Dc.
 *	Split char array in sections with size of BLOCKSIZE, to get optimal use of chars.
 *	Then get random power and raise coefficient, up to settled number of coefficients.
 *	Finally fill up settled powers to L1_value.
 * @param num_coeff			Number of settled coefficients in generated polynomial.
 * @param L1_value			L1-norm of generated polynomial.
 * @param charArray			Array with chars from message digest.
 * @param charArraySize		Number of chars in array.
 * @return	Generated element of ring Dc.
 */
ZZ_pE generate_element_Dc(ZZ num_coeff, ZZ L1_value, unsigned char* charArray, unsigned int charArraySize){

	unsigned int randomPower, bit_length;
	unsigned char koef=0,index_byte_block=0, index_byte_in_block=0, randomSettledPower, high_byte;

	ZZ_pX temp_res = ZZ_pX();
	std::vector<unsigned int> setted_coefficients;

	// while L1(element) != L1_value && inside of char array
	while((L1_norm_ZZ_pX(temp_res) != L1_value)&&((unsigned int)((BLOCKSIZE +1)*index_byte_block+index_byte_in_block) < charArraySize)){
		// While number of elements coefficients < num_coeff
		while((num_of_coeff_not_0(temp_res) < num_coeff)&&((unsigned int)((BLOCKSIZE +1)*index_byte_block+index_byte_in_block) < charArraySize)){

			if(index_byte_in_block%BLOCKSIZE == 0){
				koef = charArray[index_byte_block*(BLOCKSIZE +1)];	// byte with highByte's and coefficients
				index_byte_block++;
				index_byte_in_block = 0;
			}

			high_byte = (koef>>((BITS_HIGH_BYTE+1)*index_byte_in_block))&((1<<BITS_HIGH_BYTE) - 1);
			randomPower = charArray[index_byte_block*(BLOCKSIZE +1)+index_byte_in_block]+high_byte*(1<<8);

			if(std::find(setted_coefficients.begin(), setted_coefficients.end(), randomPower) == setted_coefficients.end()){
				// element is not in set
				setted_coefficients.push_back(randomPower);
				if((koef>>((BITS_HIGH_BYTE+1)*(index_byte_in_block+1)-1))&1){
					SetCoeff(temp_res,randomPower, 1);
				}else{
					SetCoeff(temp_res,randomPower,(-1));
				}
			}
			index_byte_in_block++;
		}

		if((L1_value > num_coeff)&&(L1_norm_ZZ_pX(temp_res) < L1_value)){	// raise one coefficient

			bit_length = (int)ceil(log2(setted_coefficients.size()));
			randomSettledPower = (charArray[index_byte_block*(BLOCKSIZE +1)+index_byte_in_block])&((1<<bit_length) - 1);

			if(randomSettledPower < setted_coefficients.size()){
				ZZ_p coeff_old = coeff(temp_res, setted_coefficients.at(randomSettledPower));

				if(2*rep(coeff_old) > coeff_old.modulus()){		// get sign of old coefficient
					SetCoeff(temp_res, setted_coefficients.at(randomSettledPower),coeff_old-1);
				}else{
					SetCoeff(temp_res, setted_coefficients.at(randomSettledPower),coeff_old+1);
				}
			}
		}
		index_byte_in_block++;
	}

	return to_ZZ_pE(temp_res);
}

/**
 * Test input parameter and return a prime p with length (2*sigma +1)^m*(2^(-128/2)).
 * @param PublicParam 	Public security parameter (n, m, sigma, k)
 * @return	A (probably) prime number with bitlength m*(ceil(log2(sigma))+1).
 */
ZZ test_input_parameter(PublicParameter PublicParam){
	ZZ binomial, p, tmp;

	long pow;
	binomial =  ZZ_binomial(to_ZZ(PublicParam.n),to_ZZ(PublicParam.k));

	if(PublicParam.n <= 0 || PublicParam.m <= 0 || PublicParam.sigma <= 0 || PublicParam.m <= 0||(PublicParam.n&(PublicParam.n-1))!= 0||(PublicParam.k +(log(binomial)/log(2)))< 160){
		printf("Some of input parameters is smaller as 0!\n");
		return ZZ::zero();
	}

	// test if n is power of 2
	if((PublicParam.n&(PublicParam.n-1))!= 0){
		printf("Error, n isn't power of 2!");
		return ZZ::zero();
	}

	/* take care that (k + log2((n over k))) >= 160 Bit */
	if((PublicParam.k +(log(binomial)/log(2)))< 160){
		printf("Error: Value k is to small!\n");
		return to_ZZ(0);
	}

	tmp = 2*power(to_ZZ(2*PublicParam.sigma+1), PublicParam.m);
	pow = log(tmp)/log(2);
	p = GenPrime_ZZ(pow,80);

	return p;
}

/**
 * Compute LWE-Hash by multiplying message with hash polynomials and add to results.
 * @param PublicParam		Public security parameter-
 * @param hash_function		Pointer to array of hash polynomials.
 * @param message			Pointer to array of polynomials to hash.
 * @param number_of_ZZ_pE	Number of polynomials in array.
 * @return					Hash-value as polynomial.
 */
ZZ_pE LWE_hash(PublicParameter PublicParam, ZZ_pE *hash_function, ZZ_pE *message, int number_of_ZZ_pE){
	ZZ_pE result = ZZ_pE::zero();
	ZZ_pE product = ZZ_pE::zero();

    // multiply message blocks with hash polynomials and add to result
    for (int i = 0; i < number_of_ZZ_pE; ++i) {
    	mul(product,message[i],hash_function[i]);
    	add(result,result,product);
	}
    return result;
}

/**
 * Generate (m) random polynomials from Dy, whose Linf-norm is <= n*m*k*sigma.
 * @param PublicParam	Public security parameter.
 * @param randomDy		Pointer to array, where to store generated polynomials.
 * @return	0 if successful, else -1.
 */
int random_element_Dy(PublicParameter PublicParam, ZZ_pE* randomDy){
	if(randomDy == NULL){
		return -1;
	}

	for(int i = 0; i < PublicParam.m; i++){
		randomDy[i] = random_element_R_with_Linf(to_ZZ(PublicParam.k*PublicParam.m*PublicParam.n*PublicParam.sigma));
	}
	return EXIT_SUCCESS;
}

/**
 *	Take as input a random hash function and a message and put an element from Dc out.
 *	This function should act as random oracle.
 * @param PublicParam		Public security parameter.
 * @param LWE_hash_rand		LWE-hash value of random element.
 * @param message			Pointer to message array of unsigned char.
 * @param message_length	Length of message array.
 * @return					Random element of ring Dc.
 */
ZZ_pE RandomOracle_Dc(
		PublicParameter 	PublicParam,
		ZZ_pE 				LWE_hash_rand,
		unsigned char* 		message,
		unsigned long long 	message_length
){

	unsigned char md_value[EVP_MAX_MD_SIZE];	// message digest
	unsigned char* char_arr;					// char-array for converted lwe-hash-value
	unsigned long long length_char_arr;
	int L1_norm=0, num_coeff=0;
	ZZ number;
	unsigned char run_index = 0;

	// convert LWE-Hash into char-array for hash-function
	char_arr =  convert_ZZ_pE_array_to_char_array(&LWE_hash_rand, 1, &length_char_arr);

	// copy message into a new array
	unsigned char* message_with_lwe = (unsigned char*)malloc(sizeof(char)*(size_t)(message_length + length_char_arr+1));

	if(NULL == message_with_lwe){
		printf("Couldn't reserve memory, abort!\n");
		return ZZ_pE::zero();
	}

	memcpy(message_with_lwe, char_arr,(size_t)length_char_arr);
	memcpy(&message_with_lwe[length_char_arr], message,(size_t)message_length);

	do{
		// append index to the end of array and hash the array
		memcpy(&message_with_lwe[message_length+ length_char_arr],(char *) &run_index, 1);
		SHA512(message_with_lwe, message_length + length_char_arr +1, md_value);

		md_value[const_BYTES_NUMBER-1] &= ((1<<const_BITS_NUMBER)-1);		// clear first bits to get a number in range Dc

		number = ZZFromBytes(md_value, const_BYTES_NUMBER);	// 8*20+2=162-Bit with
		run_index++;

	}while(number > L1_elements[PUBLIC_PARAMETER_K]);

	// get L1-Length
	for(int i = PublicParam.k; i>= 1;i--){
		if((L1_elements[i-1] < number) && (L1_elements[i] > number)){
			L1_norm = i;
			break;
		}
	}

	number -= L1_elements[L1_norm-1];
	ZZ sum_elements = L1_elements[L1_norm]-L1_elements[L1_norm-1];
	ZZ temp2;
	// get number of coefficients <= L1-length
	for (int index_num_coeff = L1_norm; index_num_coeff >= 1; index_num_coeff--){
		temp2 = count_elements_with_L1_and_coeffs(PublicParam.n, L1_norm, index_num_coeff);
		if((sum_elements - temp2 <= number) && (sum_elements > number)){
			num_coeff = index_num_coeff;
			break;
		}
		sum_elements -= temp2;
	}

	// Convert 512-Bit String (md_value) into an element of Dc
	ZZ_pE result =  generate_element_Dc(to_ZZ(num_coeff), to_ZZ(L1_norm), &md_value[const_BYTES_NUMBER], (unsigned int) EVP_MAX_MD_SIZE-const_BYTES_NUMBER);

	// delete temporary arrays and return a result
	free(char_arr);
	free(message_with_lwe);

	return result;
}

/**
 * Verify signature of message, given the message, signature and public key.
 * @param PublicParameter		Public security parameter.
 * @param message				Pointer to message array of unsigned chars.
 * @param SignaturePart_z		Pointer to array of polynomials, part z of signature.
 * @param SignaturePart_e		Pointer to array of polynomials, part e of signature.
 * @param HashPolynomials		Pointer to array of polynomials of LWE-hash function, part of public key.
 * @param PublicKey				LWE-hash of secret key, public key.
 * @param length_Sign_z			Length of polynomial array.
 * @param message_length		Length of message array.
 * @return						0 if successful, -1 else.
 */
int verify_signatur(
		PublicParameter PublicParameter,
		unsigned char* 	message,
		ZZ_pE* 			SignaturePart_z,
		ZZ_pE* 			SignaturePart_e,
		ZZ_pE* 			HashPolynomials,
		ZZ_pE 			PublicKey,
		int 			length_Sign_z,
		unsigned long long message_length
	){

	// is sign_z element of G^m ?
	ZZ max_length = to_ZZ(PublicParameter.sigma*PublicParameter.k*((PublicParameter.m*PublicParameter.n)-1));
	if(!is_element_with_max_Linf(max_length, SignaturePart_z, length_Sign_z)){
		printf("Verification failed, response is not an element of G^m! Abort!\n");
		return -1;
	}

	ZZ_pE hash_Sign_z = LWE_hash(PublicParameter, HashPolynomials, SignaturePart_z, length_Sign_z);
	ZZ_pE temp = hash_Sign_z - (*SignaturePart_e)*PublicKey;
	ZZ_pE temp_e = RandomOracle_Dc(PublicParameter,temp, message, message_length);

	return compare_ZZ_pE(SignaturePart_e,&temp_e,1);
}

/**
 *	Generate a Fiat-Shamir signature of message with given secret key.
 * @param PublicParam		Public security parameter.
 * @param message			Pointer to message array of unsigned chars.
 * @param message_length	Length of message array.
 * @param SecretKey			Pointer to array of polynomials of secret key.
 * @param HashPolynomials	Pointer to array of polynomials of LWE hash function.
 * @param SignaturePart_e	Pointer to signature part e.
 * @param SignaturePart_z	Pointer to signature part z.
 * @return					0 if successful, else -1.
 */
int sign_message(
		PublicParameter PublicParam,
		unsigned char* 	message,
		unsigned long long message_length,
		ZZ_pE* 			SecretKey,
		ZZ_pE* 			HashPolynomials,
		ZZ_pE* 			SignaturePart_e,
		ZZ_pE* 			SignaturePart_z
){
	ZZ max_length = to_ZZ(PublicParam.sigma*PublicParam.k*(PublicParam.n*PublicParam.m-1));
	ZZ_pE rand_Dy_hash;
	ZZ_pE random_Dy[PublicParam.m];

	do{
		random_element_Dy(PublicParam,random_Dy);

		rand_Dy_hash = LWE_hash(PublicParam, HashPolynomials, random_Dy, PublicParam.m);
		SignaturePart_e[0] = RandomOracle_Dc(PublicParam,rand_Dy_hash,message, message_length);

		for (int index = 0; index < PublicParam.m; ++index) {
			SignaturePart_z[index] = SecretKey[index]*SignaturePart_e[0] + random_Dy[index];
		}
	}while(!is_element_with_max_Linf(max_length, SignaturePart_z, PublicParam.m));

	/*
	ZZ_pE PK = LWE_hash(PublicParam, HashPolynomials, SecretKey, PublicParam.m);
	ZZ_pE Hz = rand_Dy_hash + PK * SignaturePart_e[0];
	return compare_ZZ_pE(&Hz, &rand_Dy_hash,1);
*/
	return 0;
}

/**
 * Generate a pair of secret and public key.
 * @param pk	Char array to store public key and polynomials of LWE hash function.
 * @param sk	Char array to store secret key and polynomials of LWE hash function.
 * @return	0 if successful, -1 else.
 */
int crypto_sign_keypair(unsigned char *pk, unsigned char *sk){

	bool result;
	struct PublicParameter PP = {PUBLIC_PARAMETER_N, PUBLIC_PARAMETER_M,PUBLIC_PARAMETER_SIGMA,PUBLIC_PARAMETER_K};

	unsigned long long length;

	NTL::ZZ prime_number = test_input_parameter(PP);
	if(prime_number == ZZ::zero()){
		return -1;
	}

	initialize_ZZ_pE(prime_number, PP.n);						// initialize ZZ_pE and ZZ_p
	NTL::ZZ_pE sk_ZZ[PP.m];
	NTL::ZZ_pE rhf[PP.m];
	result = generate_sk(PP,sk_ZZ);								// generate random secret key
	generate_random_hash_function(rhf, PP.m);					// generate random hash-function
	ZZ_pE pk_ZZ_pE = LWE_hash(PP, rhf, sk_ZZ, PP.m);			// compute public key

	// convert and copy main parameter to byte arrays pk = [p|rhf|pk] and sk = [p|rhf|sk]
	// prime number p
	NTL::BytesFromZZ(pk, prime_number, BYTES_PRIMENUMBER);
	NTL::BytesFromZZ(sk, prime_number, BYTES_PRIMENUMBER);
	// random hash function
	convert_ZZ_pE_array_to_char_array(rhf, PP.m, &length, &sk[BYTES_PRIMENUMBER]);
	memcpy(&pk[BYTES_PRIMENUMBER],&sk[BYTES_PRIMENUMBER],(size_t)length);
	// secret and public key
	convert_ZZ_pE_array_to_char_array(&pk_ZZ_pE, 1, &length, &pk[BYTES_PRIMENUMBER+BYTES_HASHFUNCTION]);
	convert_ZZ_pE_array_to_char_array(sk_ZZ, PP.m, &length, &sk[BYTES_PRIMENUMBER+BYTES_HASHFUNCTION]);

	return result;
}

/**
 * Generating a signed message sm[0],...,sm[*smlen-1] from original message m[0],...,m[mlen-1] under secret key sk[0],sk[1].
 *
 * @param sm	Char array to store signed message (message + signature).
 * @param smlen	Length of signed message.
 * @param m		Char array with message to sign.
 * @param mlen	Length of message.
 * @param sk	Pointer to secret key.
 * @return		0 if successful, -1 else.
 */
int crypto_sign(unsigned char *sm,unsigned long long *smlen, const unsigned char *m,unsigned long long mlen, const unsigned char *sk){
	struct PublicParameter PP = {PUBLIC_PARAMETER_N, PUBLIC_PARAMETER_M, PUBLIC_PARAMETER_SIGMA, PUBLIC_PARAMETER_K};

	unsigned long long length;

	*smlen = mlen + BYTES_SIGNATUR_E + BYTES_SIGNATUR_Z;
	// read prime p and initialize ZZ_pE and ZZ_p
	NTL::ZZ prime_number = NTL::ZZFromBytes(sk, BYTES_PRIMENUMBER);
	initialize_ZZ_pE(prime_number, PUBLIC_PARAMETER_N);

	NTL::ZZ_pE sign_e;
	NTL::ZZ_pE sign_z[PUBLIC_PARAMETER_M];
	NTL::ZZ_pE hash_polynomial[PUBLIC_PARAMETER_M];
	NTL::ZZ_pE sk_ZZ_pE[PUBLIC_PARAMETER_M];

	convert_char_array_to_ZZ_pE_array(hash_polynomial, (unsigned char*)&sk[BYTES_PRIMENUMBER], BYTES_HASHFUNCTION, PUBLIC_PARAMETER_M,PUBLIC_PARAMETER_N);
	convert_char_array_to_ZZ_pE_array(sk_ZZ_pE, (unsigned char*)&sk[BYTES_PRIMENUMBER+BYTES_HASHFUNCTION],BYTES_SECRETKEY,PUBLIC_PARAMETER_M,PUBLIC_PARAMETER_N);

	int result = sign_message(PP, (unsigned char*) m, mlen, sk_ZZ_pE, hash_polynomial, &sign_e, sign_z);
	/* copy message and signature */
	memcpy(sm, m, (size_t) mlen);
	convert_ZZ_pE_array_to_char_array(sign_z, PP.m, &length, sm+mlen);
	convert_ZZ_pE_array_to_char_array(&sign_e, 1, &length, sm+mlen+BYTES_SIGNATUR_Z);

	return result;
}

/**
 * verifying a signed message sm[0],...,sm[smlen-1] under public key pk and producing original message m[0],m[1],...,m[*mlen-1].
 * @param m		Char array with message.
 * @param mlen	Length of message.
 * @param sm	Char array with signed message.
 * @param smlen	Length of signed message.
 * @param pk	Pointer to public key.
 * @return		0 if successful, -1 else.
 */
int crypto_sign_open(unsigned char *m, unsigned long long *mlen, const unsigned char *sm,unsigned long long smlen, const unsigned char *pk){
	struct PublicParameter PP = {PUBLIC_PARAMETER_N, PUBLIC_PARAMETER_M,PUBLIC_PARAMETER_SIGMA,PUBLIC_PARAMETER_K};

	NTL::ZZ prime_number = NTL::ZZFromBytes(pk, BYTES_PRIMENUMBER);
	initialize_ZZ_pE(prime_number, PUBLIC_PARAMETER_N);				// initialize ZZ_pE and ZZ_p

	NTL::ZZ_pE hash_polynomial[PUBLIC_PARAMETER_M];
	NTL::ZZ_pE sign_z[PUBLIC_PARAMETER_M];
	NTL::ZZ_pE pk_ZZ_pE, sign_e;

	*mlen = smlen - BYTES_SIGNATUR_E - BYTES_SIGNATUR_Z;
	memcpy(m, sm, (size_t) *mlen);	// copy chars from signed message

	convert_char_array_to_ZZ_pE_array(hash_polynomial,(unsigned char*)&pk[BYTES_PRIMENUMBER], BYTES_HASHFUNCTION,PUBLIC_PARAMETER_M, PUBLIC_PARAMETER_N);
	convert_char_array_to_ZZ_pE_array(&pk_ZZ_pE,(unsigned char*)&pk[BYTES_PRIMENUMBER+BYTES_HASHFUNCTION],BYTES_PUBLICKEY,1,PUBLIC_PARAMETER_N);
	convert_char_array_to_ZZ_pE_array(sign_z,(unsigned char *) (sm+*mlen),BYTES_SIGNATUR_Z, PUBLIC_PARAMETER_M, PUBLIC_PARAMETER_N);
	convert_char_array_to_ZZ_pE_array(&sign_e,(unsigned char *) (sm+*mlen+BYTES_SIGNATUR_Z), BYTES_SIGNATUR_E, 1, PUBLIC_PARAMETER_N);

	return verify_signatur(PP, m, sign_z, &sign_e, hash_polynomial, pk_ZZ_pE, PUBLIC_PARAMETER_M, *mlen);
}


#include <Eigen/Dense>

#include "boundingbox.h"
#include "color.h"
#include "command.h"
#include "functions.h"
#include "numstring.h"
#include "part.h"
#include "renderer.h"
#include "mceschliz.h"

//using namespace BWCNC;

struct quickpoint {
    double x;
    double y;
};

// cheeks_atom
#define CHEEKSATOM_PNTCNT 15
static const quickpoint cheek[CHEEKSATOM_PNTCNT] = {
    { 0, 0 }, { 12, 14 }, { 12, 19 }, { -1, 29 }, { -3, 29 }, { -16, 23 }, { -18, 24 }, { -19, 25 }, { -19, 31 }, { -18, 32 }, { -17, 38 }, { -13, 38 }, { -10, 34 },  { -9, 34 }, { 0, 40 }
};

// toes_atom
#define TOESATOM_PNTCNT 16
static const quickpoint toes[TOESATOM_PNTCNT] = {
    { 0, 0 }, { 15, -3 }, { 13, 7 }, { 8, 13 }, { 10, 16 }, { 9, 18 }, { 6, 25 }, { 4, 27 }, { 0, 27 }, { -8, 26 }, { -13, 24 }, { -22, 16 }, { -20, 22 }, { -18, 25 }, { -16, 32 }, { 0, 40 }
};

// knees_atom
#define KNEESATOM_PNTCNT 14
static const quickpoint knees[KNEESATOM_PNTCNT] = {
    { 0, 0 }, { 5, 1 }, { 16, 4 }, { 26, 4 }, { 25, 7 }, { 25, 10 }, { 24, 11 }, { 23, 14 }, { 22, 16 }, { 4, 12 }, { 2, 12 }, { 0, 23 }, { -5, 30 }, { 0, 40 }
};

static BWCNC::Part * quickpoints2part( const quickpoint * points, const int listlen, bool reverse = false )
{
	BWCNC::Part * part = nullptr;
    const int firstpnt = reverse ? listlen - 1 : 0;
    const int lastpnt  = reverse ? -1 : listlen;
    const int stepsize = reverse ? -1 : 1;
    for( int i = firstpnt; i != lastpnt; i += stepsize )
    {
        const quickpoint * q = points + i;
        if( i == firstpnt )
	        part = new BWCNC::Part( Eigen::Vector3d( q->x, q->y, 0 ) );
        else
            part->lineto( Eigen::Vector3d( q->x, q->y, 0 ) );
    }
    return part;
}

BWCNC::Part * BWCNC::LizardGrid::make_cheek_atom( bool reverse ) { return quickpoints2part( cheek, CHEEKSATOM_PNTCNT, reverse ); }
BWCNC::Part * BWCNC::LizardGrid::make_toes_atom(  bool reverse ) { return quickpoints2part( toes,  TOESATOM_PNTCNT, reverse ); }
BWCNC::Part * BWCNC::LizardGrid::make_knees_atom( bool reverse ) { return quickpoints2part( knees, KNEESATOM_PNTCNT, reverse ); }

void BWCNC::LizardGrid::make_toe2toe_down_through_cheeks( BWCNC::PartContext & k, bool finish )
{
    BWCNC::Part * part = nullptr;

    part = make_toes_atom();
    part->rotate_deg( -60 );
    k.append_part( part );

    part = make_cheek_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    part = make_cheek_atom();
    part->rotate_deg( 60 );
    k.append_part( part );

    part = make_toes_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    if( finish )
    {
        part = make_toes_atom();
        part->rotate_deg( -60 );
        k.append_part( part );
    }
}

void BWCNC::LizardGrid::make_cheek2cheek_down_through_knees( BWCNC::PartContext & k, bool finish )
{
    BWCNC::Part * part = nullptr;

    part = make_cheek_atom();
    part->rotate_deg( -60 );
    k.append_part( part );

    part = make_knees_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    part = make_knees_atom();
    part->rotate_deg( 60 );
    k.append_part( part );

    part = make_cheek_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    if( finish )
    {
        part = make_cheek_atom();
        part->rotate_deg( -60 );
        k.append_part( part );
    }
}

void BWCNC::LizardGrid::make_knee2knee_down_through_toes( BWCNC::PartContext & k, bool finish )
{
    BWCNC::Part * part = nullptr;

    part = make_knees_atom();
    part->rotate_deg( -60 );
    k.append_part( part );

    part = make_toes_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    part = make_toes_atom();
    part->rotate_deg( 60 );
    k.append_part( part );

    part = make_knees_atom( true );
    part->rotate_deg( 180 );
    k.append_part( part );

    if( finish )
    {
        part = make_knees_atom();
        part->rotate_deg( -60 );
        k.append_part( part );
    }
}

static void addcolspacing( BWCNC::PartContext & k, Eigen::Vector3d & colstart )
{
    static double colspacing = 2 * 40 * ::cos(M_PI/6);
    colstart[0] += colspacing;
    BWCNC::Part * p = k.get_new_part();
    p->moveto( colstart );
    k.add_part( p );
}

static void addrowspacing( BWCNC::PartContext & k )
{
    Eigen::Vector3d pos(0,40,0);
    BWCNC::Part * p = k.get_new_part();
    p->moveto( pos, true );
    k.add_part( p );
}

void BWCNC::LizardGrid::fill_partctx_with_grid( BWCNC::PartContext & k )
{
    BWCNC::Part * p = nullptr;
    Eigen::Vector3d colstart(0,0,0);

    // create the long vertical lines
    for( int c = 0; c < m_cols; c++ )
    {
        for( int r = 0; r < m_rows; r++ ) { make_cheek2cheek_down_through_knees( k, (r + 1 == m_rows) ); }
        addcolspacing( k, colstart );

        for( int r = 0; r < m_rows; r++ ) { make_toe2toe_down_through_cheeks( k, (r + 1 == m_rows) ); }
        addcolspacing( k, colstart );

        for( int r = 0; r < m_rows; r++ ) { make_knee2knee_down_through_toes( k, (r + 1 == m_rows) ); }
        if( c < m_cols - 1 )
            addcolspacing( k, colstart );
    }

    colstart = Eigen::Vector3d(0,0,0);
    for( int c = 0; c < m_cols; c++ )
    {
        // moveto, back to top of second vertical
        // this batch connects the first two verticals (and it's cyclic)
        addcolspacing( k, colstart );
        for( int r = 0; r < m_rows; r++ )
        {
            p = make_toes_atom();
            p->rotate_deg( 60 );
            k.append_part( p );
            addrowspacing( k );

            p = make_knees_atom();
            p->rotate_deg( -60 );
            k.append_part( p );
            addrowspacing( k );
        }
        p = make_toes_atom(); // toes begin *and* end it
        p->rotate_deg( 60 );
        k.append_part( p );
        addrowspacing( k );

        // moveto, back to top of next vertical
        // this batch connects the second two verticals (and it's cyclic)
        addcolspacing( k, colstart );
        for( int r = 0; r < m_rows; r++ )
        {
            p = make_knees_atom();
            p->rotate_deg( 60 );
            k.append_part( p );
            addrowspacing( k );

            p = make_cheek_atom();
            p->rotate_deg( -60 );
            k.append_part( p );
            addrowspacing( k );
        }
        p = make_knees_atom();  // knees begin *and* end it
        p->rotate_deg( 60 );
        k.append_part( p );
        addrowspacing( k );

        if( c < m_cols - 1 || m_allow_dangling_crossings )
        {
            // moveto, back to top of next vertical
            // this batch connects the third two verticals (and it's cyclic)
            addcolspacing( k, colstart );
            for( int r = 0; r < m_rows; r++ )
            {
                p = make_cheek_atom();
                p->rotate_deg( 60 );
                k.append_part( p );
                addrowspacing( k );

                p = make_toes_atom();
                p->rotate_deg( -60 );
                k.append_part( p );
                addrowspacing( k );
            }
            p = make_cheek_atom();  // cheeks begin *and* end it
            p->rotate_deg( 60 );
            k.append_part( p );
            addrowspacing( k );
        }
    }
}


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

#include <boost/simd/arithmetic/functions/divceil.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/functions/simd/ceil.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/divs.hpp>
#include <boost/simd/include/functions/simd/minusone.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                                   , ((simd_<arithmetic_<A0>,X>))
                                     ((simd_<arithmetic_<A0>,X>))
                                   )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::ceil(boost::simd::tofloat(a0)/boost::simd::tofloat(a1)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                                   , ((simd_<unsigned_<A0>,X>))
                                     ((simd_<unsigned_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::rdivide(boost::simd::minusone(a0+a1), a1); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                                   , ((simd_<int16_<A0>,X>))
                                     ((simd_<int16_<A0>,X>))
                                   )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype,X>                       ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h );
      boost::simd::split(a1, a1l, a1h );
      return boost::simd::bitwise_cast<A0>(
        boost::simd::group(boost::simd::divceil(a0l, a1l),
                           boost::simd::divceil(a0h, a1h))
      );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                                   , ((simd_<int8_<A0>,X>))
                                     ((simd_<int8_<A0>,X>))
                                   )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type           stype;
      typedef typename dispatch::meta::upgrade<stype>::type          itype;
      typedef simd::native<itype, X>                      ivtype;
      ivtype a0l, a0h, a1l, a1h;
      boost::simd::split(a0, a0l, a0h );
      boost::simd::split(a1, a1l, a1h );
      return boost::simd::bitwise_cast<A0>(
        boost::simd::group(boost::simd::divceil(a0l, a1l),
                           boost::simd::divceil(a0h, a1h) )
      );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divceil_, tag::cpu_, (A0)(X)
                                   , ((simd_<floating_<A0>,X>))
                                     ((simd_<floating_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::ceil(a0/a1); }
  };
} } }

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_ALGEBRA_FIELD_UTILS_HPP
#define CRYPTO3_ALGEBRA_FIELD_UTILS_HPP

#include <type_traits>
#include <complex>

#include <boost/math/constants/constants.hpp>
#include <nil/crypto3/algebra/fields/params.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {
            namespace detail {

                using namespace nil::crypto3::algebra;

                std::size_t bitreverse(std::size_t n, const std::size_t l) {
                    std::size_t r = 0;
                    for (std::size_t k = 0; k < l; ++k) {
                        r = (r << 1) | (n & 1);
                        n >>= 1;
                    }
                    return r;
                }

                constexpr std::size_t get_power_of_two(std::size_t n) {
                    n--;
                    n |= n >> 1;
                    n |= n >> 2;
                    n |= n >> 4;
                    n |= n >> 8;
                    n |= n >> 16;
                    n++;

                    return n;
                }

                template<typename FieldType>
                typename FieldType::value_type coset_shift() {
                    return
                        typename FieldType::value_type(fields::arithmetic_params<FieldType>::multiplicative_generator)
                            .squared();
                }

            }    // namespace detail
        }        // namespace fft
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_FIELD_UTILS_HPP


#include <btcm/chain/protocol/asset.hpp>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace btcm { namespace chain {
      typedef boost::multiprecision::int128_t  int128_t;

      uint8_t asset::decimals()const {
         return BTCM_ASSET_PRECISION;
      }

      string asset::to_string()const {
         int64_t init_digits=amount.value/(precision());
         uint64_t fraction=amount.value%(precision());
         string output=std::to_string(init_digits);
         output=output+"."+std::to_string(precision() + fraction).erase(0,1);
         object_id_type id_t(asset_id);
         output = output +" "+ std::string(id_t);
         return output;
      }

      asset asset::from_string(const string& from) {
         int64_t amount;
         string s = fc::trim( from );
         auto dot_pos = s.find( "." );
         auto space_pos = s.find( " " );
         FC_ASSERT ( space_pos != std::string::npos );
         if(space_pos < dot_pos) { //no dot in the first part
            auto intpart = s.substr( 0, space_pos );
            amount = fc::to_int64(intpart)*static_precision();
         }else{

            auto intpart = s.substr( 0, dot_pos );
            amount = fc::to_int64(intpart)*static_precision();

            std::string fractpart = s.substr( dot_pos+1, std::min<size_t>(space_pos-dot_pos-1, BTCM_ASSET_PRECISION));
            while (fractpart.size() < BTCM_ASSET_PRECISION)
               fractpart+='0';

            uint64_t fract_amount = fc::to_int64(fractpart);

            amount = amount + fract_amount;
         }

         auto asset_id_s = s.substr( space_pos+1 );
         auto first_dot = asset_id_s.find('.');
         auto second_dot = asset_id_s.find('.',first_dot+1);
         FC_ASSERT( first_dot != second_dot );
         FC_ASSERT( first_dot != 0 && first_dot != std::string::npos );
         uint64_t number = fc::to_uint64(asset_id_s.substr( second_dot+1 ));

         FC_ASSERT( number <= GRAPHENE_DB_MAX_INSTANCE_ID );

         return asset(amount, asset_id_type(number));

      }

      share_type asset::scaled_precision( uint8_t decimals ){
         FC_ASSERT(decimals<19);
         share_type res=1;
         for (int i=0; i< decimals; i++)
            res*=10;
         return res;
      }

      int64_t asset::static_precision(){
         return 1000000;
      }

      int64_t asset::precision()const {
         return 1000000;
      }

      bool operator == ( const price& a, const price& b )
      {
         if( std::tie( a.base.asset_id, a.quote.asset_id ) != std::tie( b.base.asset_id, b.quote.asset_id ) )
             return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult == bmult;
      }

      bool operator < ( const price& a, const price& b )
      {
         if( a.base.asset_id < b.base.asset_id ) return true;
         if( a.base.asset_id > b.base.asset_id ) return false;
         if( a.quote.asset_id < b.quote.asset_id ) return true;
         if( a.quote.asset_id > b.quote.asset_id ) return false;

         const auto amult = uint128_t( b.quote.amount.value ) * a.base.amount.value;
         const auto bmult = uint128_t( a.quote.amount.value ) * b.base.amount.value;

         return amult < bmult;
      }

      bool operator <= ( const price& a, const price& b )
      {
         return (a == b) || (a < b);
      }

      bool operator != ( const price& a, const price& b )
      {
         return !(a == b);
      }

      bool operator > ( const price& a, const price& b )
      {
         return !(a <= b);
      }

      bool operator >= ( const price& a, const price& b )
      {
         return !(a < b);
      }

      asset operator * ( const asset& a, const price& b )
      {
         if( a.asset_id == b.base.asset_id )
         {
            FC_ASSERT( b.base.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.quote.amount.value)/b.base.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.quote.asset_id );
         }
         else if( a.asset_id == b.quote.asset_id )
         {
            FC_ASSERT( b.quote.amount.value > 0 );
            uint128_t result = (uint128_t(a.amount.value) * b.base.amount.value)/b.quote.amount.value;
            FC_ASSERT( result.hi == 0 );
            return asset( result.to_uint64(), b.base.asset_id );
         }
         FC_THROW_EXCEPTION( fc::assert_exception, "invalid asset * price", ("asset",a)("price",b) );
      }

      price operator / ( const asset& base, const asset& quote )
      { try {
         FC_ASSERT( base.asset_id != quote.asset_id );
         return price{ base, quote };
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

      price price::max( asset_id_type base, asset_id_type quote ) { return asset( share_type(BTCM_MAX_SHARE_SUPPLY), base ) / asset( share_type(1), quote); }
      price price::min( asset_id_type base, asset_id_type quote ) { return asset( 1, base ) / asset( BTCM_MAX_SHARE_SUPPLY, quote); }

      bool price::is_null() const { return *this == price(); }

      void price::validate() const
      { try {
         FC_ASSERT( base.amount > share_type(0) );
         FC_ASSERT( quote.amount > share_type(0) );
         FC_ASSERT( base.asset_id != quote.asset_id );
      } FC_CAPTURE_AND_RETHROW( (base)(quote) ) }

} } // btcm::chain


#define EIGEN_USE_MKL_ALL

#include <iostream>

#include <Eigen/Core>

#include "models/transformers/autoencoder.hpp"
#include "models/transformers/sparse_autoencoder.hpp"
#include "models/transformers/tied_autoencoder.hpp"
#include "models/transformers/sparse_tied_autoencoder.hpp"
#include "utils/optimizers/stochastic_gradient_descent.hpp"
#include "utils/activation_functions.hpp"

#include "../misc.hpp"

void autoencoder_examples() {
	print_info();
	std::cout << std::endl;

	auto features = 10;
	auto samples = 2000;
	auto hidden = 4;
	auto output = 10;

	Eigen::MatrixXd input = (Eigen::MatrixXd::Random(features, samples).array() + 1) / 2;
	Eigen::MatrixXd coeffs = Eigen::MatrixXd::Random(output, features) * 4 / std::sqrt(6.0 / (hidden + features));
	Eigen::VectorXd encoderCoeffs = Eigen::VectorXd::Random(hidden * features + hidden + features * hidden + features) * 4 / std::sqrt(6.0 / (hidden + features));
	Eigen::VectorXd tiedEncoderCoeffs = Eigen::VectorXd::Random(hidden * features + hidden + features) * 4 / std::sqrt(6.0 / (hidden + features));

	using activation_t = mlt::utils::activation_functions::SigmoidActivation;
	using optimizer_t = mlt::utils::optimizers::StochasticGradientDescent<>;

	optimizer_t sgd(1, 20);
	activation_t activation;

	auto autoencoder1 = mlt::models::transformers::create_autoencoder(hidden, activation, activation, sgd, 3e-3);
	eval_numerical_gradient(autoencoder1, encoderCoeffs, input, input);
	std::cin.get();

	auto autoencoder2 = mlt::models::transformers::create_sparse_autoencoder(hidden, activation, activation, sgd, 3e-3, 0.1, 3);
	eval_numerical_gradient(autoencoder2, encoderCoeffs, input, input);
	std::cin.get();

	auto autoencoder3 = mlt::models::transformers::create_tied_autoencoder(hidden, activation, activation, sgd, 3e-3);
	eval_numerical_gradient(autoencoder3, tiedEncoderCoeffs, input, input);
	std::cin.get();

	auto autoencoder4 = mlt::models::transformers::create_sparse_tied_autoencoder(hidden, activation, activation, sgd, 3e-3, 0.1, 3);
	eval_numerical_gradient(autoencoder4, tiedEncoderCoeffs, input, input);
	std::cin.get();

	autoencoder1.fit(input);
	autoencoder1.fit(input, false);

	std::cin.get();

	autoencoder2.fit(input);
	autoencoder2.fit(input, false);

	std::cin.get();

	autoencoder3.fit(input);
	autoencoder3.fit(input, false);

	std::cin.get();

	autoencoder4.fit(input);
	autoencoder4.fit(input, false);

	std::cin.get();
}


#include "precipitation.h"
#include "../basic.h"

#include <random>

#include <boost/log/trivial.hpp>
#include <boost/random.hpp>

#include <OpenSimplexNoise.h>

namespace WorldEngine
{

static void PrecipitationCalculation(World& world, uint32_t seed);

void PrecipitationSimulation(World& world, uint32_t seed)
{
   BOOST_LOG_TRIVIAL(info) << "Precipitation simulation start";

   const OceanArrayType&         ocean = world.GetOceanData();
   const PrecipitationArrayType& p     = world.GetPrecipitationData();

   PrecipitationCalculation(world, seed);

   world.SetThreshold(PrecipitationLevel::Low,
                      FindThresholdF(p, 0.75f, &ocean));
   world.SetThreshold(PrecipitationLevel::Medium,
                      FindThresholdF(p, 0.3f, &ocean));
   world.SetThreshold(PrecipitationLevel::High, 0.0f);

   BOOST_LOG_TRIVIAL(info) << "Precipitation simulation finish";
}

static void PrecipitationCalculation(World& world, uint32_t seed)
{
   BOOST_LOG_TRIVIAL(debug) << "Seed: " << seed;

   // Precipitation is a value in [-1, 1]
   std::mt19937                                      generator(seed);
   boost::random::uniform_int_distribution<uint32_t> distribution(0,
                                                                  UINT32_MAX);

   OpenSimplexNoise::Noise noise(distribution(generator));

   const int32_t width  = world.width();
   const int32_t height = world.height();
   const int32_t border = width / 4;

   float curveGamma = world.gammaCurve();
   float curveBonus = world.curveOffset();

   const TemperatureArrayType& temperature   = world.GetTemperatureData();
   PrecipitationArrayType&     precipitation = world.GetPrecipitationData();
   precipitation.resize(boost::extents[height][width]);

   uint32_t octaves = 6;
   float    freq    = 64.0f * octaves;
   float    nScale  = 1024.0f / static_cast<float>(height);

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         double n = Noise(noise, //
                          x * nScale / freq,
                          y * nScale / freq,
                          octaves);

         if (x <= border)
         {
            n *= static_cast<double>(x) / border;
            n += Noise(noise,
                       (x * nScale + width) / freq,
                       y * nScale / freq,
                       octaves) *
                 (border - x) / border;
         }

         precipitation[y][x] = static_cast<float>(n);
      }
   }

   // Find ranges
   std::pair<PrecipitationDataType*, PrecipitationDataType*> minmaxPrecip =
      std::minmax_element(precipitation.data(),
                          precipitation.data() + precipitation.num_elements());
   float minPrecip = *minmaxPrecip.first;
   float maxPrecip = *minmaxPrecip.second;

   auto minmaxTemp =
      std::minmax_element(temperature.data(), //
                          temperature.data() + temperature.num_elements());
   float minTemp = *minmaxTemp.first;
   float maxTemp = *minmaxTemp.second;

   float precipDelta = maxPrecip - minPrecip;
   float tempDelta   = maxTemp - minTemp;

   BOOST_LOG_TRIVIAL(debug)
      << "Precipitation minmax: " << minPrecip << ", " << maxPrecip;
   BOOST_LOG_TRIVIAL(debug)
      << "Temperature minmax: " << minTemp << ", " << maxTemp;

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         // Normalize temperature and precipitation arrays
         float t = (temperature[y][x] - minTemp) / tempDelta;
         float p = (precipitation[y][x] - minPrecip) / precipDelta;

         /*
          * Ok, some explanation here because why the formula is doing htis may
          * be a little confusing. We are going to generate a modified gamma
          * curve based on normalized temperature and multiply our precipitation
          * amounts by it.
          *
          * std::pow(t, curveGamma) generates a standard gamma curve. However
          * we probably don't want to be multiplying precipitation by 0 at the
          * far side of the curve. To avoid this we multiply the curve by (1 -
          * curveBonus) and then add back curveBonus. Thus, if we have a curve
          * bonus of 0.2 then the range of our modified gamma curve goes from
          * 0-1 to 0-0.8 after we multiply and then to 0.2-1 after we add back
          * the curveBonus.
          *
          * Because we renormalize there is not much point to offsetting the
          * opposite end of the curve so it is less than or more than 1. We are
          * trying to avoid setting the start of the curve to 0 because f(t) * p
          * would equal 0 when t equals 0. However f(t) * p does not
          * automatically equal 1 when t equals 1 and if we raise or lower the
          * value for f(t) at 1 it would have negligible impact after
          * renormalizing.
          */

         // Modify precipitation based on temperature
         float curve = std::pow(t, curveGamma) * (1 - curveBonus) + curveBonus;
         precipitation[y][x] = p * curve;
      }
   }

   // Renormalize precipitation because the precipitiation changes will probably
   // not fully extend from -1 to 1
   minmaxPrecip =
      std::minmax_element(precipitation.data(),
                          precipitation.data() + precipitation.num_elements());
   minPrecip   = *minmaxPrecip.first;
   maxPrecip   = *minmaxPrecip.second;
   precipDelta = maxPrecip - minPrecip;

   BOOST_LOG_TRIVIAL(debug)
      << "Precipitation minmax (modified): " << minPrecip << ", " << maxPrecip;

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         precipitation[y][x] =
            (precipitation[y][x] - minPrecip) / precipDelta * 2 - 1;
      }
   }
}

} // namespace WorldEngine


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

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

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

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_disjoint_coverage
#endif

// unit test to test disjoint for all geometry combinations

#include <iostream>

#include <boost/test/included/unit_test.hpp>

#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

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

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/io/dsv/write.hpp>

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

#include <boost/geometry/algorithms/disjoint.hpp>

#include <from_wkt.hpp>

#ifdef HAVE_TTMATH
#include <boost/geometry/extensions/contrib/ttmath_stub.hpp>
#endif

namespace bg = ::boost::geometry;

//============================================================================

struct test_disjoint
{
    template <typename Geometry1, typename Geometry2>
    static inline void apply(std::string const& case_id,
                             Geometry1 const& geometry1,
                             Geometry2 const& geometry2,
                             bool expected_result)
    {
        bool result = bg::disjoint(geometry1, geometry2);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry1)
            << ", G2: " << bg::wkt(geometry2) << " -> Expected: "
            << expected_result << ", detected: " << result);

        result = bg::disjoint(geometry2, geometry1);
        BOOST_CHECK_MESSAGE(result == expected_result,
            "case ID: " << case_id << ", G1: " << bg::wkt(geometry2)
            << ", G2: " << bg::wkt(geometry1) << " -> Expected: "
            << expected_result << ", detected: " << result);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "case ID: " << case_id << "; G1 - G2: ";
        std::cout << bg::wkt(geometry1) << " - ";
        std::cout << bg::wkt(geometry2) << std::endl;
        std::cout << std::boolalpha;
        std::cout << "expected/computed result: "
                  << expected_result << " / " << result << std::endl;
        std::cout << std::endl;
        std::cout << std::noboolalpha;
#endif
    }
};

//============================================================================

// linear-linear geometries
template <typename P>
inline void test_segment_segment()
{
    typedef bg::model::segment<P> S;

    typedef test_disjoint tester;

    tester::apply("s-s-01",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<S>("SEGMENT(0 0,0 2)"),
                  false);

    tester::apply("s-s-02",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<S>("SEGMENT(2 0,3 0)"),
                  false);

    tester::apply("s-s-03",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<S>("SEGMENT(1 0,3 0)"),
                  false);

    tester::apply("s-s-04",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<S>("SEGMENT(1 0,1 1)"),
                  false);

    tester::apply("s-s-05",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<S>("SEGMENT(1 1,2 2)"),
                  true);

    tester::apply("s-s-06",
                  from_wkt<S>("SEGMENT(0 0,1 1)"),
                  from_wkt<S>("SEGMENT(1 1,1 1)"),
                  false);

    tester::apply("s-s-07",
                  from_wkt<S>("SEGMENT(0 0,1 1)"),
                  from_wkt<S>("SEGMENT(2 2,2 2)"),
                  true);

    tester::apply("s-s-08",
                  from_wkt<S>("SEGMENT(0 0,1 1)"),
                  from_wkt<S>("SEGMENT(2 2,3 3)"),
                  true);
}

template <typename P>
inline void test_linestring_segment()
{
    typedef bg::model::segment<P> S;
    typedef bg::model::linestring<P> L;

    typedef test_disjoint tester;

    tester::apply("l-s-01",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(0 0,0 2)"),
                  false);

    tester::apply("l-s-02",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(2 0,3 0)"),
                  false);

    tester::apply("l-s-03",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,3 0)"),
                  false);

    tester::apply("l-s-04",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,1 1)"),
                  false);

    tester::apply("l-s-05",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 1,2 2)"),
                  true);

    tester::apply("l-s-06",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 1,1 1,2 2)"),
                  true);

    tester::apply("l-s-07",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,1 0,1 1,2 2)"),
                  false);

    tester::apply("l-s-08",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,1 0,3 0)"),
                  false);

    tester::apply("l-s-09",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(3 0,3 0,4 0)"),
                  true);

    tester::apply("l-s-10",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(3 0,3 0)"),
                  true);

    tester::apply("l-s-11",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(-1 0,-1 0)"),
                  true);

    tester::apply("l-s-12",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,1 0)"),
                  false);

    tester::apply("l-s-13",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 1,1 1)"),
                  true);
}

template <typename P>
inline void test_multilinestring_segment()
{
    typedef bg::model::segment<P> S;
    typedef bg::model::linestring<P> L;
    typedef bg::model::multi_linestring<L> ML;

    typedef test_disjoint tester;

    tester::apply("s-ml-01",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((0 0,0 2))"),
                  false);

    tester::apply("s-ml-02",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((2 0,3 0))"),
                  false);

    tester::apply("s-ml-03",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 0,3 0))"),
                  false);

    tester::apply("s-ml-04",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 0,1 1))"),
                  false);

    tester::apply("s-ml-05",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2))"),
                  true);

    tester::apply("s-ml-06",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2),(3 3,3 3))"),
                  true);

    tester::apply("s-ml-07",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2),(1 0,1 0))"),
                  false);

    tester::apply("s-ml-08",
                  from_wkt<S>("SEGMENT(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2),(3 0,3 0))"),
                  true);
}

template <typename P>
inline void test_linestring_linestring()
{
    typedef bg::model::linestring<P> L;

    typedef test_disjoint tester;

    tester::apply("l-l-01",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(0 0,0 2)"),
                  false);

    tester::apply("l-l-02",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(2 0,3 0)"),
                  false);

    tester::apply("l-l-03",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,3 0)"),
                  false);

    tester::apply("l-l-04",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 0,1 1)"),
                  false);

    tester::apply("l-l-05",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<L>("LINESTRING(1 1,2 2)"),
                  true);
}

template <typename P>
inline void test_linestring_multilinestring()
{
    typedef bg::model::linestring<P> L;
    typedef bg::model::multi_linestring<L> ML;

    typedef test_disjoint tester;

    tester::apply("l-ml-01",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((0 0,0 2))"),
                  false);

    tester::apply("l-ml-02",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((2 0,3 0))"),
                  false);

    tester::apply("l-ml-03",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 0,3 0))"),
                  false);

    tester::apply("l-ml-04",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 0,1 1))"),
                  false);

    tester::apply("l-ml-05",
                  from_wkt<L>("LINESTRING(0 0,2 0)"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2))"),
                  true);
}

template <typename P>
inline void test_multilinestring_multilinestring()
{
    typedef bg::model::linestring<P> L;
    typedef bg::model::multi_linestring<L> ML;

    typedef test_disjoint tester;

    tester::apply("ml-ml-01",
                  from_wkt<ML>("MULTILINESTRING((0 0,2 0))"),
                  from_wkt<ML>("MULTILINESTRING((0 0,0 2))"),
                  false);

    tester::apply("ml-ml-02",
                  from_wkt<ML>("MULTILINESTRING((0 0,2 0))"),
                  from_wkt<ML>("MULTILINESTRING((2 0,3 0))"),
                  false);

    tester::apply("ml-ml-03",
                  from_wkt<ML>("MULTILINESTRING((0 0,2 0))"),
                  from_wkt<ML>("MULTILINESTRING((1 0,3 0))"),
                  false);

    tester::apply("ml-ml-04",
                  from_wkt<ML>("MULTILINESTRING((0 0,2 0))"),
                  from_wkt<ML>("MULTILINESTRING((1 0,1 1))"),
                  false);

    tester::apply("ml-ml-05",
                  from_wkt<ML>("MULTILINESTRING((0 0,2 0))"),
                  from_wkt<ML>("MULTILINESTRING((1 1,2 2))"),
                  true);
}

//============================================================================

template <typename CoordinateType>
inline void test_linear_linear()
{
    typedef bg::model::point<CoordinateType, 2, bg::cs::cartesian> point_type;

    test_linestring_linestring<point_type>();
    test_linestring_multilinestring<point_type>();
    test_linestring_segment<point_type>();

    test_multilinestring_multilinestring<point_type>();
    test_multilinestring_segment<point_type>();

    test_segment_segment<point_type>();
}

//============================================================================

BOOST_AUTO_TEST_CASE( test_linear_linear_all )
{
    test_linear_linear<double>();
    test_linear_linear<int>();
#ifdef HAVE_TTMATH
    test_linear_linear<ttmath_big>();
#endif
}


/*
 * map.cpp
 *
 *  Created on: Jul 28, 2015
 *      Author: sontd
 */

#include <vector>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include "../../include/environment/cell.hpp"
#include "../../include/environment/map.hpp"

namespace wandrian {
namespace environment {

Map::Map(RectanglePtr boundary, std::list<RectanglePtr> obstacles) :
    boundary(boundary), obstacles(obstacles) {
}

Map::Map(std::string map_path) {
  this->map_path = map_path;
}

Map::~Map() {
}

RectanglePtr Map::get_boundary() {
  return boundary;
}

std::list<RectanglePtr> Map::get_obstacles() {
  return obstacles;
}

std::string Map::get_map_path() {
  return map_path;
}

void Map::build() {
  // Read center point of obstacles from input file
  std::vector<PointPtr> obstacle_centers;
  std::vector<double> obstacle_sizes;
  bool global_obstacle_size = true;
  double o_size;
  std::ifstream map(map_path.c_str());
  std::string line;
  // Boundary size
  std::getline(map, line, '\n');
  int delim_pos1;
  int delim_pos2;
  int delim_pos3;
  delim_pos1 = line.find(" ");
  delim_pos2 = line.find(" ", delim_pos1 + 1);
  double center_x = boost::lexical_cast<double>(
      boost::lexical_cast<double>(line.substr(0, delim_pos1)));
  double center_y = boost::lexical_cast<double>(
      boost::lexical_cast<double>(
          line.substr(delim_pos1 + 1, delim_pos2 - delim_pos1 - 1)));
  delim_pos1 = delim_pos2;
  delim_pos2 = line.find(" ", delim_pos1 + 1);
  delim_pos3 = line.find(" ", delim_pos2 + 1);
  global_obstacle_size = (delim_pos3 != std::string::npos);
  double width = boost::lexical_cast<double>(
      boost::lexical_cast<double>(
          line.substr(delim_pos1 + 1, delim_pos2 - delim_pos1 - 1)));
  double height = boost::lexical_cast<double>(
      boost::lexical_cast<double>(
          line.substr(delim_pos2 + 1,
              (global_obstacle_size ? delim_pos3 : line.length()) - delim_pos2
                  - 1)));
  boundary = RectanglePtr(
      new Rectangle(PointPtr(new Point(center_x, center_y)), width, height));
  if (global_obstacle_size)
    o_size = boost::lexical_cast<double>(
        boost::lexical_cast<double>(
            line.substr(delim_pos3 + 1, line.length() - delim_pos3 - 1)));
  // Center point of obstacles
  while (std::getline(map, line, '\n')) {
    if (line == "" || line.substr(0, 1) == "#")
      continue;
    double center_point_x;
    double center_point_y;
    int d_pos1 = line.find(" ");
    int d_pos2 = line.find(" ", d_pos1 + 1);
    center_point_x = boost::lexical_cast<double>(line.substr(0, d_pos1));
    center_point_y = boost::lexical_cast<double>(
        line.substr(d_pos1 + 1,
            (!global_obstacle_size ? d_pos2 : line.length()) - d_pos1 - 1));
    obstacle_centers.push_back(
        PointPtr(new Point(center_point_x, center_point_y)));
    if (!global_obstacle_size) {
      obstacle_sizes.push_back(
          boost::lexical_cast<double>(
              line.substr(d_pos2 + 1, line.length() - d_pos2 - 1)));
    }
  }

  // Generate obstacles
  for (int i = 0; i <= ((int) obstacle_centers.size() - 1); i++) {
    PointPtr center = obstacle_centers[i];
    bool valid = true;
    for (std::list<RectanglePtr>::iterator o = obstacles.begin();
        o != obstacles.end(); o++)
      if ((boost::static_pointer_cast<Cell>(*o))->get_center() == center) {
        valid = false;
        break;
      };
    if (valid) {
      obstacles.insert(obstacles.end(),
          CellPtr(
              new Cell(center,
                  global_obstacle_size ? o_size : obstacle_sizes[i])));
    }
  }
}

}
}


#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <fstream>
#include <ctime>
#include <unistd.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
ofstream outputfile;
ostringstream temp;
bool start = true;
struct tm* local_time()
{
    time_t now = time(0);
    tm* ltm = localtime(&now);
    ltm->tm_year += 1900;
    ltm->tm_mon += 1;
//     cout << ltm->tm_year << endl;
//     cout << ltm->tm_mon << endl;
//     cout << ltm->tm_mday << endl;
//     cout << ltm->tm_hour << endl;
//     cout << ltm->tm_min << endl;
//     cout << ltm->tm_sec << endl;
    return ltm;
}
void odomCB(const nav_msgs::Odometry::ConstPtr& msg)
{
    if(msg->pose.pose.position.x == 0&&start)
    {
        cout<<"aaaaa"<<endl;
        return;
    }
    double px = msg->pose.pose.position.x;
    double py = msg->pose.pose.position.y;
    double pz = msg->pose.pose.position.z;
    double w = msg->pose.pose.orientation.w;
    double x = msg->pose.pose.orientation.x;
    double y = msg->pose.pose.orientation.y;
    double z = msg->pose.pose.orientation.z;
    Eigen::Quaternion<double> q = Eigen::Quaternion<double>(w,x,y,z);
    //Eigen::Quaterniond q = Eigen::Quaterniond(vector4d(x,y,z,w));
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();

    outputfile <<
    R(0,0)<<' '<<R(0,1)<<' '<<R(0,2)<<' '<<px<<' '<<
    R(1,0)<<' '<<R(1,1)<<' '<<R(1,2)<<' '<<py<<' '<<
    R(2,0)<<' '<<R(2,1)<<' '<<R(2,2)<<' '<<pz<<endl;

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "odom2txt");
    ros::NodeHandle nh_;
    ///检查目录是否存在，不存在就建一个
//     if(access("/home/xcy/.robot_log", 00))
//     system("mkdir /home/xcy/.robot_log");
    string topic_name = "/lio_sam/mapping/odometry";
    if(argc>1)
    {
        topic_name = argv[1];
    }
    ostringstream temp;
    string filename;
    tm* _p_fopen_time = local_time();
    temp <<
         "/home/xcy/" <<
         _p_fopen_time->tm_year <<
         setfill('0') << setw(2) << _p_fopen_time->tm_mon <<
         setfill('0') << setw(2) << _p_fopen_time->tm_mday <<
         setfill('0') << setw(2) << _p_fopen_time->tm_hour <<
         setfill('0') << setw(2) << _p_fopen_time->tm_min <<
         setfill('0') << setw(2) << _p_fopen_time->tm_sec <<
         ".txt";
    filename = temp.str();
    temp.clear();
    temp.str("");
    outputfile.open(filename.c_str(), ios::trunc);
    ros::Subscriber log_sub_ = nh_.subscribe<nav_msgs::Odometry>(topic_name, 10, odomCB);
    //system("find /home/xcy/.robot_log -name \"*.txt\" -mtime +7 -exec rm -fv {} '\;'");
    ros::spin();
    outputfile.close();
    return 0;
}


#include <boost/test/unit_test.hpp>

#include <limits>
#include <avalon/utils/numbers.h>

BOOST_AUTO_TEST_SUITE(utils)
BOOST_AUTO_TEST_SUITE(numbers)

BOOST_AUTO_TEST_CASE(hasOppositeSigns)
{
    using avalon::utils::numbers::hasOppositeSigns;

    BOOST_CHECK(hasOppositeSigns(1, -1));
    BOOST_CHECK(hasOppositeSigns(-1, 1));

    BOOST_CHECK(!hasOppositeSigns(1, 1));
    BOOST_CHECK(!hasOppositeSigns(0, 0));
}

BOOST_AUTO_TEST_CASE(isPowerOfTwo)
{
    using avalon::utils::numbers::isPowerOfTwo;

    BOOST_CHECK( isPowerOfTwo(0));
    BOOST_CHECK( isPowerOfTwo(1));
    BOOST_CHECK( isPowerOfTwo(2));
    BOOST_CHECK(!isPowerOfTwo(3));
    BOOST_CHECK( isPowerOfTwo(4));
    BOOST_CHECK(!isPowerOfTwo(5));
    BOOST_CHECK(!isPowerOfTwo(6));
    BOOST_CHECK(!isPowerOfTwo(7));
    BOOST_CHECK( isPowerOfTwo(8));
}

BOOST_AUTO_TEST_CASE(roundUpToNextPowerOfTwo)
{
    using avalon::utils::numbers::roundUpToNextPowerOfTwo;

    BOOST_CHECK_EQUAL(1, roundUpToNextPowerOfTwo(0));
    BOOST_CHECK_EQUAL(1, roundUpToNextPowerOfTwo(1));
    BOOST_CHECK_EQUAL(2, roundUpToNextPowerOfTwo(2));
    BOOST_CHECK_EQUAL(4, roundUpToNextPowerOfTwo(3));
    BOOST_CHECK_EQUAL(4, roundUpToNextPowerOfTwo(4));
    BOOST_CHECK_EQUAL(8, roundUpToNextPowerOfTwo(5));
    BOOST_CHECK_EQUAL(8, roundUpToNextPowerOfTwo(6));
    BOOST_CHECK_EQUAL(8, roundUpToNextPowerOfTwo(7));
    BOOST_CHECK_EQUAL(8, roundUpToNextPowerOfTwo(8));
}

BOOST_AUTO_TEST_CASE(reverseBytes)
{
    using avalon::utils::numbers::reverseBytes;

    BOOST_CHECK_EQUAL(0xFF << 24, reverseBytes(0xFF << 0));
    BOOST_CHECK_EQUAL(0xFF << 16, reverseBytes(0xFF << 8));
    BOOST_CHECK_EQUAL(0xFF << 8,  reverseBytes(0xFF << 16));
}

BOOST_AUTO_TEST_CASE(reverseBytesIgnoresZero)
{
    using avalon::utils::numbers::reverseBytes;

    BOOST_CHECK_EQUAL(0, reverseBytes(0));
}

BOOST_AUTO_TEST_CASE(narrowCastInt)
{
    using avalon::utils::numbers::narrow_cast;

    auto i1 = narrow_cast<int>(1.0f);
    BOOST_CHECK_EQUAL("i", typeid(i1).name());
    BOOST_CHECK_EQUAL(1, i1);

    auto i2 = narrow_cast<int>(2.0);
    BOOST_CHECK_EQUAL("i", typeid(i2).name());
    BOOST_CHECK_EQUAL(2, i2);
}

BOOST_AUTO_TEST_CASE(narrowCastIntThrow)
{
    using avalon::utils::numbers::narrow_cast;

    BOOST_CHECK_NO_THROW(narrow_cast<int>(1.0f));
    BOOST_CHECK_THROW(narrow_cast<int>(1.2f), std::bad_cast);
}

BOOST_AUTO_TEST_CASE(narrowCastFloat)
{
    using avalon::utils::numbers::narrow_cast;

    auto f1 = narrow_cast<float>(1);
    BOOST_CHECK_EQUAL("f", typeid(f1).name());
    BOOST_CHECK_EQUAL(1.0, f1);

    auto f2 = narrow_cast<float>(5.0f/2.0f);
    BOOST_CHECK_EQUAL("f", typeid(f2).name());
    BOOST_CHECK_EQUAL(2.5, f2);
}

BOOST_AUTO_TEST_CASE(narrowCastFloatThrow)
{
    using avalon::utils::numbers::narrow_cast;

    BOOST_CHECK_NO_THROW(narrow_cast<float>(12));

    double max = std::numeric_limits<double>::max();
    BOOST_CHECK_THROW(narrow_cast<float>(max), std::bad_cast);
}

BOOST_AUTO_TEST_SUITE_END() // numbers
BOOST_AUTO_TEST_SUITE_END() // utils


/*

Copyright 2016 Emanuele Vespa, Imperial College London

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <se/utils/math_utils.h>
#include <Eigen/Core>
#include "gtest/gtest.h"

TEST(EigenUtils, ClampFixVec3) {
  Eigen::Vector3i base{0, 20, -1};
  Eigen::Vector3i min{0, 0, 0};
  Eigen::Vector3i max{10, 10, 10};
  se::math::clamp(base, min, max);

  ASSERT_TRUE(base.x() >= 0 && base.x() <= 10);
  ASSERT_TRUE(base.y() >= 0 && base.y() <= 10);
  ASSERT_TRUE(base.z() >= 0 && base.z() <= 10);
}

TEST(EigenUtils, ClampFixVec2) {
  Eigen::Vector2f base{-100.f, 34.f};
  Eigen::Vector2f min{0.f, 0.f};
  Eigen::Vector2f max{20.f, 10.f};
  se::math::clamp(base, min, max);

  ASSERT_TRUE(base.x() >= min.x() && base.x() <= max.x());
  ASSERT_TRUE(base.y() >= min.y() && base.y() <= max.y());
}


#ifndef INCLUDED_scheme_nest_maps_UnitMap_HH
#define INCLUDED_scheme_nest_maps_UnitMap_HH

#include "scheme/util/template_loop.hh"
#include "scheme/util/SimpleArray.hh"
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <boost/static_assert.hpp>
#include <iostream>
#include <vector>

namespace scheme {
namespace nest {
namespace pmap {

	///@brief Parameter to Value Map Policy Class
	///@detail just copies the [0.0,1.0] hypercube coords to Value
	///        the first dimension will incremented by cell_index
	///@tparam DIM the dimension number of the input parameter space
	///@tparam Value the output value type, default SimpleArray
	///@tparam Index index type, default size_t
	///@tparam Float float type, default double
	template<
		int DIM,
		class Value=util::SimpleArray<DIM,double>,
		class Index=uint64_t,
		class Float=double
	>
	struct UnitMap {
		static int const DIMENSION = DIM;
		typedef Value ValueType ;
		typedef Float FloatType ;
		typedef Index IndexType ;
		typedef util::SimpleArray<DIM,Index> Indices;
		typedef util::SimpleArray<DIM,Float> Params;

		BOOST_STATIC_ASSERT_MSG(DIM>0,"UnitMap DIM must be > 0");
		///@brief constructor
		UnitMap(Index num_cells=1) : num_cells_(num_cells) {}
		///@brief sets value to parameters without change
		///@return false iff invalid parameters
		bool params_to_value(
			Params const & params,
			Index cell_index,
			Index /*resl*/,
			Value & value
		) const {
			for(int i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
			assert( params[0] <= (Float)num_cells_ );
			for(int i = 1; i < DIM; ++i) assert( params[i] <= 1.0 );
            for(int i = 0; i < DIM; ++i) value[i] = params[i];
			value[0] += (Float)cell_index;
			return true;
		}
		///@brief sets params/cell_index from value
		///@note necessary for value lookup and neighbor lookup
		bool value_to_params(
			Value const & value,
			Index resl,
			Params & params,
			Index & cell_index
		) const {
			///@note neighbor lookups require out of bounds mappings to be valid
			value_to_params_for_cell(value,resl,params,0);
			cell_index = (Index)value[0];
			params[0] -= (Float)cell_index;
			for(int i = 0; i < DIM; ++i) assert( 0.0 <= params[i] );
			assert( params[0] <= (Float)num_cells_ );
			for(int i = 1; i < DIM; ++i) assert( params[i] <= 1.0 );
			return true;
		}
		///@brief get params repr of Value wrt cell cell_index
		///@note necessary only for neighbor lookup
		void value_to_params_for_cell(
			Value const & value,
			Index /*resl*/,
			Params & params,
			Index cell_index
		) const {
			for(int i = 0; i < DIM; ++i) params[i] = value[i];
			params[0] -= (Float)cell_index;
		}
		///@brief return the cell_index of neighboring cells within delta of value
		///@note delta parameter is in "Parameter Space"
		template<class OutIter>
		void get_neighboring_cells(
			Value const & value,
			Index /*resl*/,
			Float param_delta,
			OutIter out
		) const {
			// Float param_delta = 1.0 / (Float)(1<<resl);
			// this BIG thing is to ensure rounding goes down
			int const BIG = 12345678;
			int lb = std::max(                0, static_cast<int>( value[0]-param_delta + BIG ) - BIG );
			int ub = std::min((int)num_cells_-1, static_cast<int>( value[0]+param_delta + BIG ) - BIG );
			// std::cout << "lb " << lb << " ub "  << ub << std::endl;
			// assert(lb<=ub);
			for(int i = lb; i <= ub; ++i) *(out++) = i;
		}
		///@brief aka covering radius max distance from bin center to any value within bin
		Float bin_circumradius(Index resl) const { return 0.5/(Float)(1<<resl) * sqrt(DIM); }
		///@brief maximum distance from the bin center which must be within the bin
		Float bin_inradius(Index resl) const { return 1.5/(Float)(1<<resl); }
		///@brief cell size
		Index num_cells() const { return num_cells_; }
		virtual ~UnitMap(){}
	 private:
	 	///@brief number of cells
		Index num_cells_;
	};

}
}
}

#endif


#ifndef OSRM_UTIL_MSB_HPP
#define OSRM_UTIL_MSB_HPP

#include <boost/assert.hpp>

#include <climits>
#include <cstdint>
#include <utility>

namespace osrm
{
namespace util
{

// get the msb of an integer
// return 0 for integers without msb
template <typename T> std::size_t msb(T value)
{
    static_assert(std::is_integral<T>::value && !std::is_signed<T>::value, "Integer required.");
    std::size_t msb = 0;
    while (value > 0)
    {
        value >>= 1u;
        msb++;
    }
    BOOST_ASSERT(msb > 0);
    return msb - 1;
}

#if (defined(__clang__) || defined(__GNUC__) || defined(__GNUG__))
inline std::size_t msb(unsigned long long v)
{
    BOOST_ASSERT(v > 0);
    constexpr auto MSB_INDEX = CHAR_BIT * sizeof(unsigned long long) - 1;
    return MSB_INDEX - __builtin_clzll(v);
}
inline std::size_t msb(unsigned long v)
{
    BOOST_ASSERT(v > 0);
    constexpr auto MSB_INDEX = CHAR_BIT * sizeof(unsigned long) - 1;
    return MSB_INDEX - __builtin_clzl(v);
}
inline std::size_t msb(unsigned int v)
{
    BOOST_ASSERT(v > 0);
    constexpr auto MSB_INDEX = CHAR_BIT * sizeof(unsigned int) - 1;
    return MSB_INDEX - __builtin_clz(v);
}
#endif
} // namespace util
} // namespace osrm

#endif


#include "RBGL.hpp"
#include <boost/graph/dominator_tree.hpp>

extern "C"
{
    SEXP BGL_dominator_tree(SEXP num_verts_in, SEXP num_edges_in,
         SEXP R_edges_in, SEXP start )
    {
    	using namespace std;
    	using namespace boost;

        const int NV = Rf_asInteger(num_verts_in);
        const int NE = Rf_asInteger(num_edges_in);
        int v = Rf_asInteger(start);

        typedef adjacency_list < vecS, listS, bidirectionalS,
                        property < vertex_index_t, int> > VEGraph;

         VEGraph g(NV);

         typedef property_map < VEGraph, vertex_index_t > ::type IndexMap;
         IndexMap v1_index_map = get(vertex_index, g);
         vector<graph_traits<VEGraph>::vertex_descriptor> v1(NV);
         graph_traits<VEGraph>::vertex_iterator vi, v_end;
         int i = 0;
         for (tie(vi, v_end) = vertices(g); vi != v_end; ++vi, ++i)
         {
               put(v1_index_map, *vi, i);
               v1[i] = *vi;
         }

         int* edges_in = INTEGER(R_edges_in);
         for ( i = 0; i < NE; i++, edges_in += 2 )
         {
             add_edge(v1[*edges_in], v1[*(edges_in+1)], g);
         }

        typedef graph_traits < VEGraph > ::vertex_descriptor Vertex;

        vector< Vertex > domTreePredVector =
            vector<Vertex>(num_vertices(g), graph_traits<VEGraph>::null_vertex());
        iterator_property_map< vector < Vertex > ::iterator, IndexMap >
          domTreePredMap =
             make_iterator_property_map(domTreePredVector.begin(), v1_index_map);

    	lengauer_tarjan_dominator_tree(g, vertex(v, g), domTreePredMap);

        SEXP ansList;
        PROTECT(ansList = Rf_allocVector(INTSXP,num_vertices(g)));

        for (tie(vi, v_end) = vertices(g); vi != v_end; ++vi)
	{
            i = get(v1_index_map, *vi);
            if (get(domTreePredMap, *vi) != graph_traits<VEGraph>::null_vertex())
		INTEGER(ansList)[i] = get(v1_index_map, get(domTreePredMap, *vi));
            else
                INTEGER(ansList)[i] = i; // (numeric_limits<int>::max)();
	}

        UNPROTECT(1);
        return(ansList);

    }

}