/*
 * MIT License
 *
 * Copyright (c) 2020 International Business Machines
 *
 * 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.
 */

// This is a sample program for education purposes only.
// It implements a very simple homomorphic encryption based
// db search algorithm for demonstration purposes.

// This country lookup example is derived from the BGV database demo
// code originally writte by Jack Crawford for a lunch and learn
// session at IBM Research (Hursley) in 2019.
// The original example code ships with HElib and can be found at
// https://github.com/IBM-HElib/HElib/tree/master/examples/BGV_database_lookup
//
// INFO: The API used to create these examples can be found online
// ML-HElib: https://ibm.github.io/fhe-toolkit-linux
// HElib: https://ibm.github.io/fhe-toolkit-linux/html/helib/index.html
//

#include <iostream>

#include "helayers/hebase/hebase.h"
#include "helayers/hebase/helib/HelibBgvContext.h"
#include <fstream>
#include <helib/ArgMap.h>
#include <NTL/BasicThreadPool.h>

using namespace helayers;
using namespace std;

// Forward declarations. These functions are explained later.
vector<pair<string, string>> read_csv(string filename, int maxLen);
void run(HeContext& he,
         const string& db_filename,
         const std::string& countryName,
         bool debug);
bool isPowerOf2(int v);
vector<int> stringToAscii(const string& val);

int main(int argc, char* argv[])
{
  // Note: The parameters have been chosen to provide a somewhat
  // faster running time with a non-realistic security level.
  // Do Not use these parameters in real applications.

  // Plaintext prime modulus
  unsigned long p = 127;
  // Cyclotomic polynomial - defines phi(m)
  unsigned long m = 128; // this will give 32 slots
  // Hensel lifting (default = 1)
  unsigned long r = 1;
  // Number of bits of the modulus chain
  unsigned long bits = 1000;
  // Number of columns of Key-Switching matrix (default = 2 or 3)
  unsigned long c = 2;
  // Size of NTL thread pool (default =1)
  unsigned long nthreads = 1;
  // input database file name
  string db_filename =
      getExamplesDir() + "/BGV_world_country_db_lookup/countries_dataset.csv";
  // debug output (default no debug output)
  bool debug = false;

  string countryName = "";

  helib::ArgMap amap;
  amap.arg("m", m, "Cyclotomic polynomial ring");
  amap.arg("p", p, "Plaintext prime modulus");
  amap.arg("r", r, "Hensel lifting");
  amap.arg("bits", bits, "# of bits in the modulus chain");
  amap.arg("c", c, "# fo columns of Key-Switching matrix");
  amap.arg("nthreads", nthreads, "Size of NTL thread pool");
  amap.arg(
      "db_filename", db_filename, "Qualified name for the database filename");
  amap.arg("country", countryName, "Country to search for");
  amap.toggle().arg("-debug", debug, "Toggle debug output", "");
  amap.parse(argc, argv);

  // set NTL Thread pool size
  if (nthreads > 1)
    NTL::SetNumThreads(nthreads);

  cout << "\n*********************************************************";
  cout << "\n*           Privacy Preserving Search Example           *";
  cout << "\n*           =================================           *";
  cout << "\n*                                                       *";
  cout << "\n* This is a sample program for education purposes only. *";
  cout << "\n* It implements a very simple homomorphic encryption    *";
  cout << "\n* based db search algorithm for demonstration purposes. *";
  cout << "\n*                                                       *";
  cout << "\n*********************************************************";
  cout << endl;

  cout << "---Initialising HE Environment ... ";
  // Initialize context
  cout << "\nInitializing the Context ... " << endl;

  // To setup helib using the hebase layer, let's first
  // copy all configuration params to an HelibConfig object:
  HelibConfig conf;
  conf.p = p;
  conf.m = m;
  conf.r = r;
  conf.L = bits;
  conf.c = c;

  // Next we'll initialize a BGV scheme in helib.
  // The following two lines perform full intializiation
  // Including key generation.
  // (We added code for timing it).
  HELIB_NTIMER_START(timer_Context);
  HelibBgvContext he;
  he.init(conf);
  HELIB_NTIMER_STOP(timer_Context);

  // Helib-BGV is now ready to start doing some HE work.
  // which we'll do in the follwing function, defined below
  run(he, db_filename, countryName, debug);

  return 0;
}

void run(HeContext& he,
         const string& db_filename,
         const std::string& countryName,
         bool debug)
{

  // The run function receives an abstract HeContext class.
  // Therefore the code below is oblivious to a particular HE scheme
  // implementation.

  // First let's print general information on our library and scheme.
  // This will print their names, and the configuraton details.
  he.printSignature();

  // However we do have some requirements that we can
  // assert exists:
  // We require the plaintext to be over modular arithmetic.
  // We'll rely on that later.
  always_assert(he.getTraits().getIsModularArithmetic());
  // Since we store ascii codes, we need it at least to be able
  // to handle the numbers 0...127
  always_assert(he.getTraits().getArithmeticModulus() >= 127);

  // Next, print the security level
  // Note: This will be negligible to improve performance time.
  cout << "\n***Security Level: " << he.getSecurityLevel()
       << " *** Negligible for this example ***" << endl;

  // Let's also print the number of slots.
  // Each ciphertext will have this many slots.
  cout << "\nNumber of slots: " << he.slotCount() << endl;

  // Now we'll read in the database (in cleartext).
  // This function we'll make sure no string is longer than he.slotCount()
  vector<pair<string, string>> country_db =
      read_csv(db_filename, he.slotCount());

  cout << "\n---Initializing the encrypted key,value pair database ("
       << country_db.size() << " entries)...";
  cout << "\nConverting strings to numeric representation into Ptxt objects ..."
       << endl;

  // We'll now encrypt our country-capital database.
  HELIB_NTIMER_START(timer_CtxtCountryDB);
  // The encoder class handles both encoding and encrypting.
  Encoder enc(he);
  // This is the database: a vector of pairs of CTile-s.
  // A CTile is a ciphertext object.
  vector<pair<CTile, CTile>> encrypted_country_db;
  for (const auto& country_capital_pair : country_db) {
    // Create a country ciphertext, and encrypt inside
    // the ascii vector representation of each country.
    // For example, Norway is represented
    // (78,111,114,119,97,121,  0,0,0, ...)
    CTile country(he);
    enc.encodeEncrypt(country, stringToAscii(country_capital_pair.first));
    // Similarly encrypt the capital name
    CTile capital(he);
    enc.encodeEncrypt(capital, stringToAscii(country_capital_pair.second));
    // Add the pair to the database
    encrypted_country_db.emplace_back(move(country), move(capital));
  }
  HELIB_NTIMER_STOP(timer_CtxtCountryDB);

  if (debug) {
    helib::printNamedTimer(cout << endl, "timer_Context");
    helib::printNamedTimer(cout, "timer_CtxtCountryDB");
  }

  cout << "\nInitialization Completed - Ready for Queries" << endl;
  cout << "--------------------------------------------" << endl;

  /** Create the query **/

  // Read in query from the command line
  string query_string;
  if (countryName == "") {
    cout << "\nPlease enter the name of a Country: ";
    getline(cin, query_string);
  } else
    query_string = countryName;

  cout << "Looking for the Capital of " << query_string << endl;
  cout << "This may take a few minutes ... " << endl;

  HELIB_NTIMER_START(timer_TotalQuery);
  HELIB_NTIMER_START(timer_EncryptQuery);

  // Encrypt the query similar to the way we encrypted
  // the country and capital names
  CTile query(he);
  enc.encodeEncrypt(query, stringToAscii(query_string));

  HELIB_NTIMER_STOP(timer_EncryptQuery);

  /************ Perform the database search ************/

  HELIB_NTIMER_START(timer_QuerySearch);
  vector<CTile> mask;
  mask.reserve(country_db.size());
  NativeFunctionEvaluator eval(he);
  long modulusP = he.getTraits().getArithmeticModulus();

  // For every entry in our database we perform the following
  // calculation:
  for (const auto& encrypted_pair : encrypted_country_db) {
    //  Copy of database key: a country name
    CTile mask_entry = encrypted_pair.first;
    // Calculate the difference
    // In each slot now we'll have 0 when characters match,
    // or non-zero when there's a mismatch
    mask_entry.sub(query);

    // Fermat's little theorem:
    // Since the underlying plaintext are in modular arithmetic,
    // Raising to the power of modulusP convers all non-zero values
    // to 1.
    eval.powerInPlace(mask_entry, modulusP - 1);

    // Negate the ciphertext
    // Now we'll have 0 for match, -1 for mismatch
    mask_entry.negate();

    // Add +1
    // Now we'll have 1 for match, 0 for mismatch
    mask_entry.addScalar(1);

    // We'll now multiply all slots together, since
    // we want a complete match across all slots.

    // If slot count is a power of 2 there's an efficient way
    // to do it:
    // we'll do a rotate-and-multiply algorithm, similar to
    // a rotate-and-sum one.
    if (isPowerOf2(he.slotCount())) {
      for (int rot = 1; rot < he.slotCount(); rot *= 2) {
        CTile tmp(mask_entry);
        tmp.rotate(-rot);
        mask_entry.multiply(tmp);
      }
    } else {
      // Otherwise we'll create all possible rotations, and multiply all of
      // them.
      // Note that for non powers of 2 a rotate-and-multiply algorithm
      // can still be used as well, though it's more complicated and
      // beyond the scope of this example.
      vector<CTile> rotated_masks(he.slotCount(), mask_entry);
      for (int i = 1; i < rotated_masks.size(); i++)
        rotated_masks[i].rotate(-i); // Rotate each of the masks
      eval.totalProduct(mask_entry,
                        rotated_masks); // Multiply each of the masks
    }

    // mask_entry is now either all 1s if query==country,
    // or all 0s otherwise.
    // After we multiply by capital name it will be either
    // the capital name, or all 0s.
    mask_entry.multiply(encrypted_pair.second);
    // We collect all our findings.
    mask.push_back(mask_entry);
  }
  HELIB_NTIMER_STOP(timer_QuerySearch);

  // Aggregate the results into a single ciphertext
  // Note: This code is for educational purposes and thus we try to refrain
  // from using the STL and do not use std::accumulate
  CTile value = mask[0];
  for (int i = 1; i < mask.size(); i++)
    value.add(mask[i]);

  // /************ Decrypt and print result ************/

  HELIB_NTIMER_START(timer_DecryptQueryResult);
  vector<int> res = enc.decryptDecodeInt(value);
  HELIB_NTIMER_STOP(timer_DecryptQueryResult);

  // Convert from ASCII to a string
  string string_result;
  for (long i = 0; i < res.size(); ++i)
    string_result.push_back(static_cast<long>(res[i]));

  HELIB_NTIMER_STOP(timer_TotalQuery);

  // Print DB Query Timers
  if (debug) {
    helib::printNamedTimer(cout << endl, "timer_EncryptQuery");
    helib::printNamedTimer(cout, "timer_QuerySearch");
    helib::printNamedTimer(cout, "timer_DecryptQueryResult");
    cout << endl;
  }

  if (string_result.at(0) == 0x00) {
    string_result = "Country name not in the database.\n*** Please make sure "
                    "to enter the name of an European Country\n*** with the "
                    "first letter in upper case.";
  }

  cout << "\nQuery result: " << string_result << endl;
  helib::printNamedTimer(std::cout, "timer_TotalQuery");
}

// Utility function to read <K,V> CSV data from file
vector<pair<string, string>> read_csv(string filename, int maxLen)
{
  vector<pair<string, string>> dataset;
  ifstream data_file(filename);

  if (!data_file.is_open())
    throw runtime_error(
        "Error: This example failed trying to open the data file: " + filename +
        "\n           Please check this file exists and try again.");

  vector<string> row;
  string line, entry, temp;

  if (data_file.good()) {
    // Read each line of file
    while (getline(data_file, line)) {
      row.clear();
      stringstream ss(line);
      while (getline(ss, entry, ',')) {
        row.push_back(entry);
      }
      if (row[0].size() > maxLen)
        throw runtime_error("Country name " + row[0] + " too long");
      if (row[1].size() > maxLen)
        throw runtime_error("Capital name " + row[1] + " too long");

      // Add key value pairs to dataset
      dataset.push_back(make_pair(row[0], row[1]));
    }
  }

  data_file.close();
  return dataset;
}

// Return a vector of ints with the i'th element containing the ascii
// code of the i'th character
vector<int> stringToAscii(const string& val)
{
  vector<int> res;
  res.reserve(val.size());
  for (size_t i = 0; i < val.size(); ++i) {
    res.push_back(val[i]);
  }
  return res;
}

// Returns true if v is a power of 2
bool isPowerOf2(int v) { return (v & v - 1) == 0; }


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <gtest/gtest.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/math/CMatrixFixed.h>

#include <Eigen/Dense>
#include <sstream>

#include "mrpt_test.h"

MRPT_TEST(MatrixYaml, FromToEigen)
{
	const Eigen::MatrixXd m = Eigen::MatrixXd::Identity(3, 4);
	const auto y1 = mrpt::containers::yaml::FromMatrix(m);

	mrpt::containers::yaml d = mrpt::containers::yaml::Map();
	d["K"] = y1;

	EXPECT_TRUE(y1.isMap());
	EXPECT_TRUE(y1.has("rows"));
	EXPECT_TRUE(y1.has("cols"));
	EXPECT_TRUE(y1.has("data"));

	EXPECT_TRUE(y1["rows"].isScalar());
	EXPECT_TRUE(y1["cols"].isScalar());
	EXPECT_TRUE(y1["data"].isSequence());
	EXPECT_EQ(y1["data"].asSequence().size(), 12UL);

	const std::string expectedStr = R"(K:
  cols: 4
  data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
  rows: 3
)";

	mrpt::containers::YamlEmitOptions eo;
	eo.emitHeader = false;

	std::stringstream ss;
	d.printAsYAML(ss, eo);
	EXPECT_EQ(expectedStr, ss.str());

	// 2nd part: to Eigen:
	{
		Eigen::MatrixXd m2;
		y1.toMatrix(m2);
		EXPECT_EQ(m2.cols(), m.cols());
		EXPECT_EQ(m2.rows(), m.rows());
		EXPECT_EQ(m2, m);
	}
	{
		Eigen::Matrix<double, 3, 4> m2;
		y1.toMatrix(m2);
		EXPECT_EQ(m2.cols(), m.cols());
		EXPECT_EQ(m2.rows(), m.rows());
		EXPECT_EQ(m2, m);
	}
	{
		Eigen::Matrix<double, 3, 3> m2;
		EXPECT_ANY_THROW(y1.toMatrix(m2));
	}
}
MRPT_TEST_END()

MRPT_TEST(MatrixYaml, Vector)
{
	const Eigen::Vector3d m = Eigen::Vector3d::Constant(1.0);
	const auto y1 = mrpt::containers::yaml::FromMatrix(m);

	EXPECT_TRUE(y1.isMap());
	EXPECT_TRUE(y1.has("rows"));
	EXPECT_TRUE(y1.has("cols"));
	EXPECT_TRUE(y1.has("data"));

	EXPECT_EQ(y1["rows"].as<int>(), 3);
	EXPECT_EQ(y1["cols"].as<int>(), 1);
	EXPECT_TRUE(y1["data"].isSequence());
	EXPECT_EQ(y1["data"].asSequence().size(), 3UL);
}
MRPT_TEST_END()


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "Translate.hpp"
using namespace QuadProgMm;

#define CHECK_SIZES(t) do { \
  BOOST_CHECK_EQUAL(t.variables.size(), t.G.nrows()); \
  BOOST_CHECK_EQUAL(t.G.ncols(), t.G.nrows()); \
  BOOST_CHECK_EQUAL(t.g0.size(), t.G.nrows()); \
  BOOST_CHECK_EQUAL(t.CE.nrows(), t.G.nrows()); \
  BOOST_CHECK_EQUAL(t.ce0.size(), t.CE.ncols()); \
  BOOST_CHECK_EQUAL(t.CI.nrows(), t.G.nrows()); \
  BOOST_CHECK_EQUAL(t.ci0.size(), t.CI.ncols()); \
} while(false)

#define CHECK_MATRIX(actual, expected) do { \
  const int rows = expected.size(); \
  BOOST_CHECK_EQUAL(actual.nrows(), rows); \
  if(rows) { \
    for(int row = 0; row != rows; ++row) { \
      const int cols = expected[row].size(); \
      BOOST_CHECK_EQUAL(actual.ncols(), cols); \
      for(int col = 0; col != cols; ++col) { \
        BOOST_CHECK_EQUAL(actual[row][col], expected[row][col]); \
      } \
    } \
  } else { \
    BOOST_CHECK_EQUAL(actual.ncols(), 0); \
  } \
} while(false)

#define CHECK_VECTOR(actual, expected) do { \
  const int n = expected.size(); \
  BOOST_CHECK_EQUAL(actual.size(), n); \
  for(int i = 0; i != n; ++i) { \
    BOOST_CHECK_EQUAL(actual[i], expected[i]); \
  } \
} while(false)

#define CHECK_QUADRATIC_EXPRESSION_TRANSLATION(q, _G, _G0, expected_g00) do { \
  const Translation t = translate(q, {}); \
  const std::vector<std::vector<double>> expected_G _G; \
  const std::vector<double> expected_g0 _G0; \
  CHECK_SIZES(t); \
  CHECK_MATRIX(t.G, expected_G); \
  CHECK_VECTOR(t.g0, expected_g0); \
  BOOST_CHECK_EQUAL(t.g00, expected_g00); \
} while(false)

BOOST_AUTO_TEST_CASE(TranslateQuadraticExpression) {
  Variable a, b;

  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(0, {}, {}, 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(1, {}, {}, 1);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a, {{0}}, {1}, 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(1 + a, {{0}}, {1}, 1);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a*a, {{2}}, {0}, 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(1 + a*a, {{2}}, {0}, 1);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(1 + a + a*a, {{2}}, {1}, 1);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(0.5*a*a, {{1}}, {0}, 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a*a + b*b, ({{2, 0}, {0, 2}}), ({0, 0}), 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a*a + 0.5*b*b, ({{2, 0}, {0, 1}}), ({0, 0}), 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(0.5*b*b + a*a, ({{2, 0}, {0, 1}}), ({0, 0}), 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a*b, ({{0, 1}, {1, 0}}), ({0, 0}), 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(b*a, ({{0, 1}, {1, 0}}), ({0, 0}), 0);
  CHECK_QUADRATIC_EXPRESSION_TRANSLATION(a*a + 2*b*b + 3*b*a + 4*a + 5*b + 6, ({{2, 3}, {3, 4}}), ({4, 5}), 6);
}

#define CHECK_EQUALITY_CONSTRAINT_TRANSLATION(c, _CE, _CE0) do { \
  const Translation t = translate(0, {c}); \
  const std::vector<std::vector<double>> expected_CE _CE; \
  const std::vector<double> expected_ce0 _CE0; \
  CHECK_SIZES(t); \
  CHECK_MATRIX(t.CE, expected_CE); \
  CHECK_VECTOR(t.ce0, expected_ce0); \
} while(false)

BOOST_AUTO_TEST_CASE(TranslateEqualityConstraint) {
  Variable a, b;

  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a == 0, {{1}}, {0});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a == 1, {{1}}, {-1});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(2*a == 1, {{2}}, {-1});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a == 0.5, {{1}}, {-0.5});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a - a == 0, {{0}}, {0});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a + b == 0, ({{1}, {1}}), {0});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a + b + 2 == 0, ({{1}, {1}}), {2});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(b + a + 2 == 0, ({{1}, {1}}), {2});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(a + 2*b + 3 == 0, ({{1}, {2}}), {3});
  CHECK_EQUALITY_CONSTRAINT_TRANSLATION(b + 2*a + 3 == 0, ({{2}, {1}}), {3});
}

#define CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(c, _CI, _CI0) do { \
  const Translation t = translate(0, {c}); \
  const std::vector<std::vector<double>> expected_CI _CI; \
  const std::vector<double> expected_ci0 _CI0; \
  CHECK_SIZES(t); \
  CHECK_MATRIX(t.CI, expected_CI); \
  CHECK_VECTOR(t.ci0, expected_ci0); \
} while(false)

BOOST_AUTO_TEST_CASE(TranslateGreaterThanConstraint) {
  Variable a, b;

  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a >= 0, {{1}}, {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a >= 1, {{1}}, {-1});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(2*a >= 1, {{2}}, {-1});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a >= 0.5, {{1}}, {-0.5});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a - a >= 0, {{0}}, {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + b >= 0, ({{1}, {1}}), {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + b + 2 >= 0, ({{1}, {1}}), {2});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(b + a + 2 >= 0, ({{1}, {1}}), {2});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + 2*b + 3 >= 0, ({{1}, {2}}), {3});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(b + 2*a + 3 >= 0, ({{2}, {1}}), {3});
}

BOOST_AUTO_TEST_CASE(TranslateLessThanConstraint) {
  Variable a, b;

  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a <= 0, {{-1}}, {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a <= 1, {{-1}}, {1});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(2*a <= 1, {{-2}}, {1});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a <= 0.5, {{-1}}, {0.5});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a - a <= 0, {{0}}, {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + b <= 0, ({{-1}, {-1}}), {0});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + b + 2 <= 0, ({{-1}, {-1}}), {-2});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(b + a + 2 <= 0, ({{-1}, {-1}}), {-2});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(a + 2*b + 3 <= 0, ({{-1}, {-2}}), {-3});
  CHECK_INEQUALITY_CONSTRAINT_TRANSLATION(b + 2*a + 3 <= 0, ({{-2}, {-1}}), {-3});
}

auto d2(Variable left, Variable right, float l0) {
  auto d = (right - left) - l0;
  return d * d;
}

BOOST_AUTO_TEST_CASE(MixedConstraints) {
  Variable a, b, c;

  Translation t = translate(0, {
    a == 1,
    a <= 4,
    b >= 8,
    a >= 7,
    b == 2,
    c >= 9,
    b <= 5,
    c <= 6,
  });

  CHECK_SIZES(t);

  CHECK_MATRIX(t.CE, std::vector<std::vector<double>>({{1, 0}, {0, 1}, {0, 0}}));
  CHECK_VECTOR(t.ce0, std::vector<double>({-1, -2}));

  CHECK_MATRIX(t.CI, std::vector<std::vector<double>>({{-1, 0, 1, 0, 0, 0}, {0, 1, 0, 0, -1, 0}, {0, 0, 0, 1, 0, -1}}));
  CHECK_VECTOR(t.ci0, std::vector<double>({4, -8, -7, -9, 5, 6}));
}

BOOST_AUTO_TEST_CASE(SpringChainExample) {
  Variable x0, x1, x2, x3, x4;
  Translation t = translate(
    0.5 * (
      1 * d2(x0, x1, 2)
      + 1 * d2(x1, x2, 3)
      + 10 * d2(x2, x3, 5)
      + 1 * d2(x3, x4, 2)
    ),
    {x0 == 0, x4 == 10}
  );

  CHECK_SIZES(t);

  CHECK_MATRIX(t.G, std::vector<std::vector<double>>({
    { 1, -1,   0,   0,  0},
    {-1,  2,  -1,   0,  0},
    { 0, -1,  11, -10,  0},
    { 0,  0, -10,  11, -1},
    { 0,  0,   0,  -1,  1},
  }));
  CHECK_VECTOR(t.g0, std::vector<double>({2, 1, 47, -48, -2}));
  BOOST_CHECK_EQUAL(t.g00, 133.5);

  CHECK_MATRIX(t.CE, std::vector<std::vector<double>>({{1, 0}, {0, 0}, {0, 0}, {0, 0}, {0, 1}}));
  CHECK_VECTOR(t.ce0, std::vector<double>({0, -10}));

  CHECK_MATRIX(t.CI, std::vector<std::vector<double>>({{}, {}, {}, {}, {}}));
  CHECK_VECTOR(t.ci0, std::vector<double>({}));
}

BOOST_AUTO_TEST_CASE(QuickStart) {
  Variable a, b, c;

  Translation t = translate(
    a + b + (a - b) * (a - b) + c + (b - c) * (b - c),
    {a <= 1, c >= 4, a - 2 * b <= 12}
  );

  CHECK_SIZES(t);

  CHECK_MATRIX(t.G, std::vector<std::vector<double>>({
    {2, -2, 0},
    {-2, 4, -2},
    {0, -2, 2},
  }));
  CHECK_VECTOR(t.g0, std::vector<double>({1, 1, 1}));
  BOOST_CHECK_EQUAL(t.g00, 0);

  CHECK_MATRIX(t.CE, std::vector<std::vector<double>>({{}, {}, {}}));
  CHECK_VECTOR(t.ce0, std::vector<double>({}));

  CHECK_MATRIX(t.CI, std::vector<std::vector<double>>({
    {-1, 0, -1},
    {0, 0, 2},
    {0, 1, 0},
  }));
  CHECK_VECTOR(t.ci0, std::vector<double>({1, -4, 12}));
}


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

using namespace Eigen;

int main()
{
  Matrix4d m;
  m.resize(4,4); // no operation
  std::cout << "The matrix m is of size "
            << m.rows() << "x" << m.cols() << std::endl;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

/**
 * @file Solve a double integrator optimal control problem as a nonlinear program.
 */

#include <Eigen/Core>
#include <smooth/feedback/collocation/dyn_error.hpp>
#include <smooth/feedback/compat/ipopt.hpp>
#include <smooth/feedback/ocp_to_nlp.hpp>

#include <chrono>
#include <iostream>

#include "ocp_doubleintegrator.hpp"

#ifdef ENABLE_PLOTTING
#include "common.hpp"
#include <matplot/matplot.h>
#endif

int main()
{
  smooth::feedback::test_ocp_derivatives(ocp_di);

  // target optimality
  double target_err = 1e-6;

  // define mesh
  smooth::feedback::Mesh<5, 10> mesh;

  // declare solution variable
  std::vector<decltype(ocp_di)::Solution> sols;
  std::optional<smooth::feedback::NLPSolution> nlpsol;

  const auto t0 = std::chrono::high_resolution_clock::now();

  for (auto iter = 0u; iter < 10; ++iter) {
    std::cout << "---------- ITERATION " << iter << " ----------" << std::endl;
    std::cout << "mesh: " << mesh.N_ivals() << " intervals, " << mesh.N_colloc()
              << " collocation pts" << std::endl;

    // transcribe optimal control problem to nonlinear programming problem
    const auto nlp = smooth::feedback::ocp_to_nlp<smooth::diff::Type::Analytic>(ocp_di, mesh);

    // solve nonlinear programming problem
    std::cout << "solving..." << std::endl;
    nlpsol = smooth::feedback::solve_nlp_ipopt(
      nlp,
      nlpsol,
      {
        {"print_level", 5},
      },
      {
        {"linear_solver", "mumps"},
        {"hessian_approximation", "exact"},
        // {"derivative_test", "first-order"},
        // {"derivative_test_print_all", "yes"},
        {"print_timing_statistics", "yes"},
      },
      {
        {"tol", 1e-6},
      });

    // convert solution of nlp insto solution of ocp_di
    auto sol = smooth::feedback::nlpsol_to_ocpsol(ocp_di, mesh, nlpsol.value());
    sols.push_back(sol);

    // calculate errors
    mesh.increase_degrees();
    auto errs = smooth::feedback::mesh_dyn_error(ocp_di.f, mesh, sol.t0, sol.tf, sol.x, sol.u);
    mesh.decrease_degrees();

    std::cout << "interval errors " << errs.transpose() << std::endl;

    if (errs.maxCoeff() > target_err) {
      mesh.refine_errors(errs, 0.1 * target_err);
      nlpsol = smooth::feedback::ocpsol_to_nlpsol(ocp_di, mesh, sol);
    } else {
      break;
    }
  }

  const auto dur = std::chrono::high_resolution_clock::now() - t0;

  std::cout << "TOTAL TIME: " << std::chrono::duration_cast<std::chrono::milliseconds>(dur).count()
            << "ms" << std::endl;

#ifdef ENABLE_PLOTTING
  using namespace matplot;

  const auto tt = linspace(0., sols.back().tf, 500);
  const auto tt_nodes =
    r2v(mesh.all_nodes() | std::views::transform([&](double d) { return d * sols.back().tf; }));
  const auto tt_weights = r2v(mesh.all_weights());

  figure();
  hold(on);
  plot(tt_nodes, transform(tt_nodes, [](auto) { return 0; }), "xk")->marker_size(10);
  plot(tt_nodes, tt_weights, "or")->marker_size(5);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.x(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.x(t).y(); }), "-b")->line_width(lw);
  }
  matplot::legend({"nodes", "weights", "pos", "vel"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).x(); }), "-r")->line_width(lw);
    plot(tt, transform(tt, [&](double t) { return sol.lambda_dyn(t).y(); }), "-b")->line_width(lw);
  }
  matplot::legend({"lambda_x", "lambda_y"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&](double t) { return sol.lambda_cr(t).x(); }), "-r")->line_width(lw);
  }
  matplot::legend(std::vector<std::string>{"lambda_{cr}"});

  figure();
  hold(on);
  for (auto it = 0u; const auto & sol : sols) {
    int lw = it++ + 1 < sols.size() ? 1 : 2;
    plot(tt, transform(tt, [&sol](double t) { return sol.u(t).x(); }), "-r")->line_width(lw);
  }
  matplot::legend(std::vector<std::string>{"input"});

  show();
#endif

  return EXIT_SUCCESS;
}


#include <algorithm>
#include <boost/math/tools/roots.hpp>
#include <cmath>
#include <cstdlib>
#include <ibs>
#include <iostream>
#include <map>
#include <math.h>
#include <string>
#include <vector>

namespace CTESYNCH {
/*
 *****************************************************************************
 *****************************************************************************
 * FUNCTOR TO RETURN VOLTAGE * CHARGE - U0 AND DERIVATIVE AS TUPLE
 * THIS IS USED AS INPUT FOR BOOST NEWTON RAPHSON ROOT SEARCH.
 *****************************************************************************
 * Authors:
 *  - Tom Mertens
 *
 * History:
 *  - 10/08/2021 : updated version from original ste code
 *****************************************************************************
 * Arguments:
 * ----------
 *  - T const &target
 *      Target value for the voltage (RF compensate for U0 so U0 in eV)
 *  - std::vector<double> &voltages
 *      Voltages of RF systems IMPORTANT: first element is for main RF
 *  - std::vector<double> &harmonicNumbers
 *      Harmonic numbers  IMPORTANT: first element is for main RF
 *  - double charge
 *      Particle charge
 *  - T const &phi
 *      phase in rad
 ******************************************************************************
 ******************************************************************************
 */
template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, double charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {

    // init
    T vrf = ch * volts[0] * sin(phi);
    T dvrf = ch * volts[0] * cos(phi);

    // add the rest taking harmonic numbers into account
    for (int i = 1; i < hs.size(); i++) {
      vrf += ch * volts[i] * sin((hs[i] / hs[0]) * phi);
      dvrf += ch * volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
    }

    std::tuple<double, double> out = {vrf - U0, dvrf};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

/*
================================================================================
================================================================================
BOOST NEWTON RAPHSON ROOT SEARCH FOR SYNCHRONOUS PHASE.
================================================================================

================================================================================
Arguments:

//phi is in rad
================================================================================
================================================================================
*/
template <class T>
T synchronousPhaseFunctorRoot(T x, std::vector<double> &voltages,
                              std::vector<double> &harmnumbers, double charge,
                              T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
};

/********************************************************************************
 ********************************************************************************
 * CALCULATE TOTAL RF VOLTAGE FOR GIVEN PHASE (IN RAD)
 *
 ********************************************************************************
 */
double VoltageRf(double phi, std::vector<double> &volts,
                 std::vector<double> &hs) {
  double vrf = volts[0] * sin(phi);

  for (int i = 1; i < hs.size(); i++) {
    vrf += volts[i] * sin((hs[i] / hs[0]) * phi);
  }

  return vrf;
};

double VoltageRfPrime(double phi, double charge, std::vector<double> &volts,
                      std::vector<double> &hs) {
  // init
  double vrf = volts[0] * cos(phi);

  // add other rfs
  for (int i = 1; i < volts.size(); i++) {
    vrf += volts[i] * (hs[i] / hs[0]) * cos((hs[i] / hs[0]) * phi);
  }

  // V -> eV
  vrf *= charge;
  return vrf;
}

double SynchrotronTune(std::map<std::string, double> &twheader,
                       std::map<std::string, double> &longparams,
                       std::vector<double> &volts, std::vector<double> &hs) {

  double charge = twheader["CHARGE"];
  double phis = longparams["phis"];
  double pc = twheader["PC"];

  return sqrt(hs[0] * twheader["eta"] *
              fabs(charge * VoltageRfPrime(phis, charge, volts, hs)) /
              (2 * pi * pc * 1e9));
}
} // namespace CTESYNCH


#include <iostream>

#include <El.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

#include <H5Cpp.h>

namespace bmpi =  boost::mpi;
namespace skybase = skylark::base;
namespace skysketch =  skylark::sketch;
namespace skynla = skylark::nla;
namespace skyalg = skylark::algorithms;
namespace skyutil = skylark::utility;

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

    El::Initialize(argc, argv);
    skybase::context_t context(23234);

    skybase::sparse_matrix_t<double> A;
    El::Matrix<double> b;

    boost::mpi::timer timer;

    // Load A and b from HDF5 file
    std::cout << "Reading the matrix... ";
    std::cout.flush();
    timer.restart();
    H5::H5File in(argv[1], H5F_ACC_RDONLY);
    skyutil::io::ReadHDF5(in, "A", A);
    in.close();
    std::cout <<"took " << boost::format("%.2e") % timer.elapsed() << " sec\n";

    timer.restart();
    El::Matrix<double> u_min, u_max, v_min, v_max;
    double cond, sigma_min, sigma_min_c, sigma_max;
    skynla::condest_params_t condest_params;
    condest_params.am_i_printing = true;
    condest_params.log_level = 2;
    condest_params.iter_lim = 10000;
    skynla::CondEst(A, cond, sigma_max, v_max, u_max,
        sigma_min, sigma_min_c, v_min, u_min, context, condest_params);
    std::cout <<"Took " << boost::format("%.2e") % timer.elapsed() << " sec\n";
    std::cout << "Condition number = " << cond
              << " sigma_max = " << sigma_max
              << " sigma_min = " << sigma_min << std::endl;

    return 0;
}


#include <boost/math/distributions/inverse_gamma.hpp>


#define CATCH_CONFIG_MAIN

#include "catch.hpp"
#include "helpers.hpp"
#include <Eigen/Dense>
#include <celerite2/celerite2.h>

using namespace celerite2::test;
using namespace celerite2::core;

TEMPLATE_LIST_TEST_CASE("check the results of solve_lower", "[solve_lower]", TestKernels) {
  SETUP_TEST(50);

  Matrix K, S, Z, F;
  to_dense(x, c, a, U, V, K);

  // Do the Cholesky using celerite
  int flag = factor(x, c, a, U, V, a, V, S);
  REQUIRE(flag == 0);

  // Brute force the Cholesky factorization
  Eigen::LDLT<Eigen::MatrixXd> LDLT(K);
  Eigen::MatrixXd expect = LDLT.matrixL().solve(Y);

  SECTION("general") {
    solve_lower(x, c, U, V, Y, Z, F);
    double resid = (Z - expect).array().abs().maxCoeff();
    REQUIRE(resid < 1e-12);
  }

  SECTION("no grad") {
    solve_lower(x, c, U, V, Y, Z);
    double resid = (Z - expect).array().abs().maxCoeff();
    REQUIRE(resid < 1e-12);
  }

  SECTION("inplace") {
    Z = Y;
    solve_lower(x, c, U, V, Z, Z, F);
    double resid = (Z - expect).array().abs().maxCoeff();
    REQUIRE(resid < 1e-12);
  }
}


///////////////////////////////////////////////////////////////////////////////
// ars::test::normal_distribution.cpp                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_TEST_NORMAL_DISTRIBUTION_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_TEST_NORMAL_DISTRIBUTION_HPP_ER_2009
#include <iostream>
#include <string>
#include <boost/random/mersenne_twister.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/ars/test/standard_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{
namespace test{

struct normal_distribution{

    // Samples from normal distribution using adaptive rejection sampling and
    // outputs convergence statistics
	template<typename T>
	static void call(
    	T mu,
    	T sigma,
    	T init_0, //must be < mu
    	T init_1, //must be > mu
    	unsigned n1,    // 1e2
    	unsigned n2,    // 10
    	unsigned n3,    // 1
    	unsigned n4,    // 10
    	unsigned n_max_reject,
    	std::ostream& out
	)
	{

    	using namespace boost;
    	typedef double                                          value_t;
    	typedef ars::constant<value_t>                          const_;
    	typedef math::normal_distribution<value_t>              mdist_t;
    	typedef boost::mt19937                                  urng_t;

    	const value_t inf_ = const_::inf_;

    	mdist_t mdist(mu,sigma);
    	urng_t urng;

    	ars::test::standard_distribution::call(
        	mdist,
        	inf_,
        	inf_,
        	init_0,
        	init_1,
        	urng,
        	n1,
        	n2,
        	n3,
        	n4,
        	n_max_reject,
        	out
    	);
    }
};

}//test
}// ars
}// detail
}// statistics
}// boost

#endif


#include <SFML/System.hpp>
#include <SFML/Graphics.hpp>
#include <SFML/Window.hpp>
#include <SFML/Audio.hpp>
#include <boost/math/constants/constants.hpp>

#include <map>
#include <iostream>
#include <cassert>
#include <cmath>

#include "GameBall.h"
#include "Game.h"
#include "Logger.h"

GameBall::GameBall() :
    _velocity(230.0f), //pixels per second speed
    _elapsedTimeSinceStart(0.0f)
{
    //constructor
    Load("resources/ball.png");
    assert(IsLoaded());

    GetSprite().setOrigin(15,15);

    //Generates a random number between 1 and 360.
    float random_integer = std::rand() % 360 + 1;
    _angle = random_integer;
    INFO << "Angle: " << _angle;
}

GameBall::~GameBall()
{
    //destructor
}

//Parameter is the time since last frame in seconds. VERY small number.
void GameBall::Update(float elapsedTime)
{
    //INFO << "Time since last frame: " << elapsedTime;
    _elapsedTimeSinceStart += elapsedTime;

    // Delay game from starting until 3 seconds have passed
    if (_elapsedTimeSinceStart < 3.0f)
        return;

    float moveAmount = _velocity  * elapsedTime;

    float moveByX = LinearVelocityX(_angle) * moveAmount;
    float moveByY = LinearVelocityY(_angle) * moveAmount;

    //INFO << GetPosition().x << GetPosition().y;

    //collide with the left side of the screen
    if (GetPosition().x + moveByX <= 0 + GetWidth()/2 || GetPosition().x + GetHeight()/2 + moveByX >= Game::width)
    {
        //Ricochet!
        _angle = 360.0f - _angle;
        if (_angle > 260.0f && _angle < 280.0f)
            _angle += 20.0f;
        if (_angle > 80.0f && _angle < 100.0f)
            _angle += 20.0f;

        moveByX = -moveByX;
    }

    PlayerPaddle* player1 = dynamic_cast<PlayerPaddle*>(Game::GetGameObjectManager().Get("Paddle1"));
    if (player1 != NULL)
    {
        sf::Rect<float> p1BB = player1->GetBoundingRect();

        if (p1BB.intersects(GetBoundingRect()))
        {
            _angle =  360.0f - (_angle - 180.0f);
            if (_angle > 360.0f)
                _angle -= 360.0f;

            moveByY = -moveByY;

            // Make sure ball isn't inside paddle
            if (GetBoundingRect().width > player1->GetBoundingRect().top)
            {
                SetPosition(GetPosition().x,player1->GetBoundingRect().top - GetWidth()/2 -1 );
            }

            // Now add "English" based on the players velocity.
            float playerVelocity = player1->GetVelocity();

            if (playerVelocity < 0)
            {
                // moving left
                _angle -= 20.0f;
                if(_angle < 0 )
                    _angle = 360.0f - _angle;
            }
            else if(playerVelocity > 0)
            {
                _angle += 20.0f;
                if(_angle > 360.0f)
                    _angle = _angle - 360.0f;
            }

            _velocity += 5.0f;
        }

        if (GetPosition().y - GetHeight()/2 <= 0)
        {
            _angle =  180 - _angle;
            moveByY = -moveByY;
        }

        if(GetPosition().y + GetHeight()/2 + moveByY >= Game::height)
        {
            INFO << "Moving to middle screen...";
            Reset();
        }

        GetSprite().move(moveByX,moveByY);
    } else {
        ERROR << "Something bad happened with the casting...";
    }
}

void GameBall::Reset()
{
    _elapsedTimeSinceStart = 0.0f;
    GetSprite().setPosition(Game::width/2, Game::height/2);
    _angle = (std::rand()%360)+1;
    _velocity = 230.0f;
}

float GameBall::LinearVelocityX(float angle)
{
    angle -= 90;
    if (angle < 0)
        angle = 360 + angle;
    const double pi = boost::math::constants::pi<float>();
    return (float)std::cos( angle * ( pi / 180.0f ));
}

float GameBall::LinearVelocityY(float angle)
{
    angle -= 90;
    if (angle < 0)
        angle = 360 + angle;
    const double pi = boost::math::constants::pi<float>();
    return (float)std::sin( angle * ( pi / 180.0f ));
}


/*
 [auto_generated]
 libs/numeric/odeint/test/generic_stepper.cpp

 [begin_description]
 This file tests the generic stepper.
 [end_description]

 Copyright 2011 Mario Mulansky
 Copyright 2012 Karsten Ahnert

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

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

#define BOOST_TEST_MODULE odeint_generic_stepper

#include <iostream>
#include <utility>

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/explicit_generic_rk.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>

#include <boost/array.hpp>

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

namespace fusion = boost::fusion;

typedef double value_type;
typedef boost::array< value_type , 2 > state_type;

void sys( const state_type &x , state_type &dxdt , const value_type &t )
{
    dxdt[ 0 ] = x[ 0 ] + 2 * x[ 1 ];
    dxdt[ 1 ] = x[ 1 ];
}

typedef explicit_generic_rk< 4 , 4 , state_type> rk_generic_type;
typedef runge_kutta4< state_type > rk4_generic_type;

const boost::array< double , 1 > a1 = {{ 0.5 }};
const boost::array< double , 2 > a2 = {{ 0.0 , 0.5 }};
const boost::array< double , 3 > a3 = {{ 0.0 , 0.0 , 1.0 }};

const rk_generic_type::coef_a_type a = fusion::make_vector( a1 , a2 , a3 );
const rk_generic_type::coef_b_type b = {{ 1.0/6 , 1.0/3 , 1.0/3 , 1.0/6 }};
const rk_generic_type::coef_c_type c = {{ 0.0 , 0.5 , 0.5 , 1.0 }};

typedef runge_kutta4_classic< state_type > rk4_type;

BOOST_AUTO_TEST_SUITE( generic_stepper_test )

BOOST_AUTO_TEST_CASE( test_generic_stepper )
{
    //simultaneously test copying
    rk_generic_type rk_generic_( a , b , c );
    rk_generic_type rk_generic = rk_generic_;

    rk4_generic_type rk4_generic_;
    rk4_generic_type rk4_generic = rk4_generic_;

    //std::cout << stepper;

    rk4_type rk4_;
    rk4_type rk4 = rk4_;

    typedef rk_generic_type::state_type state_type;
    typedef rk_generic_type::value_type stepper_value_type;
    typedef rk_generic_type::deriv_type deriv_type;
    typedef rk_generic_type::time_type time_type;

    state_type x = {{ 0.0 , 1.0 }};
    state_type y = x;
    state_type z = x;

    rk_generic.do_step( sys , x , 0.0 , 0.1 );

    rk4_generic.do_step( sys , y , 0.0 , 0.1 );

    rk4.do_step( sys , z , 0.0 , 0.1 );

    BOOST_CHECK_NE( 0.0 , x[0] );
    BOOST_CHECK_NE( 1.0 , x[1] );
    // compare with analytic solution of above system
    BOOST_CHECK_EQUAL( x[0] , y[0] );
    BOOST_CHECK_EQUAL( x[1] , y[1] );
    BOOST_CHECK_EQUAL( x[0] , z[0] );
    BOOST_CHECK_EQUAL( x[1] , z[1] );

}

BOOST_AUTO_TEST_SUITE_END()


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

// These are sample tests that show the different features of the framework

using namespace math;

template<typename T>
void testTrans2d()
{
	trans2d<T> m0(
		vec2<T>(0.6f, 0.2f),
		0.7f,
		vec2<T>(0.3f, 0.5f));

	{
		BOOST_CHECK(is_identity(trans2d<T>::identity, epsilon<T>()));
		trans2d<T> inv0 = inverse(trans2d<T>::identity);
		BOOST_CHECK(is_identity(inv0, epsilon<T>()));
	}

	{
		trans2d<T> inv0 = inverse(m0);
		trans2d<T> res0 = m0 * inv0;
		BOOST_CHECK(is_identity(res0, 0.0001f));
	}
	{
		trans2d<T> m0;
		BOOST_CHECK(is_identity(m0));
	}
	{
		mat4<T> m0(mat4<T>::one);
		//this should leave m0 unchanged
		auto* t = new (&m0) trans2d<T>(trans2d<T>::uninitialized);

		BOOST_CHECK(m0 == mat4<T>::one);
	}
	{
		T rot = T(2.7);
		mat2<T> rotm(mat2<T>::rotation, rot);
		trans2d<T> t0(vec2<T>(23, 24), rotm, vec2<T>(1, 1));
		trans2d<T> t1(vec2<T>(23, 24), rot, vec2<T>(1, 1));

		BOOST_CHECK(t0 == t1);
		BOOST_CHECK(t0 != m0);
		BOOST_CHECK(t1 != m0);

		BOOST_CHECK(t0.get_translation() == vec2<T>(23, 24));
		BOOST_CHECK(equals(t0.get_scale(), vec2<T>(1, 1), T(0.001)));
		t0.post_scale(T(1) / t0.get_scale());
		BOOST_CHECK(equals(t0.get_rotation(), rotm, T(0.001)));
	}

	{
		T rot = T(2.7);
		mat2<T> rotm(mat2<T>::rotation, rot);
		trans2d<T> t0(vec2<T>(23, 24), rotm, vec2<T>(1, 1));
		trans2d<T> t1(vec2<T>(23, 24), rot, vec2<T>(1, 1));

		auto res0 = t0 * t1;
		trans2d<T> res1;
		res1.mat = t0.mat * t1.mat;
		res1.repair();

		BOOST_CHECK(res0 == res1);
		t0 *= t1;
		BOOST_CHECK(t0 == res1);
	}
	{
		T rot = T(2.7);
		mat2<T> rotm(mat2<T>::rotation, rot);
		trans2d<T> t0(vec2<T>(23, 24), rotm, vec2<T>(1, 1));
		t0.set_identity();
		BOOST_CHECK(t0 == trans2d<T>::identity);
	}
	{
		T rot = T(2.7);
		mat2<T> rotm(mat2<T>::rotation, rot);
		trans2d<T> t0(vec2<T>(23, 24), rotm, vec2<T>(1, 1));
		t0.set_rotation_identity();
		BOOST_CHECK(t0.get_rotation() == mat2<T>::identity);
		BOOST_CHECK(t0.get_translation() == vec2<T>(23, 24));
	}
	{
		mat2<T> m0(0.72f);
		trans2d<T> m1(vec2<T>(1, 2), m0, vec2<T>(1, 1));
		BOOST_CHECK(m1.get_axis_x() == vec2<T>(0.72f, 0.72f));
		BOOST_CHECK(m1.get_axis_y() == vec2<T>(0.72f, 0.72f));
		BOOST_CHECK(m1.get_translation() == vec2<T>(1, 2));
	}

	int a = 0;
}

BOOST_AUTO_TEST_CASE(Trans2d)
{
	testTrans2d<float>();
//	testTrans2d<double>();
}


//
//  Copyright (c) 2011-2013 Vladimir Chalupecky
//
//  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.

#include <umeshu/Delaunay_mesher.h>
#include <umeshu/Delaunay_triangulation.h>
#include <umeshu/Delaunay_triangulation_items.h>
#include <umeshu/Exceptions.h>
#include <umeshu/Polygon.h>
#include <umeshu/Relaxer.h>
#include <umeshu/Triangulator.h>
#include <umeshu/io/OBJ.h>
#include <umeshu/io/OFF.h>
#include <umeshu/io/PLY.h>
#include <umeshu/io/EPS.h>
#include <umeshu/io/STL.h>

#include <boost/program_options.hpp>

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

typedef Delaunay_triangulation< Delaunay_triangulation_items_with_id > Mesh;
typedef Mesh::Node_handle     Node_handle;
typedef Mesh::Halfedge_handle Halfedge_handle;
typedef Mesh::Edge_handle     Edge_handle;
typedef Mesh::Face_handle     Face_handle;
typedef Delaunay_mesher< Mesh > Mesher;
typedef Relaxer< Mesh >       Relax;

int main( int argc, const char* argv[] )
{
  double max_area;
  double min_angle;

  po::options_description po_desc( "Allowed options" );
  po_desc.add_options()
    ( "help", "produce help message" )
    ( "max-size,s", po::value<double>( &max_area )->default_value( 0.01 ), "set the maximum triangle area for the refinement algorithm" )
    ( "min-angle,a", po::value<double>( &min_angle )->default_value( 21 ), "set the minimum angle for the refinement algorithm" )
    ( "input-file", po::value<std::string>(), "input file describing the polygonal boundary (in Well-Known Text format)");

  po::positional_options_description po_pdesc;
  po_pdesc.add("input-file", -1);

  po::variables_map po_vm;
  po::store( po::command_line_parser( argc, argv ).options( po_desc ).positional( po_pdesc ).run(), po_vm );
  po::notify( po_vm );

  if ( po_vm.count( "help" ) )
  {
    std::cout << po_desc << std::endl;
    return EXIT_SUCCESS;
  }

  if ( ! po_vm.count( "input-file" ) )
  {
    std::cout << "Name of the input file not specified\n";
    std::cout << po_desc << std::endl;
    return EXIT_FAILURE;
  }

  std::cout << "Parameters used:" << std::endl
    << "  maximum triangle area = " << max_area << std::endl
    << "  minimum angle = " << min_angle << std::endl;

  try
  {
    Polygon boundary;
    read_polygon( po_vm["input-file"].as< std::string >(), boundary );

    Mesh mesh;

    Triangulator<Mesh> triangulator;
    triangulator.triangulate( boundary, mesh );
    io::write_eps( "mesh_1.eps", mesh );

    mesh.make_cdt();
    io::write_eps( "mesh_2.eps", mesh );

    Mesher mesher;
    mesher.refine( mesh, max_area, min_angle );
    io::write_eps( "mesh_3.eps", mesh );
    io::write_stl( "mesh_3.stl", mesh );
    io::write_off( "mesh_3.off", mesh );
    io::write_obj( "mesh_3.obj", mesh );
    io::write_ply( "mesh_3.ply", mesh );

    Relax relax;
    relax.relax( mesh );
    io::write_eps( "mesh_4.eps", mesh );

    // smoother smooth;
    // smooth.smooth(m, 1);
    // Postscript_stream ps5("mesh_5.eps", m.bounding_box());
    // ps5 << m;

    // // code does not pass a debug assert:
    // meshgen.refine(0.0001000, 25);
    // Postscript_stream ps6("mesh_6.eps", m.bounding_box());
    // ps6 << m;

    // // smooth.smooth(m, 5);
    // // Postscript_stream ps7("mesh_7.eps", m.bounding_box());
    // // ps7 << m;

    std::cout << "Final mesh:" << std::endl
      << "  # nodes: " << mesh.number_of_nodes() << std::endl
      << "  # edges: " << mesh.number_of_edges() << std::endl
      << "  # faces: " << mesh.number_of_faces() << std::endl;
  }
  catch ( boost::exception& e )
  {
    std::cerr << boost::diagnostic_information( e );
  }

  return EXIT_SUCCESS;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <vector>
#include <fstream>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  K;
typedef K::Point_3                                Point_3;

int main(int argc, char* argv[]){
  std::ifstream in( (argc>1)? argv[1] : "data/cloud.pol");
  boost::property_tree::ptree tree;
  boost::property_tree::read_xml(in, tree);

  std::vector<Point_3> points;

  for(boost::property_tree::ptree::value_type& node : tree.get_child("PolySet.Polygon")){
    boost::property_tree::ptree subtree = node.second;
    if( node.first == "Point" ){
      for( boost::property_tree::ptree::value_type const& v : subtree.get_child( "" ) ) {
        std::string label = v.first;

        if ( label == "<xmlattr>" ) {
          Point_3 p(subtree.get<double>( label+".X"),
                    subtree.get<double>( label+".Y"),
                    subtree.get<double>( label+".Z"));
          points.push_back(p);
        }
      }
    }
  }

  std::cout << points.size() << " points read"<< std::endl;
  return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 6, 6> test_float_6_6(const Eigen::Matrix<float, 6, 6> & M)
{
	return M;
}
void export_float_6_6()
{
	boost::python::def("test_float_6_6",test_float_6_6);
}


#include "catch.h"
#include "../data_structures/Vector.h"
#include "../math/Random.h"
#include "../math/VectorMath.h"

// optional test used for benchmarking, set to 0 to disable, 1 to enable
#if 0

// boost time helpers
#include <boost/date_time/posix_time/posix_time.hpp>
namespace bpt = boost::posix_time;
#define bpt_now() bpt::microsec_clock::local_time()

TEST_CASE("Benchmark Dot Product")
{
    GapsRandomState randState(123);
    GapsRng rng(&randState);

    std::vector<Vector> mVecs;
    for (unsigned i = 0; i < 1300; ++i)
    {
        mVecs.push_back(Vector(50000));
        for (unsigned j = 0; j < mVecs[i].size(); ++j)
        {
            mVecs[i][j] = rng.uniform(0.f, 100.f);
        }
    }

    float sum = 0.f;
    bpt::ptime start = bpt_now();
    for (unsigned i = 0; i < mVecs.size(); ++i)
    {
        for (unsigned j = i; j < mVecs.size(); ++j)
        {
            sum += gaps::dot(mVecs[i], mVecs[j]);
        }
    }
    bpt::time_duration diff = bpt_now() - start;
    gaps_printf("-------\n-------\n-------\n-------\n", sum);
    gaps_printf("sum: %f\n", sum);
    gaps_printf("dot product milliseconds: %lu\n", diff.total_milliseconds());
    gaps_printf("-------\n-------\n-------\n-------\n", sum);
}
#endif

TEST_CASE("Test Vector.h")
{
    GapsRandomState randState(123);

    SECTION("Test size constructor")
    {
        Vector v(100);
        REQUIRE(v.size() == 100);
        REQUIRE(gaps::isVectorZero(v));
        REQUIRE(gaps::sum(v) == 0.f);
    }

    SECTION("Test std::vector constructor")
    {
        GapsRng rng(&randState);
        std::vector<float> in_v;
        for (unsigned n = 0; n < 1000; ++n)
        {
            in_v.push_back(rng.uniform());
        }
        Vector v(in_v);

        REQUIRE(v.size() == 1000);
        REQUIRE(!gaps::isVectorZero(v));
        REQUIRE(gaps::max(v) <= 1.f);
        REQUIRE(gaps::min(v) >= 0.f);
    }

    SECTION("TEST += operator")
    {
        GapsRng rng(&randState);
        std::vector<float> in_v;
        for (unsigned n = 0; n < 1000; ++n)
        {
            in_v.push_back(rng.uniform());
        }
        Vector v(in_v);

        float s = gaps::sum(v);
        v += v;
        REQUIRE(gaps::sum(v) == 2.f * s);
    }
}


/* ----------------------------------------------------------------------------
 * 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;

// custom typedefs
typedef std::vector<GRBLinExpr> GRBVector;
typedef std::vector<std::vector<GRBLinExpr>> GRBMatrix;

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;
}

inline void addVectorEqConstraint(GRBModel& m, const GRBVector a, const Eigen::Vector3d& b)
{
  for (int i = 0; i < a.size(); i++)
  {
    m.addConstr(a[i] == b[i]);
  }
}

inline void addVectorLessEqualConstraint(GRBModel& m, const GRBVector a, const Eigen::Vector3d& b)
{
  for (int i = 0; i < a.size(); i++)
  {
    m.addConstr(a[i] <= b[i]);
  }
}

inline void addVectorGreaterEqualConstraint(GRBModel& m, const GRBVector a, const Eigen::Vector3d& b)
{
  for (int i = 0; i < a.size(); i++)
  {
    m.addConstr(a[i] >= b[i]);
  }
}

inline void resetCompleteModel(GRBModel& m)
{
  GRBConstr* c = 0;
  c = m.getConstrs();
  for (int i = 0; i < m.get(GRB_IntAttr_NumConstrs); ++i)
  {
    m.remove(c[i]);
  }

  GRBQConstr* cq = 0;
  cq = m.getQConstrs();
  for (int i = 0; i < m.get(GRB_IntAttr_NumQConstrs); ++i)
  {
    m.remove(cq[i]);
  }

  GRBGenConstr* gc = 0;
  gc = m.getGenConstrs();
  for (int i = 0; i < m.get(GRB_IntAttr_NumGenConstrs); ++i)
  {
    m.remove(gc[i]);
  }

  GRBVar* vars = 0;
  vars = m.getVars();
  for (int i = 0; i < m.get(GRB_IntAttr_NumVars); ++i)
  {
    m.remove(vars[i]);
  }

  m.reset();  // Note that this function, only by itself, does NOT remove vars or constraints
}

// See https://www.gurobi.com/documentation/9.0/refman/optimization_status_codes.html#sec:StatusCodes
inline void printGurobiStatus(int status)
{
  switch (status)
  {
    case GRB_LOADED:
      std::cout << "GUROBI Status: GRB_LOADED" << std::endl;
      break;
    case GRB_OPTIMAL:
      std::cout << "GUROBI Status: GRB_OPTIMAL" << std::endl;
      break;
    case GRB_INFEASIBLE:
      std::cout << "GUROBI Status: GRB_INFEASIBLE" << std::endl;
      break;
    case GRB_INF_OR_UNBD:
      std::cout << "GUROBI Status: GRB_INF_OR_UNBD" << std::endl;
      break;
    case GRB_UNBOUNDED:
      std::cout << "GUROBI Status: GRB_UNBOUNDED" << std::endl;
      break;
    case GRB_CUTOFF:
      std::cout << "GUROBI Status: GRB_CUTOFF" << std::endl;
      break;
    case GRB_ITERATION_LIMIT:
      std::cout << "GUROBI Status: GRB_ITERATION_LIMIT" << std::endl;
      break;
    case GRB_NODE_LIMIT:
      std::cout << "GUROBI Status: GRB_NODE_LIMIT" << std::endl;
      break;
    case GRB_TIME_LIMIT:
      std::cout << "GUROBI Status: GRB_TIME_LIMIT" << std::endl;
      break;
    case GRB_SOLUTION_LIMIT:
      std::cout << "GUROBI Status: GRB_SOLUTION_LIMIT" << std::endl;
      break;
    case GRB_INTERRUPTED:
      std::cout << "GUROBI Status: GRB_INTERRUPTED" << std::endl;
      break;
    case GRB_NUMERIC:
      std::cout << "GUROBI Status: GRB_NUMERIC" << std::endl;
      break;
    case GRB_SUBOPTIMAL:
      std::cout << "GUROBI Status: GRB_SUBOPTIMAL" << std::endl;
      break;
    case GRB_INPROGRESS:
      std::cout << "GUROBI Status: GRB_INPROGRESS" << std::endl;
      break;
    case GRB_USER_OBJ_LIMIT:
      std::cout << "GUROBI Status: GRB_USER_OBJ_LIMIT" << std::endl;
      break;
    default:
      std::cout << "GUROBI Status Code=: " << status << std::endl;
  }
}

template <typename T, typename R>
GRBVector matrixMultiply(const std::vector<std::vector<R>>& A, const std::vector<T>& x)
{
  GRBVector 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>
std::vector<GRBVector> matrixMultiply(const std::vector<std::vector<T>>& A, const std::vector<std::vector<double>>& B)
{
  std::vector<GRBVector> result(A.size(), GRBVector(B[0].size(), 0.0));  // Initialize all the
                                                                         // elements to zero

  for (int i = 0; i < A.size(); i++)  // multiply row if of A
  {
    for (int j = 0; j < B[0].size(); j++)  // times column j of B
    {
      GRBLinExpr lin_exp = 0;
      for (int m = 0; m < B.size(); m++)
      {
        lin_exp += A[i][m] * B[m][j];
      }
      result[i][j] = 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>  // Overload *
std::vector<T> operator*(const double& a, const std::vector<T>& b)
{
  std::vector<T> result;

  for (int i = 0; i < b.size(); i++)
  {
    result.push_back(a * b[i]);
  }

  return result;
}

template <typename T>
GRBVector operator-(const std::vector<T>& x, const std::vector<double>& b)
{
  GRBVector 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;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

template <typename T>
std::vector<T> eigenVector2std(const Eigen::Matrix<T, 3, 1>& x)  // TODO: Merge with the previous one?
{
  std::vector<T> result;
  for (int i = 0; i < x.rows(); i++)
  {
    result.push_back(x(i, 1));
  }
  return result;
}

inline std::vector<std::vector<double>> eigenMatrix2std(const Eigen::Matrix<double, -1, -1>& x)
{
  std::vector<std::vector<double>> result;

  for (int i = 0; i < x.rows(); i++)
  {
    std::vector<double> 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;
}

// // GRBLinExpr novale = A[i][m] * B[m][j];
// // std::cout << "B[m][j]=" << B[m][j] << std::endl;
// // std::cout << "novale.size() =" << novale.size() << std::endl;

// // std::cout << "novale.getCoeff(0) =" << novale.getCoeff(0) << std::endl;
// // std::cout << "novale.getCoeff(0) =" << novale.getCoeff(1) << std::endl;

// std::vector<std::vector<double>> matrixMultiply(const std::vector<std::vector<double>>& A,
//                                                 const std::vector<std::vector<double>>& B)
// {
//   std::vector<std::vector<double>> result(A.size(), std::vector<double>(B[0].size(), 0.0));  // Initialize all the
//                                                                                // elements to zero

//   for (int i = 0; i < A.size(); i++)  // multiply row if of A
//   {
//     for (int j = 0; j < B[0].size(); j++)  // times column j of B
//     {
//       double lin_exp = 0;
//       for (int m = 0; m < B.size(); m++)
//       {
//         lin_exp+ = + A[i][m] * B[m][j];
//       }
//       result[i][j] = lin_exp;
//     }
//   }
//   return result;
// }

// std::vector<std::vector<double>> tmp;

// std::vector<double> row1;
// row1.push_back(2.0);
// row1.push_back(1.0);

// std::vector<double> row2;
// row2.push_back(3.0);
// row2.push_back(-5.0);

// tmp.push_back(row1);
// tmp.push_back(row2);

// std::cout << "MINI TEST sizes" << std::endl;
// std::cout << "tmp.size()=" << tmp.size() << std::endl;
// std::cout << "tmp[0].size()=" << tmp[0].size() << std::endl;

// std::vector<std::vector<double>> result = matrixMultiply(tmp, tmp);

// std::cout << "MINI TEST result" << std::endl;
// for (auto tmp : result)
// {
//   std::cout << tmp[0] << ", " << tmp[1] << std::endl;
// }

// template <typename T, typename R>
// GRBVector matrixMultiply(const std::vector<std::vector<R>>& A, const std::vector<std::vector<R>>& B)
// {
//   std::vector<GRBVector> 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>
// Eigen::Matrix<GRBLinExpr, -1, -1> myMultDV(const Eigen::Matrix<double, -1, -1>& A,
//                                            const Eigen::Matrix<GRBVar, -1, 1>& b)
// {
//   Eigen::Matrix<GRBLinExpr, -1, -1> result(A.rows(), b.cols());

//   for (int i = 0; i < A.rows(); i++)
//   {
//     GRBLinExpr exp = 0;
//     for (int m = 0; m < b.rows(); m++)
//     {
//       exp = exp + A(i, m) * b[m];
//     }
//     result(i, 1) = exp;
//   }
//   return result;
// }

// Eigen::Matrix<GRBLinExpr, -1, -1> myMultVD(const Eigen::Matrix<GRBVar, -1, -1>& A,
//                                            const Eigen::Matrix<double, -1, 1>& b)
// {
//   Eigen::Matrix<GRBLinExpr, -1, -1> result(A.rows(), b.cols());

//   for (int i = 0; i < A.rows(); i++)
//   {
//     GRBLinExpr exp = 0;
//     for (int m = 0; m < b.rows(); m++)
//     {
//       exp = exp + A(i, m) * b[m];
//     }
//     result(i, 1) = exp;
//   }
//   return result;
// }

// // squared norm of a vector or matrix
// GRBLinExpr getNorm2(const Eigen::Matrix<GRBLinExpr, -1, -1>& x)
// {
//   GRBLinExpr result = 0;
//   for (int i = 0; i < x.rows(); i++)
//   {
//     for (int j = 0; j < x.cols(); j++)
//     {
//       result = result + x(i, j) * x(i, j);
//     }
//   }
//   return result;
// }

// // Overload - to substract Elementwise std::vectors
// Eigen::Matrix<GRBLinExpr, -1, -1> operator-(const Eigen::Matrix<GRBLinExpr, -1, -1>& a,
//                                             const Eigen::Matrix<GRBLinExpr, -1, -1>& b)
// {
//   assert(a.rows() == b.rows());
//   assert(a.cols() == b.cols());

//   Eigen::Matrix<GRBLinExpr, -1, -1> result(a.cols(), a.rows());

//   for (int i = 0; i < a.rows(); i++)
//   {
//     for (int j = 0; j < a.cols(); j++)
//     {
//       result(i, j) = a(i, j) + b(i, j);
//     }
//   }

//   return result;
// }

// Eigen::Matrix<GRBLinExpr, -1, -1> myMult(const Eigen::Matrix<GRBVar, -1, -1>& A, const Eigen::Matrix<GRBVar, -1, 1>&
// b)
// {
//   Eigen::Matrix<GRBLinExpr, -1, -1> result(A.rows(), b.cols());

//   for (int i = 0; i < A.rows(); i++)
//   {
//     GRBLinExpr exp = 0;
//     for (int m = 0; m < b.rows(); m++)
//     {
//       exp = exp + A(i, m) * b[m];
//     }
//     result(i, 1) = exp;
//   }
//   return result;
// }

// Eigen::Matrix<GRBLinExpr, 2, 1> operator*(const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<GRBVar, 2, 1>& b)
// {
//   Eigen::Matrix<GRBLinExpr, 2, 1> result;

//   for (int i = 0; i < A.rows(); i++)
//   {
//     GRBLinExpr exp = 0;
//     for (int m = 0; m < b.rows(); m++)
//     {
//       exp = exp + A(i, m) * b[m];
//     }
//     result(i, 1) = exp;
//   }
//   return result;
// }

// template<typename Derived>
// void printFirstRow(const Eigen::MatrixBase<Derived>& x)
// {

// template <typename T>
// Eigen::Matrix<GRBLinExpr, 2, 1> operator*(const Eigen::MatrixBase<Derived1>& A, const Eigen::MatrixBase<Derived2>& b)
// {
//   Eigen::Matrix<GRBLinExpr, 2, 1> result;

//   for (int i = 0; i < A.rows(); i++)
//   {
//     GRBLinExpr exp = 0;
//     for (int m = 0; m < b.rows(); m++)
//     {
//       exp = exp + A(i, m) * b[m];
//     }
//     result(i, 1) = exp;
//   }
//   return result;
// }

/*GRBVector MatrixMultiply(const std::vector<std::vector<double>>& A, const GRBVector& x)
{
  GRBVector 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;
}*/

#endif


//
//  Copyright Toon Knapen, Karl Meerbergen
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include "ublas_heev.hpp"

#include <boost/numeric/bindings/lapack/driver/syev.hpp>
#include <boost/numeric/bindings/noop.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

template <char UPLO>
struct translate_uplo
{
} ;

template <>
struct translate_uplo<'U'>
{
  typedef ublas::upper_tag type ;
} ;

template <>
struct translate_uplo<'L'>
{
  typedef ublas::lower_tag type ;
} ;

template <typename T, typename W, typename UPLO>
int do_memory_uplo(int n, W& workspace)
{
  typedef typename bindings::remove_imaginary<T>::type real_type ;

  typedef ublas::matrix<T, ublas::column_major>     matrix_type ;
  typedef ublas::symmetric_adaptor<matrix_type, UPLO> symmetric_type ;
  typedef ublas::vector<real_type>                  vector_type ;

  // Set matrix
  matrix_type a(n, n);
  a.clear();
  vector_type e1(n);
  vector_type e2(n);

  fill(a);
  matrix_type a2(a);

  // Compute eigen decomposition.
  symmetric_type s_a(a);
  lapack::syev('V', bindings::noop(s_a), e1, workspace) ;

  if(check_residual(a2, e1, a)) return 255 ;

  symmetric_type s_a2(a2);
  lapack::syev('N', s_a2, e2, workspace) ;
  if(norm_2(e1 - e2) > n * norm_2(e1) * std::numeric_limits< real_type >::epsilon()) return 255 ;

  // Test for a matrix range
  fill(a);
  a2.assign(a);

  typedef ublas::matrix_range< matrix_type > matrix_range ;
  typedef ublas::symmetric_adaptor<matrix_range, UPLO> symmetric_range_type;

  ublas::range r(1,n-1) ;
  matrix_range a_r(a, r, r);
  ublas::vector_range< vector_type> e_r(e1, r);

  symmetric_range_type s_a_r(a_r);
  lapack::syev('V',  s_a_r, e_r, workspace);

  matrix_range a2_r(a2, r, r);
  if(check_residual(a2_r, e_r, a_r)) return 255 ;

  // Test for symmetric_adaptor
  fill(a);
  a2.assign(a);
  ublas::symmetric_adaptor< matrix_type, UPLO> a_uplo(a) ;
  lapack::syev('V', a_uplo, e1, workspace) ;
  if(check_residual(a2, e1, a)) return 255 ;

  return 0 ;
} // do_memory_uplo()

template <typename T, typename W>
int do_memory_type(int n, W workspace)
{
  std::cout << "  upper\n" ;
  if(do_memory_uplo<T,W,ublas::upper>(n, workspace)) return 255 ;
  std::cout << "  lower\n" ;
  if(do_memory_uplo<T,W,ublas::lower>(n, workspace)) return 255 ;
  return 0 ;
}

template <typename T>
struct Workspace
{
  typedef ublas::vector<T>                         array_type ;
  typedef lapack::detail::workspace1< array_type > type ;

  Workspace(size_t n)
    : work_(3*n-1)
  {}

  type operator()()
  {
    return lapack::workspace(work_) ;
  }

  array_type work_ ;
};

template <typename T>
int do_value_type()
{
  const int n = 8 ;

  std::cout << " optimal workspace\n";
  if(do_memory_type<T,lapack::optimal_workspace>(n, lapack::optimal_workspace())) return 255 ;

  std::cout << " minimal workspace\n";
  if(do_memory_type<T,lapack::minimal_workspace>(n, lapack::minimal_workspace())) return 255 ;

  std::cout << " workspace array\n";
  Workspace<T> work(n);
  if(do_memory_type<T,typename Workspace<T>::type >(n, work())) return 255 ;
  return 0;
} // do_value_type()

int main()
{
  // Run tests for different value_types
  std::cout << "float\n" ;
  if(do_value_type<float>()) return 255;

  std::cout << "double\n" ;
  if(do_value_type<double>()) return 255;

  std::cout << "Regression test succeeded\n" ;
  return 0;
}


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

#ifndef BOOST_MATH_DISTRIBUTIONS_LOGISTIC
#define BOOST_MATH_DISTRIBUTIONS_LOGISTIC

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/constants/constants.hpp>
#include <utility>

namespace boost { namespace math {

    template <class RealType = double, class Policy = policies::policy<> >
    class logistic_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      logistic_distribution(RealType l_location=0, RealType l_scale=1) // Constructor.
        : m_location(l_location), m_scale(l_scale)
      {
        static const char* function = "boost::math::logistic_distribution<%1%>::logistic_distribution";

        RealType result;
        detail::check_scale(function, l_scale, &result, Policy());
        detail::check_location(function, l_location, &result, Policy());
      }
      // Accessor functions.
      RealType scale()const
      {
        return m_scale;
      }

      RealType location()const
      {
        return m_location;
      }
    private:
      // Data members:
      RealType m_location;  // distribution location aka mu.
      RealType m_scale;  // distribution scale aka s.
    }; // class logistic_distribution

    typedef logistic_distribution<double> logistic;

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const logistic_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable x.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(
         std::numeric_limits<RealType>::has_infinity ? -std::numeric_limits<RealType>::infinity() : -max_value<RealType>(),
         std::numeric_limits<RealType>::has_infinity ? std::numeric_limits<RealType>::infinity() : max_value<RealType>());
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const logistic_distribution<RealType, Policy>& /* dist */)
    { // Range of supported values for random variable x.
      // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(-max_value<RealType>(), max_value<RealType>()); // - to + infinity
    }

    template <class RealType, class Policy>
    inline RealType pdf(const logistic_distribution<RealType, Policy>& dist, const RealType& x)
    {
       static const char* function = "boost::math::pdf(const logistic_distribution<%1%>&, %1%)";
       RealType scale = dist.scale();
       RealType location = dist.location();
       RealType result = 0;

       if(false == detail::check_scale(function, scale , &result, Policy()))
       {
          return result;
       }
       if(false == detail::check_location(function, location, &result, Policy()))
       {
          return result;
       }

       if((boost::math::isinf)(x))
       {
          return 0; // pdf + and - infinity is zero.
       }

       if(false == detail::check_x(function, x, &result, Policy()))
       {
          return result;
       }

       BOOST_MATH_STD_USING
       RealType exp_term = (location - x) / scale;
       if(fabs(exp_term) > tools::log_max_value<RealType>())
          return 0;
       exp_term = exp(exp_term);
       if((exp_term * scale > 1) && (exp_term > tools::max_value<RealType>() / (scale * exp_term)))
          return 1 / (scale * exp_term);
       return (exp_term) / (scale * (1 + exp_term) * (1 + exp_term));
    }

    template <class RealType, class Policy>
    inline RealType cdf(const logistic_distribution<RealType, Policy>& dist, const RealType& x)
    {
       RealType scale = dist.scale();
       RealType location = dist.location();
       RealType result = 0; // of checks.
       static const char* function = "boost::math::cdf(const logistic_distribution<%1%>&, %1%)";
       if(false == detail::check_scale(function, scale, &result, Policy()))
       {
          return result;
       }
       if(false == detail::check_location(function, location, &result, Policy()))
       {
          return result;
       }

       if((boost::math::isinf)(x))
       {
          if(x < 0) return 0; // -infinity
          return 1; // + infinity
       }

       if(false == detail::check_x(function, x, &result, Policy()))
       {
          return result;
       }
       BOOST_MATH_STD_USING
       RealType power = (location - x) / scale;
       if(power > tools::log_max_value<RealType>())
          return 0;
       if(power < -tools::log_max_value<RealType>())
          return 1;
       return 1 / (1 + exp(power));
    }

    template <class RealType, class Policy>
    inline RealType quantile(const logistic_distribution<RealType, Policy>& dist, const RealType& p)
    {
       BOOST_MATH_STD_USING
       RealType location = dist.location();
       RealType scale = dist.scale();

       static const char* function = "boost::math::quantile(const logistic_distribution<%1%>&, %1%)";

       RealType result = 0;
       if(false == detail::check_scale(function, scale, &result, Policy()))
          return result;
       if(false == detail::check_location(function, location, &result, Policy()))
          return result;
       if(false == detail::check_probability(function, p, &result, Policy()))
          return result;

       if(p == 0)
       {
          return -policies::raise_overflow_error<RealType>(function,"probability argument is 0, must be >0 and <1",Policy());
       }
       if(p == 1)
       {
          return policies::raise_overflow_error<RealType>(function,"probability argument is 1, must be >0 and <1",Policy());
       }
       //Expressions to try
       //return location+scale*log(p/(1-p));
       //return location+scale*log1p((2*p-1)/(1-p));

       //return location - scale*log( (1-p)/p);
       //return location - scale*log1p((1-2*p)/p);

       //return -scale*log(1/p-1) + location;
       return location - scale * log((1 - p) / p);
     } // RealType quantile(const logistic_distribution<RealType, Policy>& dist, const RealType& p)

    template <class RealType, class Policy>
    inline RealType cdf(const complemented2_type<logistic_distribution<RealType, Policy>, RealType>& c)
    {
       BOOST_MATH_STD_USING
       RealType location = c.dist.location();
       RealType scale = c.dist.scale();
       RealType x = c.param;
       static const char* function = "boost::math::cdf(const complement(logistic_distribution<%1%>&), %1%)";

       RealType result = 0;
       if(false == detail::check_scale(function, scale, &result, Policy()))
       {
          return result;
       }
       if(false == detail::check_location(function, location, &result, Policy()))
       {
          return result;
       }
       if((boost::math::isinf)(x))
       {
          if(x < 0) return 1; // cdf complement -infinity is unity.
          return 0; // cdf complement +infinity is zero.
       }
       if(false == detail::check_x(function, x, &result, Policy()))
       {
          return result;
       }
       RealType power = (x - location) / scale;
       if(power > tools::log_max_value<RealType>())
          return 0;
       if(power < -tools::log_max_value<RealType>())
          return 1;
       return 1 / (1 + exp(power));
    }

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<logistic_distribution<RealType, Policy>, RealType>& c)
    {
       BOOST_MATH_STD_USING
       RealType scale = c.dist.scale();
       RealType location = c.dist.location();
       static const char* function = "boost::math::quantile(const complement(logistic_distribution<%1%>&), %1%)";
       RealType result = 0;
       if(false == detail::check_scale(function, scale, &result, Policy()))
          return result;
       if(false == detail::check_location(function, location, &result, Policy()))
          return result;
       RealType q = c.param;
       if(false == detail::check_probability(function, q, &result, Policy()))
          return result;
       using boost::math::tools::max_value;

       if(q == 1)
       {
          return -policies::raise_overflow_error<RealType>(function,"probability argument is 1, but must be >0 and <1",Policy());
       }
       if(q == 0)
       {
          return policies::raise_overflow_error<RealType>(function,"probability argument is 0, but must be >0 and <1",Policy());
       }
       //Expressions to try
       //return location+scale*log((1-q)/q);
       return location + scale * log((1 - q) / q);

       //return location-scale*log(q/(1-q));
       //return location-scale*log1p((2*q-1)/(1-q));

       //return location+scale*log(1/q-1);
       //return location+scale*log1p(1/q-2);
    }

    template <class RealType, class Policy>
    inline RealType mean(const logistic_distribution<RealType, Policy>& dist)
    {
      return dist.location();
    } // RealType mean(const logistic_distribution<RealType, Policy>& dist)

    template <class RealType, class Policy>
    inline RealType variance(const logistic_distribution<RealType, Policy>& dist)
    {
      BOOST_MATH_STD_USING
      RealType scale = dist.scale();
      return boost::math::constants::pi<RealType>()*boost::math::constants::pi<RealType>()*scale*scale/3;
    } // RealType variance(const logistic_distribution<RealType, Policy>& dist)

    template <class RealType, class Policy>
    inline RealType mode(const logistic_distribution<RealType, Policy>& dist)
    {
      return dist.location();
    }

    template <class RealType, class Policy>
    inline RealType median(const logistic_distribution<RealType, Policy>& dist)
    {
      return dist.location();
    }
    template <class RealType, class Policy>
    inline RealType skewness(const logistic_distribution<RealType, Policy>& /*dist*/)
    {
      return 0;
    } // RealType skewness(const logistic_distribution<RealType, Policy>& dist)

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const logistic_distribution<RealType, Policy>& /*dist*/)
    {
      return static_cast<RealType>(6)/5;
    } // RealType kurtosis_excess(const logistic_distribution<RealType, Policy>& dist)

    template <class RealType, class Policy>
    inline RealType kurtosis(const logistic_distribution<RealType, Policy>& dist)
    {
      return kurtosis_excess(dist) + 3;
    } // RealType kurtosis_excess(const logistic_distribution<RealType, Policy>& dist)
  }}

// Must come at the end:
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_DISTRIBUTIONS_LOGISTIC


#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "IO/readPLY.h"
#include "IO/writePLY.h"

#include "visualization/plotMesh.h"
#include "visualization/plotTwoMeshes.h"
#include "visualization/plotCloud.h"

#include "mesh/computeNormals.h"
#include "mesh/computeFacesCentroids.h"
#include "occupancyGridWithColor.h"

int main() {
    bool visualization = true;
    int grid_resolution = 100;          // grid_resolution is used to define the grid resolution in the maximum direction
    double bounding_box_scale = 1;

    // IO: load files
    std::cout << "Progress: load data\n";
    Eigen::MatrixXd V, cubes_V, faces_V;
    Eigen::MatrixXi F, cubes_F;
    Eigen::MatrixXd N, faces_N;
    Eigen::MatrixXi RGB;

    readPLY("../data/crab_full.ply", V, F, N, RGB);

    faces_V = compute_faces_centroids(V,F);
    faces_N = compute_faces_normals(V,F);

    Eigen::MatrixXd faces_RGB, cubes_RGB;
    Eigen::MatrixXd RGB_double = RGB.cast<double> ();
    RGB_double /=256;

    faces_RGB = compute_faces_centroids(RGB_double,F);

    if (visualization)
        plot_mesh(V,F, RGB_double);

    if (visualization)
        plot_cloud(faces_V, faces_RGB);

    OccupancyGridWithColor occupancy_grid(faces_V, faces_N, faces_RGB, grid_resolution, bounding_box_scale);

    Eigen::MatrixXd graph_V;
    Eigen::MatrixXi graph_E;
    occupancy_grid.generate_graph(graph_V, graph_E);
    occupancy_grid.print_to_folder("../data/occupancy_grid/");
    occupancy_grid.print_to_yaml("../data/crab.yaml");

    occupancy_grid.generate_mesh(cubes_V, cubes_F, cubes_RGB);

    if (visualization)
        plot_mesh(cubes_V,cubes_F, cubes_RGB);
        //plot_two_meshes(V,F,cubes_V,cubes_F, cubes_RGB);

}