#include <algorithm>
#include <boost/multiprecision/cpp_int.hpp>
#include <fstream>
#include <iostream>
#include <iterator>
#include <numeric>
#include <vector>

using namespace std;
using boost::multiprecision::cpp_int;

int main(int argc, char *argv[]) {
	// Read in the data file into the vector.
	ifstream input("Data.txt");
	vector<cpp_int> numbers;
	istream_iterator<cpp_int> end, start(input);
	copy(start, end, back_inserter(numbers));
	// Accumulate the result and put it into a variable.
	cpp_int result = accumulate(numbers.begin(), numbers.end(), (cpp_int)0);
	// Print the first 10 digits.
	while(cpp_int(result / (cpp_int)pow(10, 10)) != 0) {
		result = result / 10;
	}
	cout << result << endl;
	input.close();
	return 0;
}


// Copyright 2008 John Maddock
//
// 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_DETAIL_HG_CDF_HPP
#define BOOST_MATH_DISTRIBUTIONS_DETAIL_HG_CDF_HPP

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/distributions/detail/hypergeometric_pdf.hpp>

namespace boost{ namespace math{ namespace detail{

   template <class T, class Policy>
   T hypergeometric_cdf_imp(unsigned x, unsigned r, unsigned n, unsigned N, bool invert, const Policy& pol)
   {
#ifdef BOOST_MSVC
#  pragma warning(push)
#  pragma warning(disable:4267)
#endif
      BOOST_MATH_STD_USING
      T result = 0;
      T mode = floor(T(r + 1) * T(n + 1) / (N + 2));
      if(x < mode)
      {
         result = hypergeometric_pdf<T>(x, r, n, N, pol);
         T diff = result;
         unsigned lower_limit = static_cast<unsigned>((std::max)(0, (int)(n + r) - (int)(N)));
         while(diff > (invert ? T(1) : result) * tools::epsilon<T>())
         {
            diff = T(x) * T((N + x) - n - r) * diff / (T(1 + n - x) * T(1 + r - x));
            result += diff;
            BOOST_MATH_INSTRUMENT_VARIABLE(x);
            BOOST_MATH_INSTRUMENT_VARIABLE(diff);
            BOOST_MATH_INSTRUMENT_VARIABLE(result);
            if(x == lower_limit)
               break;
            --x;
         }
      }
      else
      {
         invert = !invert;
         unsigned upper_limit = (std::min)(r, n);
         if(x != upper_limit)
         {
            ++x;
            result = hypergeometric_pdf<T>(x, r, n, N, pol);
            T diff = result;
            while((x <= upper_limit) && (diff > (invert ? T(1) : result) * tools::epsilon<T>()))
            {
               diff = T(n - x) * T(r - x) * diff / (T(x + 1) * T((N + x + 1) - n - r));
               result += diff;
               ++x;
               BOOST_MATH_INSTRUMENT_VARIABLE(x);
               BOOST_MATH_INSTRUMENT_VARIABLE(diff);
               BOOST_MATH_INSTRUMENT_VARIABLE(result);
            }
         }
      }
      if(invert)
         result = 1 - result;
      return result;
#ifdef BOOST_MSVC
#  pragma warning(pop)
#endif
   }

   template <class T, class Policy>
   inline T hypergeometric_cdf(unsigned x, unsigned r, unsigned n, unsigned N, bool invert, const Policy&)
   {
      BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T>::type result_type;
      typedef typename policies::evaluation<result_type, Policy>::type value_type;
      typedef typename policies::normalise<
         Policy,
         policies::promote_float<false>,
         policies::promote_double<false>,
         policies::discrete_quantile<>,
         policies::assert_undefined<> >::type forwarding_policy;

      value_type result;
      result = detail::hypergeometric_cdf_imp<value_type>(x, r, n, N, invert, forwarding_policy());
      if(result > 1)
      {
         result  = 1;
      }
      if(result < 0)
      {
         result = 0;
      }
      return policies::checked_narrowing_cast<result_type, forwarding_policy>(result, "boost::math::hypergeometric_cdf<%1%>(%1%,%1%,%1%,%1%)");
   }

}}} // namespaces

#endif


////////////////////////////////////////////////////////////////////////////////
// Exact Diagonalization: Kitaev Model, Honeycomb Lattice, 2x2 unit cells     //
// cylindrical boundary conditions
// Copyright (C) 2016 by Tim Eschmann
////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <fstream>
#include <math.h>
#include <complex>
//#define ARMA_64BIT_WORD
#include <armadillo>

using namespace arma;
using namespace std;

// Flip k1-th and k2-th bit of integer number
int bitflip(int *v, int k1, int k2)
{
    int erg;
    erg = *v;
    // Flip bits:
    erg ^= 1 << k1;
    erg ^= 1 << k2;
    return erg;
}

// Calculate diagonal element (periodic boundary conditions)
int diagel(int *w)
{
    int b, b0, b1, b2, b3, b4, b5, b6, b7;
    signed int s1, s2, s3, s4;

    b = *w;
    // Check bits of states
    b0 = (b >> 0) & 1; // 0th bit and so on ...
    b1 = (b >> 1) & 1;
    b2 = (b >> 2) & 1;
    b3 = (b >> 3) & 1;
    b4 = (b >> 4) & 1;
    b5 = (b >> 5) & 1;
    b6 = (b >> 6) & 1;
    b7 = (b >> 7) & 1;

    if (b0 == b4)
        s1 = 1;
    else
        s1 = -1;

    if (b5 == b1)
        s2 = 1;
    else
        s2 = -1;

    if (b2 == b6)
        s3 = 1;
    else
        s3 = -1;

    if (b7 == b3)
        s4 = 1;
    else
        s4 = -1;

    return s1 + s2 + s3 + s4;
}

// Sign check for yy interactions
int signcheck(int *x, int var1, int var2)
{
    int c, c1, c2;
    signed int s;
    c = *x;
    c1 = (c >> var1) & 1;
    c2 = (c >> var2) & 1;

    if (c1 == c2)
        s = 1;
    else
        s = -1;

    return s;
}

// Main function
int main(void)
{
    int i,j,k,l,m,p; // running indices
    int steps = 100; // How many measurement steps?
    int Jx, Jy, Jz; // coupling constants
    int N = 8; //  number of sites
    int size = 256; // matrix size
    double Z, E; // partition sum, energy value

    int xflips[3], yflips[3]; // Arrays for spin flips, size depending on # of bonds
    signed int si0, si1, si2;

    vec ev;
    double temp[steps];
    double energy[steps];

    mat ham = mat(size,size, fill::zeros); // Initialize matrix

    Jx = 1;
    Jy = 1;
    Jz = 1;

    cout << "Calculating matrix ..." << endl;

    // Fill Hamiltonian matrix with elements
    for (j = 0; j < size; j++)
    {
        // Diagonal element
        ham(j,j) = -Jz * diagel(&j);
        // Calculate interaction states via spin flips
        // xx:
        xflips[0] = bitflip(&j, 1, 2);
        xflips[1] = bitflip(&j, 4, 5);
        xflips[2] = bitflip(&j, 6, 7);

        //yy:
        yflips[0] = bitflip(&j, 0, 1);
        yflips[1] = bitflip(&j, 2, 3);
        yflips[2] = bitflip(&j, 5, 6);

        // signs for yy-interaction matrix elements
        si0 = signcheck(&j, 0, 1);
        si1 = signcheck(&j, 2, 3);
        si2 = signcheck(&j, 5, 6);

        ham(j,xflips[0]) = Jx;
        ham(j,xflips[1]) = Jx;
        ham(j,xflips[2]) = Jx;

        ham(xflips[0], j) = -Jx;
        ham(xflips[1], j) = -Jx;
        ham(xflips[2], j) = -Jx;

        ham(j,yflips[0]) = Jy*si0;
        ham(j,yflips[1]) = Jy*si1;
        ham(j,yflips[2]) = Jy*si2;

        ham(yflips[0], j) = -Jy*si0;
        ham(yflips[1], j) = -Jy*si1;
        ham(yflips[2], j) = -Jy*si2;

    }

    cout << "Diagonalizing ... " << endl;

    // Compute eigenvalues of ham:
    ev = eig_sym(ham);
    ev.save("eigenvalues.mat", csv_ascii);

    cout << "Calculating energy per temperature ... " << endl;

    // Calculating energy per temperature (with Boltzmann weights):
    for (l = 0; l < steps; l++)
    {
        Z = 0;
        E = 0;
        //temp[l] = 0.000001 + l*0.01;
        temp[l] = pow(10,-2+(3*l/float(steps))); //logspace

        for (m = 0; m < size; m++)
        {
            Z = Z + exp(-ev[m]/temp[l]);
            E = E + ev[m]*exp(-ev[m]/temp[l]);
        }
        energy[l] = E/(N*Z);
    }

    // Write measured data to text file:
    std::fstream f("dataexd.txt", std::ios::out);
    for (int i = 0; i < steps; ++i)
    {
        if (i == 0 || i == steps - 1)
            f << temp[i] << " " << energy[i] << " " << 0 << "\n";
        else
            f << temp[i] << " " << energy[i] << " " << (energy[i+1] - energy[i-1])/(2*(temp[i+1] - temp[i-1])) << "\n";
    }
    f.close();

    return 0;

}


#include <iostream>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

int main()
{
	cv::Mat_<float> a = cv::Mat_<float>::ones(2, 2);
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> b;
	cv::cv2eigen(a, b);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE COMPUTE

#include <boost/test/unit_test.hpp>

#include "api/umo.hpp"

#include <cmath>
#include <stdexcept>
#include <vector>

#define STRINGIFY(x) #x
#define TOSTRING(x) STRINGIFY(x)

using namespace umo;

const double eps = 0.001;

BOOST_AUTO_TEST_CASE(LinearizationFloatDec) {
    Model model;
    FloatExpression x1 = model.floatVar(-3.0, 10.2);
    FloatExpression x2 = model.floatVar(-50.9, 10.33);
    FloatExpression x3 = model.floatVar(-4.4, -2.0);
    FloatExpression x4 = model.floatVar(10.0, 21.2);
    maximize(x1 + x2 + x3 + x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 10.2, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 10.33, eps);
    BOOST_CHECK_CLOSE(x3.getValue(), -2.0, eps);
    BOOST_CHECK_CLOSE(x4.getValue(), 21.2, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationIntDec) {
    Model model;
    IntExpression x1 = model.intVar(0, 10);
    IntExpression x2 = model.intVar(-50, 10);
    IntExpression x3 = model.intVar(-4, -2);
    IntExpression x4 = model.intVar(10, 21);
    maximize(x1 + x2 + x3 + x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(x1.getValue(), 10);
    BOOST_CHECK_EQUAL(x2.getValue(), 10);
    BOOST_CHECK_EQUAL(x3.getValue(), -2);
    BOOST_CHECK_EQUAL(x4.getValue(), 21);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationBoolDec) {
    Model model;
    BoolExpression x1 = model.boolVar();
    BoolExpression x2 = model.boolVar();
    BoolExpression x3 = model.boolVar();
    BoolExpression x4 = model.boolVar();
    maximize(x1 + x2 + x3 + x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(x1.getValue());
    BOOST_CHECK(x2.getValue());
    BOOST_CHECK(x3.getValue());
    BOOST_CHECK(x4.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp1) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(x > 2.0);
    minimize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp2) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(x < 2.0);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp3) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(x >= 2.0);
    minimize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp4) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(x <= 2.0);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp5) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(5.0, 10.0);
    constraint(x >= y);
    minimize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 5.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatCmp6) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(5.0, 10.0);
    constraint(x <= y);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 10.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatEq1) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(x == 2.0);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatEq2) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(0.0, 2.0);
    constraint(x == y);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatEq3) {
    Model model;
    FloatExpression x = model.floatVar();
    constraint(!(x != 2.0));
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatEq4) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(0.0, 2.0);
    constraint(!(x != y));
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x.getValue(), 2.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatNeq1) {
    Model model;
    FloatExpression x = model.floatVar(0.0, 5.0);
    constraint(x != 2.0);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatNeq2) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(0.0, 2.0);
    constraint(x != y);
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatNeq3) {
    Model model;
    FloatExpression x = model.floatVar(0.0, 5.0);
    constraint(!(x == 2.0));
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationFloatNeq4) {
    Model model;
    FloatExpression x = model.floatVar();
    FloatExpression y = model.floatVar(0.0, 2.0);
    constraint(!(x == y));
    maximize(x);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationAnd1) {
    Model model;
    BoolExpression dec1 = model.boolVar();
    BoolExpression dec2 = model.boolVar();
    BoolExpression dec3 = model.boolVar();
    std::vector<BoolExpression> vec{dec1, dec2, dec3};
    BoolExpression obj = logical_and(vec);
    maximize(obj);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(dec1.getValue() && dec2.getValue() && dec3.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationAnd2) {
    Model model;
    BoolExpression dec1 = model.boolVar();
    BoolExpression dec2 = model.boolVar();
    BoolExpression dec3 = model.boolVar();
    std::vector<BoolExpression> vec{dec1, dec2, dec3};
    BoolExpression obj = logical_and(vec);
    minimize(obj);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(dec1.getValue() && dec2.getValue() && dec3.getValue()));
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationOr1) {
    Model model;
    BoolExpression dec1 = model.boolVar();
    BoolExpression dec2 = model.boolVar();
    BoolExpression dec3 = model.boolVar();
    std::vector<BoolExpression> vec{dec1, dec2, dec3};
    BoolExpression obj = logical_or(vec);
    maximize(obj);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(dec1.getValue() || dec2.getValue() || dec3.getValue());
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationOr2) {
    Model model;
    BoolExpression dec1 = model.boolVar();
    BoolExpression dec2 = model.boolVar();
    BoolExpression dec3 = model.boolVar();
    std::vector<BoolExpression> vec{dec1, dec2, dec3};
    BoolExpression obj = logical_or(vec);
    minimize(obj);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK(!(dec1.getValue() || dec2.getValue() || dec3.getValue()));
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationMax1) {
    Model model;
    FloatExpression dec1 = model.floatVar();
    FloatExpression dec2 = model.floatVar();
    maximize(umo::max(dec1, dec2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationMin1) {
    Model model;
    FloatExpression dec1 = model.floatVar();
    FloatExpression dec2 = model.floatVar();
    maximize(umo::min(dec1, dec2));
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationMultiObjective) {
    Model model;
    FloatExpression dec1 = model.floatVar();
    FloatExpression dec2 = model.floatVar();
    maximize(dec1);
    minimize(dec2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    BOOST_CHECK_THROW(model.solve(), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(LinearizationUnfeasible) {
    Model model;
    FloatExpression dec1 = model.floatVar(0, 10);
    FloatExpression dec2 = model.floatVar(0, 10);
    constraint(dec1 + 1 <= dec2);
    constraint(dec2 + 1 <= dec1);
    maximize(dec1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    if (TOSTRING(SOLVER_PARAM) == "glpk")
        return; // Yes GLPK screws up the return value here
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Infeasible);
}

BOOST_AUTO_TEST_CASE(LinearizationUnbounded) {
    Model model;
    FloatExpression dec1 = model.floatVar();
    FloatExpression dec2 = model.floatVar();
    constraint(dec1 <= dec2);
    maximize(dec1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    if (TOSTRING(SOLVER_PARAM) == "glpk")
        return; // Yes GLPK screws up the return value here
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Unbounded);
}

BOOST_AUTO_TEST_CASE(LinearizationSimple1) {
    Model model;
    FloatExpression x1 = model.floatVar();
    FloatExpression x2 = model.floatVar();
    constraint(3 * x1 + x2 <= 4);
    constraint(2 * x1 + 3 * x2 <= 5);
    constraint(2 * x1 + 9 * x2 <= 8);
    maximize(x1 + 2 * x2);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 1.12, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 0.64, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationSimple2) {
    Model model;
    FloatExpression x1 = model.floatVar();
    FloatExpression x2 = model.floatVar();
    FloatExpression x3 = model.floatVar();
    FloatExpression x4 = model.floatVar();
    constraint(x1 <= 5);
    constraint(x2 <= 3);
    constraint(x3 <= 10);
    constraint(x4 < 50);
    constraint(x1 + 3 * x2 <= 3);
    constraint(x1 + 2 * x2 <= 4);
    constraint(x2 + x1 <= 3);
    constraint(x3 + x4 <= 2);
    constraint(x1 + x3 <= 5);
    constraint(x1 + x4 + 3 * x2 <= 4);
    maximize(x1 + x2 + 2 * x3 + x4);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), -5, eps);
    BOOST_CHECK_CLOSE(x2.getValue(), 2.6666666, eps);
    BOOST_CHECK_CLOSE(x3.getValue(), 10, eps);
    BOOST_CHECK_CLOSE(x4.getValue(), -8, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(LinearizationWithTimeLimit) {
    Model model;
    FloatExpression dec1 = model.floatVar(0.0, 5.0);
    maximize(dec1);
    model.setTimeLimit(2.0);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(ExplicitConstraint1) {
    Model model;
    FloatExpression x1 = model.floatVar();
    linearConstraint(umo::unbounded(), 5.0, {x1});
    maximize(x1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 5.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}

BOOST_AUTO_TEST_CASE(ExplicitConstraint2) {
    Model model;
    FloatExpression x1 = model.floatVar();
    linearConstraint(umo::unbounded(), 10.0, {x1}, {2.0});
    maximize(x1);
    model.setSolver(TOSTRING(SOLVER_PARAM));
    model.solve();
    BOOST_CHECK_CLOSE(x1.getValue(), 5.0, eps);
    BOOST_CHECK_EQUAL(model.getStatus(), Status::Optimal);
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Musegaas, P. (2012). Optimization of Space Trajectories Including Multiple Gravity Assists
 *          and Deep Space Maneuvers. MSc Thesis, Delft University of Technology, Delft,
 *          The Netherlands.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

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

#include <Eigen/Core>

#include <Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h>
#include <Tudat/Basics/testMacros.h>

#include "Tudat/Astrodynamics/TrajectoryDesign/departureLegMga1DsmPosition.h"

namespace tudat
{
namespace unit_tests
{

//! Test implementation of departure leg within MGA trajectory model
BOOST_AUTO_TEST_SUITE( test_departure_leg_mga_1dsm_position )

//! Test delta-V computation
BOOST_AUTO_TEST_CASE( testVelocities )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the first leg of the ideal Messenger trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 1408.99421278 + 910.801673341206;
    const Eigen::Vector3d expectedVelocity ( 17969.3166254715, -23543.6915939142,
                                             6.38384671663485 );

    // Specify the required parameters.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( -148689402143.081, 7454242895.97607, 0. );
    const Eigen::Vector3d planet2Position ( -128359548637.032, -78282803797.7343, 0. );
    const Eigen::Vector3d planet1Velocity ( -1976.48781307596, -29863.4035321021, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 399.999999715 * physical_constants::JULIAN_DAY;

    // Set the gravitational parameter.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    // Set DSM parameters.
    const double dsmTimeOfFlightFraction = 0.234594654679;
//?!? const Eigen::Vector3d dsmLocation ( 10272022720.5409, -135966957958.46, 26316361.9336743 );
    const double inPlaneAngle = 1.69629206466940;
    const double outOfPlaneAngle = 0.00019299969467;
    const double dimensionlessRadiusDsm = 0.915891737859598;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga1DsmPosition legTest ( planet1Position, planet2Position, timeOfFlight,
                                          planet1Velocity, sunGravitationalParameter,
                                          earthGravitationalParameter, semiMajorAxis, eccentricity,
                                          dsmTimeOfFlightFraction, //dsmLocation
                                          dimensionlessRadiusDsm, inPlaneAngle, outOfPlaneAngle );

    // Prepare the variables for the results.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Compute delta-V of the leg.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedVelocity, resultingVelocity, tolerance );
}

//! Test updating the variables.
BOOST_AUTO_TEST_CASE( testUpdatingVariables )
{
    // Set tolerance. Due to iterative nature in the process, (much) higher accuracy cannot be
    // achieved.
    const double tolerance = 1.0e-6;

    // Expected test result based on the first leg of the ideal Messenger trajectory as modelled by
    // GTOP software distributed and downloadable from the ESA website, or within the PaGMO
    // Astrotoolbox.
    const double expectedDeltaV = 1408.99421278 + 910.801673341206;
    const Eigen::Vector3d expectedVelocity ( 17969.3166254715, -23543.6915939142,
                                             6.38384671663485 );

    // Specify the required parameters.
    // Set the dummy positions and velocities.
    const Eigen::Vector3d dummyPosition1 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );
    const Eigen::Vector3d dummyPosition2 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );
    const Eigen::Vector3d dummyVelocity1 ( TUDAT_NAN, TUDAT_NAN, TUDAT_NAN );

    // Set the dummy time of flight.
    const double dummyTimeOfFlight = TUDAT_NAN;

    // Set the gravitational parameter.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = std::numeric_limits< double >::infinity( );
    const double eccentricity = 0.;

    // Set the dummy DSM variables.
    const double dummyVariable1 = TUDAT_NAN;
    const double dummyVariable2 = TUDAT_NAN;
    const double dummyVariable3 = TUDAT_NAN;
    const double dummyVariable4 = TUDAT_NAN;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga1DsmPosition legTest ( dummyPosition1, dummyPosition2, dummyTimeOfFlight,
                                          dummyVelocity1, sunGravitationalParameter,
                                          earthGravitationalParameter, semiMajorAxis, eccentricity,
                                          dummyVariable1,
                                          dummyVariable2, dummyVariable3, dummyVariable4 );

    // Prepare the variables for the results.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Compute delta-V of the leg.
    //legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Specify the values for the parameters that are to be updated.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( -148689402143.081, 7454242895.97607, 0. );
    const Eigen::Vector3d planet2Position ( -128359548637.032, -78282803797.7343, 0. );
    const Eigen::Vector3d planet1Velocity ( -1976.48781307596, -29863.4035321021, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 399.999999715 * physical_constants::JULIAN_DAY;

    // Set DSM parameters.
    const double dsmTimeOfFlightFraction = 0.234594654679;
    const double inPlaneAngle = 1.69629206466940;
    const double outOfPlaneAngle = 0.00019299969467;
    const double dimensionlessRadiusDsm = 0.915891737859598;

    // Create a variable vector containing these parameters.
    Eigen::VectorXd variableVector( 5 );
    variableVector << timeOfFlight, dsmTimeOfFlightFraction, dimensionlessRadiusDsm, inPlaneAngle,
                      outOfPlaneAngle;

    // Pass both the new ephemeris and trajectory defining variables to the leg.
    legTest.updateEphemeris( planet1Position, planet2Position, planet1Velocity );
    legTest.updateDefiningVariables( variableVector );

    // Recompute the delta-V of the leg.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test if the computed delta-V corresponds to the expected value within the specified
    // tolerance and if the computed velocity before target planet matches the expected velocity
    // within the specified tolerance.
    BOOST_CHECK_CLOSE_FRACTION( expectedDeltaV, resultingDeltaV, tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedVelocity, resultingVelocity, tolerance );
}

//! Test intermediate points function.
BOOST_AUTO_TEST_CASE( testIntermediatePoints )
{
    // Set tolerance.
    const double tolerance = 1e-13;

    // Specify the required parameters, for the first leg of Messenger within GTOP.
    // Set the planetary positions and velocities.
    const Eigen::Vector3d planet1Position ( -148689402143.081, 7454242895.97607, 0. );
    const Eigen::Vector3d planet2Position ( -128359548637.032, -78282803797.7343, 0. );
    const Eigen::Vector3d planet1Velocity ( -1976.48781307596, -29863.4035321021, 0. );

    // Set the time of flight, which has to be converted from JD (in GTOP) to seconds (in Tudat).
    const double timeOfFlight = 399.999999715 * physical_constants::JULIAN_DAY;

    // Set the gravitational parameters.
    const double sunGravitationalParameter = 1.32712428e20;
    const double earthGravitationalParameter = 3.9860119e14;

    // Set the departure parking orbit (at inifinity)
    const double semiMajorAxis = 1e300;
    const double eccentricity = 0.;

    // Set DSM parameters.
    const double dsmTimeOfFlightFraction = 0.234594654679;
    const double inPlaneAngle = 1.69629206466940;
    const double outOfPlaneAngle = 0.00019299969467;
    const double dimensionlessRadiusDsm = 0.915891737859598;

    // Set test case.
    using namespace tudat::transfer_trajectories;
    DepartureLegMga1DsmPosition legTest ( planet1Position, planet2Position, timeOfFlight,
                                          planet1Velocity, sunGravitationalParameter,
                                          earthGravitationalParameter, semiMajorAxis, eccentricity,
                                          dsmTimeOfFlightFraction,
                                          dimensionlessRadiusDsm, inPlaneAngle, outOfPlaneAngle );

    // Initiate vectors for storing the results.
    std::vector < Eigen::Vector3d > positionVector1, positionVector2;
    std::vector < double > timeVector1, timeVector2;

    // Test the functionality in case the leg has not been calculated yet.
    legTest.intermediatePoints( 50. * physical_constants::JULIAN_DAY,
                                positionVector1, timeVector1 );

    // Prepare the variables for calculating the leg actively.
    Eigen::Vector3d resultingVelocity;
    double resultingDeltaV;

    // Calculate the leg actively.
    legTest.calculateLeg( resultingVelocity, resultingDeltaV );

    // Test the functionality in case the leg has been calculated already.
    legTest.intermediatePoints( 25. * physical_constants::JULIAN_DAY,
                                positionVector2, timeVector2 );

    // Test if the halfway points in the first part of the leg match between the intermediate
    // points functions.
    BOOST_CHECK_CLOSE_FRACTION( timeVector1[1] , timeVector2[2], tolerance );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( positionVector1[1], positionVector2[2], tolerance );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef __CRUNCHY__
#define __CRUNCHY__

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/algorithm/string.hpp>

#include <vector>

#include "result.hpp"

namespace ipaddress {
/**
 * based
 * Crunch - Arbitrary-precision integer arithmetic library
 * Copyright (C) 2014 Nenad Vukicevic crunch.secureroom.net/license
 *
 */
/**
 * @module Crunch
 * Radix: 28 bits
 * Endianness: Big
 *
 * @param {boolean} rawIn   - expect 28-bit arrays
 * @param {boolean} rawOut  - return 28-bit arrays
 */
// import Crunch from './crunch';

class NotImplementedException : public std::exception
{
public:
    NotImplementedException() {}
    ~NotImplementedException() {}
    virtual char const * what() const throw() { return "Function not yet implemented."; }
};

class Crunchy {
  // we need for ipv6 129 bits
  typedef boost::multiprecision::uint256_t CrunchyType;
  CrunchyType num;
public:
  Crunchy clone() const {
    Crunchy ret;
    ret.num = this->num;
    return ret;
  }

  static Crunchy from_8bit(const std::vector<unsigned char> &number) {
    Crunchy ret;
    for (auto i : number) {
        ret.num = (ret.num << 8) + CrunchyType(i);
    }
    return ret;
  }

  static Result<Crunchy> parse(const std::string &val) {
    return Crunchy::from_string(val, 10);
  }

  static Crunchy from_number(size_t num)  {
    Crunchy ret;
    ret.num = CrunchyType(num);
    return ret;
  }
  static Result<Crunchy> from_string(const std::string &val, size_t radix = 10) {
  Crunchy ret;
  auto f(val.begin());
  auto l(val.end());
  if (radix == 10) {
    boost::spirit::qi::int_parser<CrunchyType, 10, 0, 39> uint256_dec;
    if (!boost::spirit::qi::parse(f, l, uint256_dec, ret.num)) {
        return Err<Crunchy>("inpossible to parse");
    }
  } else if (radix == 16) {
    boost::spirit::qi::int_parser<CrunchyType, 16, 0, 39> uint256_hex;
    if (!boost::spirit::qi::parse(f, l, uint256_hex, ret.num)) {
        return Err<Crunchy>("inpossible to parse");
    }
  } else {
    throw NotImplementedException();
  }
  return Ok(ret);
  }

  std::vector<unsigned char> to_8bit() const {
    std::vector<unsigned char> ret;
    auto my = this->num;
    // std::cout << "to_8bit:" << std::hex << my << std::endl;
    do {
        ret.push_back(static_cast<size_t>(my&0xff));
        my = my >> 8;
    } while (my != 0);
    std::reverse(ret.begin(),ret.end());
    // std::stringstream s2;
    // for (auto i : ret) {
    //     s2 << " " << std::hex << static_cast<size_t>(i);
    // }
    // std::cout << "to_8bit:" << s2.str() << std::endl;
    return ret;
  }

  int compare(const Crunchy &y) const {
    if (this->num < y.num) {
      // std::cout << "-1=" << this->num << "---" << y.num << std::endl;
      return -1;
    } else if (this->num > y.num) {
      // std::cout << "1=" << this->num << "---" << y.num << std::endl;
      return 1;
    }
    // std::cout << "0=" << this->num << "---" << y.num << std::endl;
    return 0;
  }

  bool eq(const Crunchy &oth) const {
    return this->compare(oth) == 0;
  }

  bool lte(const Crunchy &oth) const {
    return this->compare(oth) <= 0;
  }
  bool lt(const Crunchy &oth) const {
    return this->compare(oth) < 0;
  }

  bool gt(const Crunchy &oth) const {
    return this->compare(oth) > 0;
  }
  bool gte(const Crunchy &oth) const {
    return this->compare(oth) >= 0;
  }

  Crunchy add(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num + oth.num;
    return ret;
  }

  Crunchy sub(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num - oth.num;
    return ret;
  }
  Crunchy mul(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num * oth.num;
    return ret;
  }

  Crunchy shr(size_t s) const {
    Crunchy ret;
    ret.num = this->num >> s;
    return ret;
  }

  Crunchy shl(size_t s) const {
    Crunchy ret;
    ret.num = this->num << s;
    // std::cout << "shl:" << std::hex << this->num << ":" << s << ":" << ret.num << std::endl;
    return ret;
  }

  Crunchy div(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num / oth.num;
    return ret;
  }
  // public sub(cry: Crunchy): Crunchy {
  //   return Crunchy.from_8bit(Crunch.sub(this->num, cry.num));
  // }

  Crunchy mod(const Crunchy &oth) const {
    Crunchy ret;
    ret.num = this->num % oth.num;
    return ret;
  }

  size_t mds(size_t n) const {
    return static_cast<size_t>(this->num % n);
  }

  std::string toString(size_t radix = 10) const {
    std::stringstream s2;
    if (radix == 10) {
      s2 << std::dec;
      s2 << this->num;
      return s2.str();
    } else if (radix == 16) {
      s2 << std::hex;
      s2 << this->num;
      auto ret = s2.str();
      boost::algorithm::to_lower(ret);
      return ret;
    } else if (radix == 2) {
      std::vector<size_t> bits;
      auto my = this->num;
      do {
        size_t tmp = static_cast<size_t>(my&0x1);
        bits.push_back(tmp);
        my = my >> 1;
      } while(my != 0);
      for (int i = bits.size()-1; i >= 0; --i) {
        s2 << bits[i];
      }
      return s2.str();
    } else {
      throw NotImplementedException();
    }
  }

  static const Crunchy &zero();
  static const Crunchy &one();
  static const Crunchy &two();

};

std::ostream& operator<<(std::ostream &o, const Crunchy &crunchy);

}

#endif


#include <iostream>

/*
   Copyright 2015 Vladimir Lysyy (mrbald@github)

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#include <boost/endian/arithmetic.hpp>
#include <boost/integer.hpp>

#include <cstdint>
#include <utility>
#include <array>
#include <algorithm>

namespace z85
{

using uchar_t = unsigned char;

namespace
{

uchar_t const en_codes[]{"0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ.-:+=^!/*?&<>()[]{}@%$#"};

std::array<uint8_t, std::numeric_limits<uint8_t>::max() + 1> const de_codes = []
{
    std::array<uint8_t, std::numeric_limits<uint8_t>::max() + 1> x{};
    for (uint8_t i = 0; i < sizeof(en_codes) - 1; ++i)
    {
        x[en_codes[i]] = i;
    }
    return x;
}();

constexpr inline
uint32_t cpow(uint32_t base, uint8_t exp) noexcept
{
    return (exp == 0) ? 1:
        (exp % 2 == 0) ?
                cpow(base, exp / 2) * cpow(base, exp / 2):
                base * cpow(base, (exp - 1) / 2) * cpow(base, (exp - 1) / 2);
}

template <class X> inline
constexpr X sum(X&& x) noexcept { return std::forward<X>(x); }

template <class X, class... Xs> inline
constexpr
std::enable_if_t<!!sizeof...(Xs), std::common_type_t<X, Xs...>> sum(X&& x, Xs&&... xs) noexcept { return x + sum(std::forward<Xs>(xs)...); }

/*
 * Encoder implementation code:
 *   uint32_t (native byte order) ==> array<uchar,5>
 */
template <size_t base, size_t N, size_t... Is> inline
std::array<uchar_t, sizeof...(Is)> _encode(uint32_t val, std::index_sequence<Is...>) noexcept
{
    uint32_t buf{};
    return { en_codes[buf = (val -= buf * cpow(base, N - Is)) / cpow(base, N - Is - 1)] ... };
}

template <size_t base, size_t N> inline
std::array<uchar_t, N> _encode(uint32_t val) noexcept { return _encode<base, N>(val, std::make_index_sequence<N>()); }

/*
 * Decoder implementation code:
 *   array<uchar,5> ==> uint32_t (native byte order)
 */
template <size_t base, size_t N, size_t... Is> inline
uint32_t _decode(std::array<uchar_t, N> const& val, std::index_sequence<Is...>) noexcept
{
    return sum(de_codes[val[Is]] * cpow(base, N - Is - 1)...);
}

template <size_t base, size_t N> inline
uint32_t _decode(std::array<uchar_t, N> const& val) noexcept { return _decode<base, N>(val, std::make_index_sequence<N>()); }

using boost::endian::big_uint32_t;

} // local namespace

using cursor_t = std::pair<uchar_t const*, uchar_t*>;

template <size_t base, size_t N> inline
cursor_t encode(cursor_t locs) noexcept
{
    auto encoded = _encode<base, N>((big_uint32_t&)(*locs.first));
    std::copy_n(encoded.begin(), N, locs.second);
    return {locs.first + sizeof(big_uint32_t), locs.second + N};
}

template <size_t base, size_t N> inline
cursor_t decode(cursor_t locs) noexcept
{
    std::array<uchar_t, N> buf;
    std::copy_n(locs.first, N, buf.begin());
    ((big_uint32_t&)(*locs.second)) = _decode<base, N>(buf);
    return {locs.first + N, locs.second + sizeof(big_uint32_t)};
}

} // namespace z85

using namespace z85;

#include <random>
#include <chrono>
#include <functional>

// g++ -g -Og -std=c++14 -Wall -pedantic -Wno-unused -isystem /path/to/boost/headers z85.cc -o z85
int main()
{
    std::array<uchar_t, 8> const sample {0x86, 0x4F, 0xD2, 0x6F, 0xB5, 0x59, 0xF7, 0x5B};
    std::array<uchar_t, 10> encoded {};
    std::array<uchar_t, sample.size()> decoded {};

    cursor_t encoded_locs{sample.data(), encoded.data()};
    cursor_t decoded_locs{encoded.data(), decoded.data()};

    std::cout << "source: ";
    for (auto x : sample)
        std::cout << std::hex << "0x" << ((unsigned)x) << ' ';
    std::cout << std::endl;

    while (encoded_locs.first < sample.data() + sample.size())
    {
        std::cout << "encoding " << ((void*)encoded_locs.first) << "...\n";
        encoded_locs = encode<85, 5>(encoded_locs);
    }

    std::cout << "encoded: ";
    for (auto x : encoded)
        std::cout << x << '.';
    std::cout << std::endl;

    while (decoded_locs.first < encoded.data() + encoded.size())
    {
        std::cout << "decoding " << ((void*)decoded_locs.first) << "...\n";
        decoded_locs = decode<85, 5>(decoded_locs);
    }

    std::cout << "decoded: ";
    for (auto x : decoded)
        std::cout << std::hex << "0x" << ((unsigned)x) << ' ';
    std::cout << std::endl;

    {
        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<uchar_t> dis;
        std::array<uchar_t, (4 * 1ul<<10)> samples {};
        std::array<uchar_t, (5 * 1ul<<10)> encoded {};
        std::array<uchar_t, (4 * 1ul<<10)> decoded {};

        std::generate_n(samples.begin(), samples.size(), std::bind(dis, std::mt19937(rd())));

        auto const iterations = 20000;

        using clock = std::chrono::steady_clock;

        auto en_code = [&] {
            cursor_t encoded_locs{samples.data(), encoded.data()};
            while (encoded_locs.first < samples.data() + samples.size())
                encoded_locs = encode<85, 5>(encoded_locs);
        };
        en_code();

        auto encoder_started = clock::now();
        for (int i = 0; i < iterations; ++i) en_code();
        auto encoder_stopped = clock::now();

        auto de_code = [&]{
            cursor_t decoded_locs{encoded.data(), decoded.data()};
            while (decoded_locs.first < encoded.data() + encoded.size())
                decoded_locs = decode<85, 5>(decoded_locs);
        };
        de_code();

        auto decoder_started = clock::now();
        for (int i = 0; i < iterations; ++i) de_code();
        auto decoder_stopped = clock::now();

        auto encoder_usec = std::chrono::duration_cast<std::chrono::microseconds>(encoder_stopped - encoder_started);
        auto decoder_usec = std::chrono::duration_cast<std::chrono::microseconds>(decoder_stopped - decoder_started);
        std::cout << "encoder: " << std::dec << encoder_usec.count() << " us, " << ((samples.size() * iterations) / encoder_usec.count()) << " bytes/us \n";
        std::cout << "decoder: " << std::dec << decoder_usec.count() << " us, " << ((samples.size() * iterations) / decoder_usec.count()) << " bytes/us \n";
    }

    return 0;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "random.hpp"

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

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "ublas using vectors : nrm2(v) = " << ublas::norm_2(v) << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "ublas using vectors : nrm2(v) = " << ublas::norm_2(v) << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    ublas::matrix_column<matrix> mc(M, 2);
    ublas::matrix_row<matrix> mr(M, 3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<real>::get();
    std::cout << "eigen using vectors : nrm2(v) = " << v.norm() << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v(n);
    for (size_type i=0; i<n; ++i)
      v(i)=rand_normal<complex>::get();
    std::cout << "eigen using vectors : nrm2(v) = " << v.norm() << '\n'
	      << "blas using vectors  : nrm2(v) = " << blas::nrm2(v) << '\n';
    matrix M(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i)
    	M(i, j)=0;
    auto mc=M.col(2);
    auto mr=M.row(3);
    mc=v;
    std::cout << "blas using cols     : nrm2(v) = " << blas::nrm2(mc) << '\n';
    mr=v;
    std::cout << "blas using rows     : nrm2(v) = " << blas::nrm2(mr) << '\n';
  }
  return EXIT_SUCCESS;
}


#pragma once

#include <polyfem/Common.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>

#include <polyfem/AutodiffTypes.hpp>

#include <Eigen/Dense>
#include <functional>

namespace polyfem
{
	class IncompressibleLinearElasticityDispacement
	{
	public:
		// res is R^{dim²}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 9, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);
		inline int size() const { return size_; }

		inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

		void set_parameters(const json &params);

		void compute_von_mises_stresses(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;
	private:
		int size_ = -1;
		double mu_ = 1;
		double lambda_ = 1;

		void assign_stress_tensor(const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};

	class IncompressibleLinearElasticityMixed
	{
	public:
		// res is R^{dim}
		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		assemble(const ElementAssemblyValues &psi_vals, const ElementAssemblyValues &phi_vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		void set_size(const int size);

		inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

		inline int rows() const { return size_; }
		inline int cols() const { return 1; }

		void set_parameters(const json &params);
	private:
		int size_ = -1;
		double mu_ = 1;
		double lambda_ = 1;
	};

	class IncompressibleLinearElasticityPressure
	{
	public:
		// res is R^{1}
		Eigen::Matrix<double, 1, 1>
		assemble(const ElementAssemblyValues &vals, const int i, const int j, const QuadratureVector &da) const;

		Eigen::Matrix<double, 1, 1>
		compute_rhs(const AutodiffHessianPt &pt) const
		{
			assert(false);
			return Eigen::Matrix<double, 1, 1>::Zero(1,1);
		}

		inline int size() const { return 1; }

		void set_parameters(const json &params);

				inline double &mu() { return mu_; }
		inline double mu() const { return mu_; }

		inline double &lambda() { return lambda_; }
		inline double lambda() const { return lambda_; }

	private:
		double mu_ = 1;
		double lambda_ = 1;
	};
}


#include <skelly_sim.hpp>

#include "cnpy.hpp"
#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <mpi.h>
#include <parse_util.hpp>
#include <system.hpp>

#ifdef NDEBUG
#undef NDEBUG
#include <cassert>
#define NDEBUG
#else
#include <cassert>
#endif

#include <body.hpp>
#include <fiber.hpp>

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXd;

template <typename DerivedA, typename DerivedB>
bool allclose(
    const Eigen::DenseBase<DerivedA> &a, const Eigen::DenseBase<DerivedB> &b,
    const typename DerivedA::RealScalar &rtol = Eigen::NumTraits<typename DerivedA::RealScalar>::dummy_precision(),
    const typename DerivedA::RealScalar &atol = Eigen::NumTraits<typename DerivedA::RealScalar>::epsilon()) {
    return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all();
}

int main(int argc, char *argv[]) {
    int thread_level;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_FUNNELED, &thread_level);
    std::string config_file("test_link_matrix.toml");
    System::init(config_file);
    FiberContainer &fc = *System::get_fiber_container();
    toml::value &param_table = *System::get_param_table();

    toml::table special = param_table["special"].as_table();
    VectorXd fibers_xt = parse_util::convert_array(special.at("fibers_xt").as_array());
    VectorXd body_velocities_flat = parse_util::convert_array(special.at("body_velocities").as_array());
    VectorXd force_torque_ref = parse_util::convert_array(special.at("force_torque").as_array());
    VectorXd velocities_on_fiber_ref = parse_util::convert_array(special.at("fiber_velocities").as_array());
    MatrixXd body_velocities = Map<MatrixXd>(body_velocities_flat.data(), 6, body_velocities_flat.size() / 6);

    fc.update_derivatives();

    MatrixXd force_torque, velocities_on_fiber;
    std::tie(force_torque, velocities_on_fiber) =
        System::calculate_body_fiber_link_conditions(fibers_xt, body_velocities);

    assert(allclose(force_torque, force_torque_ref, 1E-7));
    assert(allclose(velocities_on_fiber, velocities_on_fiber_ref));
    MPI_Finalize();

    std::cout << "Test passed\n";
    return 0;
}


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/LSM9DS1.hpp>
#include <duds/hardware/interface/linux/DevI2c.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <cmath>
#include <boost/exception/diagnostic_information.hpp>
#include <Eigen/Geometry>

bool quit = false;

void makeHorizontal(
	Eigen::Vector3f &res,
	const Eigen::Vector3f &grav,
	const Eigen::Vector3f &mod,
	float &angle
) {
	static const Eigen::Vector3f z(0, 0, 1);
	Eigen::Vector3f g(grav.normalized());
	angle = std::acos(g.dot(z));
	Eigen::Vector3f axis = g.cross(z);
	/*float*/ //angle = std::asin(axis.norm());
	axis.normalize();
	Eigen::Quaternion<float> q(Eigen::AngleAxisf(angle, axis));
	res = q * mod;
}

float heading(const Eigen::Vector3f &dir) {
	/*
	static const Eigen::Vector3f fwd(1, 0, 0);
	Eigen::Vector3f d(dir(0), dir(1), 0);
	d.normalize();
	Eigen::Vector3f a = d.cross(fwd);
	return std::asin(a.norm());
	*/
	float h = std::atan2(dir(2), dir(0));
	if (h < 0) {
		h += 2 * M_PI;
	}
	return h;
}

void runtest(duds::hardware::devices::instruments::LSM9DS1 &acclgyromag)
try {
	static const Eigen::Vector3f z(0, 0, 1);
	duds::hardware::devices::instruments::LSM9DS1::RawSample rsA, rsM;
	acclgyromag.start();
	std::cout.precision(1);
	std::cout << std::fixed;
	// doesn't account for time spent in loop
	std::chrono::milliseconds delay = std::chrono::milliseconds(
		(int)(1000.0f / /*acclgyromag.sampleRate()*/ 2.5f)
	);
	std::this_thread::sleep_for(delay);
	do {
		while (!acclgyromag.sample() && !quit) {
			std::this_thread::sleep_for(std::chrono::milliseconds(8));
		}
		rsA = acclgyromag.rawAccelerometer();
		rsM = acclgyromag.rawMagnetometer();
		Eigen::Vector3f m(rsM.x, rsM.y, rsM.z);
		Eigen::Vector3f g(rsA.x, rsA.y, rsA.z);
		Eigen::Vector3f mT;
		std::cout << "A: " << std::setw(8) << g.norm() << ' ' <<
		std::setw(6) << rsA.x << ", " <<
		std::setw(6) << rsA.y << ", " << std::setw(6) << rsA.z;
		float th;
		makeHorizontal(mT, g, m, th);
		float head = heading(/*mT*/ m);
		std::cout << " th " << std::setw(5) << (th * 180.0f / M_PI);
		// magnetometer vector modified to have X-Y plane perpindicular to the
		// gravity vector
		std::cout << "   M: " << std::setw(7) << mT.norm() << ' ' <<
		std::setw(7) << mT(0) << ", " << std::setw(7) << mT(1) << ", " <<
		std::setw(7) << mT(2) << "  h: " << std::setw(5) <<
		(head * 180.0f / M_PI) << " \r";// std::endl;
		/* unmodified magnetometer vector
		std::cout << "   Mag: " << std::setw(7) << m.norm() << ' ' <<
		std::setw(5) << rsM.x << ", " <<
		std::setw(5) << rsM.y << ", " << std::setw(5) << rsM.z << " \r";// std::endl;
		*/
		std::cout.flush();
		std::this_thread::sleep_for(delay);
	} while (!quit);
} catch (...) {
	std::cerr << "Program failed in runtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

const duds::hardware::devices::instruments::LSM9DS1::Settings config = {
	/* .accelerometer = */ 1,
	/* .gyroscope = */ 0,
	/* .magnetometer = */ 1,
	/* .accelRange = */ duds::hardware::devices::instruments::LSM9DS1::AccelRange2g,
	/* .gyroRange = */ duds::hardware::devices::instruments::LSM9DS1::GyroRange4p276rps,
	/* .magRange = */ duds::hardware::devices::instruments::LSM9DS1::MagRange400uT,
	/* .gyroLowPower = */ 1,
	/* .gyroHighPass = */ 0,
	/* .magLowPower = */ 0,
	/* .xyMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .zMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .magTempComp = */ 0
};

int main(int argc, char *argv[])
try {
	{ // vector math tests
		Eigen::Vector3f res;
		Eigen::Vector3f grav(1.0f, 0, 9.0f);
		Eigen::Vector3f mod(1.0f, 1.0f, 9.0f);
		float angle;
		std::cout << "Vector rotation tests" << std::endl;
		makeHorizontal(res, grav, grav, angle);
		res.normalize();
		std::cout << "Test 1 result: " << res(0) << ", " << res(1) << ", " <<
		res(2) << "  angle: " << (angle * 180.0f / M_PI) << "  heading: " <<
		(heading(res) * 180.0f / M_PI) << std::endl;
		makeHorizontal(res, grav, mod, angle);
		res.normalize();
		std::cout << "Test 2 result: " << res(0) << ", " << res(1) << ", " <<
		res(2) << "  angle: " << (angle * 180.0f / M_PI) << "  heading: " <<
		(heading(res) * 180.0f / M_PI) << std::endl;
	}
	std::unique_ptr<duds::hardware::interface::I2c> magI2c(
		new duds::hardware::interface::linux::DevI2c(
			argc > 1 ? argv[1] : "/dev/i2c-1",
			0x1E
		)
	);
	std::unique_ptr<duds::hardware::interface::I2c> accelI2c(
		new duds::hardware::interface::linux::DevI2c(
			argc > 1 ? argv[1] : "/dev/i2c-1",
			0x6B
		)
	);
	duds::hardware::devices::instruments::LSM9DS1 acclgyromag(accelI2c, magI2c);
	acclgyromag.configure(2.0f, 2.0f, config);
	std::cout.precision(4);
	//std::cout << "Sampling frequency reported as " << acclgyromag.sampleRate() <<
	//"Hz" << std::endl;
	std::thread doit(runtest, std::ref(acclgyromag));
	std::cin.get();
	quit = true;
	doit.join();
	std::cout << std::endl;
	return 0;
} catch (...) {
	std::cerr << "Program failed in main(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
	return 1;
}


//####### Test module for Breit Wheeler engine (SI units) ######################

//Define Module name
 #define BOOST_TEST_MODULE "Nonliner Breit Wheeler engine (SI)"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

 #include <memory>

//Include Boost unit tests library
#include <boost/test/unit_test.hpp>

//SI units are used for this test
#define PXRMP_USE_SI_UNITS
#include "breit_wheeler_engine.hpp"

#include "rng_wrapper.hpp"

using namespace picsar::multi_physics;

//Helper function
template<typename REAL>
breit_wheeler_engine<REAL, stl_rng_wrapper<REAL>> get_bw_stl_set_lambda(uint64_t seed,
breit_wheeler_engine_ctrl<REAL> bw_ctrl = breit_wheeler_engine_ctrl<REAL>())
{
    stl_rng_wrapper<REAL> wrap{seed};
    auto bw_engine =  breit_wheeler_engine<REAL, stl_rng_wrapper<REAL>>
        {std::move(wrap), 1.0, bw_ctrl};
    return bw_engine;
}

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-3;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-2;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//Special tolerance for a specific case (check carefully)

//Special Tolerance for double precision calculations
const double double_spec_tolerance = 2.0e-2;

//Special Tolerance for single precision calculations
const float float_spec_tolerance = 2.0e-2;

//Special Templated tolerance
template <typename T>
T spec_tolerance()
{
    if(std::is_same<T,float>::value)
        return float_spec_tolerance;
    else
        return double_spec_tolerance;
}

//***SI UNITS***
//SI units for momenta
const double me_c = electron_mass * light_speed;
//SI units for fields
double lambda = 800.0 * si_nanometer;
double eref = 2.0*pi*electron_mass*light_speed*light_speed/
            (lambda*elementary_charge);
double bref = eref/light_speed;
//SI units for dt and rate
double dtref = lambda/(2.0*pi*light_speed);
double rateref = 1.0/dtref;

//Test get/set lambda for breit_wheeler_engine generic
template <typename T>
void breit_wheeler_engine_gs()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);
    BOOST_CHECK_EQUAL( static_cast<T>(1.0), bw_engine.get_lambda());
}

//Test get/set lambda for breit_wheeler_engine (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_gs_double_1 )
{
    breit_wheeler_engine_gs<double>();
}

//Test get/set lambda for breit_wheeler_engine (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_gs_single_1 )
{
    breit_wheeler_engine_gs<float>();
}

// ------------- optical depth --------------
//Test get/set lambda for breit_wheeler_engine generic
template <typename T>
void breit_wheeler_engine_opt()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);
    BOOST_TEST ( bw_engine.get_optical_depth() >= static_cast<T>(0.0) );
}

//Test get new optical depth (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_opt_double_1 )
{
    breit_wheeler_engine_opt<double>();
}

//Test get new optical depth (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_opt_single_1 )
{
    breit_wheeler_engine_opt<float>();
}

// ------------- Pair production rates --------------

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_1()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(0.0*me_c);
    T py =  static_cast<T>(0.0*me_c);
    T pz =  static_cast<T>(0.0*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(-861.074*eref);
    T ez =  static_cast<T>(944.652*eref);
    T bx =  static_cast<T>(531.406*bref);
    T by =  static_cast<T>(670.933*bref);
    T bz =  static_cast<T>(660.057*bref);

    //T exp = static_cast<T>(0.0*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_1 )
{
    breit_wheeler_engine_prod_1<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_1 )
{
    breit_wheeler_engine_prod_1<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_2()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(149.825*me_c);
    T py =  static_cast<T>(0.0*me_c);
    T pz =  static_cast<T>(0.0*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(0.0*eref);
    T ez =  static_cast<T>(0.0*eref);
    T bx =  static_cast<T>(0.0*bref);
    T by =  static_cast<T>(0.0*bref);
    T bz =  static_cast<T>(0.0*bref);

    //T exp = static_cast<T>(0.0*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_2 )
{
    breit_wheeler_engine_prod_2<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_2 )
{
    breit_wheeler_engine_prod_2<float>();
}

template <typename T>
void breit_wheeler_engine_prod_3()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(6.81696*me_c);
    T py =  static_cast<T>(9.68933*me_c);
    T pz =  static_cast<T>(2.81229*me_c);
    T ex =  static_cast<T>(-4.89986*eref);
    T ey =  static_cast<T>(-9.65535*eref);
    T ez =  static_cast<T>(3.69471*eref);
    T bx =  static_cast<T>(8.89549*bref);
    T by =  static_cast<T>(-5.46574*bref);
    T bz =  static_cast<T>(-6.75393*bref);

    //T exp = static_cast<T>(0.0*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_3 )
{
    breit_wheeler_engine_prod_3<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_3 )
{
    breit_wheeler_engine_prod_3<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_4()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(149.825*me_c);
    T py =  static_cast<T>(933.115*me_c);
    T pz =  static_cast<T>(-538.195*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(-861.074*eref);
    T ez =  static_cast<T>(944.652*eref);
    T bx =  static_cast<T>(531.406*bref);
    T by =  static_cast<T>(670.933*bref);
    T bz =  static_cast<T>(660.057*bref);

    T exp = static_cast<T>(1.50648551484*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL((exp-res)/exp, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_4 )
{
    breit_wheeler_engine_prod_4<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_4 )
{
    breit_wheeler_engine_prod_4<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_5()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(-44.4546*me_c);
    T py =  static_cast<T>(-0.2033*me_c);
    T pz =  static_cast<T>(94.5843*me_c);
    T ex =  static_cast<T>(39.8996*eref);
    T ey =  static_cast<T>(-29.2501*eref);
    T ez =  static_cast<T>(58.7720*eref);
    T bx =  static_cast<T>(44.3417*bref);
    T by =  static_cast<T>(15.5024*bref);
    T bz =  static_cast<T>(29.4024*bref);

    //T exp = static_cast<T>(4.69766211952e-73*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL(res, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_5 )
{
    breit_wheeler_engine_prod_5<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_5 )
{
    breit_wheeler_engine_prod_5<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_6()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(-965.61*me_c);
    T py =  static_cast<T>(-3975.11*me_c);
    T pz =  static_cast<T>(6917.22*me_c);
    T ex =  static_cast<T>(11.17*eref);
    T ey =  static_cast<T>(-2117.72*eref);
    T ez =  static_cast<T>(-1407.19*eref);
    T bx =  static_cast<T>( 6259.79*bref);
    T by =  static_cast<T>(7557.54*bref);
    T bz =  static_cast<T>(773.11*bref);

    T exp = static_cast<T>(3.51855878777*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL((exp-res)/exp, tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_6 )
{
    breit_wheeler_engine_prod_6<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_6 )
{
    breit_wheeler_engine_prod_6<float>();
}

//Test pair production rates (generic)
template <typename T>
void breit_wheeler_engine_prod_7()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(61019.1*me_c);
    T py =  static_cast<T>(-24359.3*me_c);
    T pz =  static_cast<T>(65116.2*me_c);
    T ex =  static_cast<T>(69942.0*eref);
    T ey =  static_cast<T>(38024.7*eref);
    T ez =  static_cast<T>(-43604.1*eref);
    T bx =  static_cast<T>(-26990.0*bref);
    T by =  static_cast<T>(58267.8*bref);
    T bz =  static_cast<T>(-63485.8*bref);

    T exp = static_cast<T>(7.63488202211*rateref);
    T res = bw_engine.compute_dN_dt(norm(vec3<T>{px,py,pz})*light_speed, chi_photon(px,py,pz,ex,ey,ez,bx,by,bz) );

    BOOST_CHECK_SMALL((exp-res)/exp, spec_tolerance<T>());
}

//Test pair production rates (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_double_7 )
{
    breit_wheeler_engine_prod_7<double>();
}

//Test pair production rates (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_prod_single_7 )
{
    breit_wheeler_engine_prod_7<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_1()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(149.825*me_c);
    T py =  static_cast<T>(933.115*me_c);
    T pz =  static_cast<T>(-538.195*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(-861.074*eref);
    T ez =  static_cast<T>(944.652*eref);
    T bx =  static_cast<T>(531.406*bref);
    T by =  static_cast<T>(670.933*bref);
    T bz =  static_cast<T>(660.057*bref);

    T initial_optical_depth = static_cast<T>(1.0);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01*dtref);

    T exp_rate = static_cast<T>(1.50648551484*rateref);

    bool has_event_happend;
    T dt_prod;

    bw_engine.compute_dN_dt_lookup_table(nullptr);

    std::tie(has_event_happend, dt_prod) =
        bw_engine.evolve_opt_depth_and_determine_event
        (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;

    BOOST_CHECK_EQUAL(has_event_happend, false);
    BOOST_CHECK_EQUAL(dt_prod, static_cast<T>(0.0));
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
        tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_1 )
{
    breit_wheeler_engine_detopt_1<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_1 )
{
    breit_wheeler_engine_detopt_1<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_2()
{
    auto bw_engine = get_bw_stl_set_lambda<T>(390109317);

    T px =  static_cast<T>(149.825*me_c);
    T py =  static_cast<T>(933.115*me_c);
    T pz =  static_cast<T>(-538.195*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(-861.074*eref);
    T ez =  static_cast<T>(944.652*eref);
    T bx =  static_cast<T>(531.406*bref);
    T by =  static_cast<T>(670.933*bref);
    T bz =  static_cast<T>(660.057*bref);

    T initial_optical_depth = static_cast<T>(1.0e-3);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01*dtref);

    T exp_rate = static_cast<T>(1.50648551484*rateref);

    bool has_event_happend;
    T dt_prod;

    bw_engine.compute_dN_dt_lookup_table(nullptr);

    std::tie(has_event_happend, dt_prod) =
        bw_engine.evolve_opt_depth_and_determine_event
        (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;
    T exp_dt_prod = initial_optical_depth/exp_rate ;

    BOOST_CHECK_EQUAL(has_event_happend, true);
    BOOST_CHECK_SMALL(exp_dt_prod, dt_prod);
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
        tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_2 )
{
    breit_wheeler_engine_detopt_2<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_2 )
{
    breit_wheeler_engine_detopt_2<float>();
}

//Test evolve_opt_depth_and_determine_event (generic)
template <typename T>
void breit_wheeler_engine_detopt_3()
{
    breit_wheeler_engine_ctrl<T> bw_ctrl;
    bw_ctrl.chi_phot_min =  static_cast<T>(0.0001);

    auto bw_engine = get_bw_stl_set_lambda<T>(390109317, bw_ctrl);

    T px =  static_cast<T>(149.825*me_c);
    T py =  static_cast<T>(933.115*me_c);
    T pz =  static_cast<T>(-538.195*me_c);
    T ex =  static_cast<T>(931.686*eref);
    T ey =  static_cast<T>(-861.074*eref);
    T ez =  static_cast<T>(944.652*eref);
    T bx =  static_cast<T>(531.406*bref);
    T by =  static_cast<T>(670.933*bref);
    T bz =  static_cast<T>(660.057*bref);

    T initial_optical_depth = static_cast<T>(1.0e-3);
    T optical_depth = initial_optical_depth;
    T dt = static_cast<T>(0.01*dtref);

    T exp_rate = static_cast<T>(1.50648551484*rateref);

    bool has_event_happend;
    T dt_prod;

    bw_engine.compute_dN_dt_lookup_table(nullptr);

    std::tie(has_event_happend, dt_prod) =
    bw_engine.evolve_opt_depth_and_determine_event
    (px, py, pz, ex, ey, ez, bx, by, bz, dt, optical_depth);

    T exp_opt_depth = initial_optical_depth - dt*exp_rate;
    T exp_dt_prod = initial_optical_depth/exp_rate ;

    BOOST_CHECK_EQUAL(has_event_happend, true);
    BOOST_CHECK_SMALL(exp_dt_prod, dt_prod);
    BOOST_CHECK_SMALL((optical_depth-exp_opt_depth)/exp_opt_depth,
    tolerance<T>());
}

//Test evolve_opt_depth_and_determine_event (double precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_double_3 )
{
    breit_wheeler_engine_detopt_3<double>();
}

//Test evolve_opt_depth_and_determine_event (single precision)
BOOST_AUTO_TEST_CASE( breit_wheeler_engine_detopt_single_3 )
{
    breit_wheeler_engine_detopt_3<float>();
}


#ifndef MOCHIMOCHI_ADAM_HPP_
#define MOCHIMOCHI_ADAM_HPP_

#include <Eigen/Dense>
#include <cassert>
#include "../../functions/enumerate.hpp"

class ADAM {
private :
  const std::size_t kDim;

private :
  std::size_t _timestep;
  Eigen::VectorXd _w;
  Eigen::VectorXd _m;
  Eigen::VectorXd _v;

public :
  ADAM(const std::size_t dim)
    : kDim(dim),
      _timestep(0),
      _w(Eigen::VectorXd::Zero(kDim)),
      _m(Eigen::VectorXd::Zero(kDim)),
      _v(Eigen::VectorXd::Zero(kDim)) {

    assert(dim > 0);
  }

  virtual ~ADAM() { }

private :

  double suffer_loss(const Eigen::VectorXd& x, const int y) const {
    return std::max(0.0, 1.0 - y * _w.dot(x));
  }

  double calculate_margin(const Eigen::VectorXd& x) const {
    return _w.dot(x);
  }

public :

  bool update(const Eigen::VectorXd& feature, const int label) {
    constexpr auto kAlpha = 0.001;
    constexpr auto kBeta1 = 0.9;
    constexpr auto kBeta2 = 0.999;
    constexpr auto  kEpsilon = 0.00000001;
    constexpr auto kLambda = 0.99999999;

    if (suffer_loss(feature, label) <= 0.0) { return false; }

    const Eigen::VectorXd gradiant = -label * feature;
    const auto beta1_t = std::pow(kLambda, _timestep) * kBeta1;

    _timestep++;
    functions::enumerate(gradiant.data(), gradiant.data() + gradiant.size(), 0,
                       [&](const std::size_t index, const double value) {
                         _m[index] = beta1_t * _m[index] + (1.0 - beta1_t) * value;
                         _v[index] = kBeta2 * _v[index] + (1.0 - kBeta2) * value * value;
                         const auto m_t = _m[index] / (1.0 - std::pow(kBeta1, _timestep));
                         const auto v_t = _v[index] / (1.0 - std::pow(kBeta2, _timestep));
                         _w[index] -= kAlpha * m_t / (std::sqrt(v_t) + kEpsilon);
                       });

    return true;
  }

  int predict(const Eigen::VectorXd& feature) const {
    return calculate_margin(feature) > 0.0 ? 1 : -1;
  }

};

#endif //MOCHIMOCHI_ADAM_HPP_


#pragma once

#include <cmath>
#include <string>
#include <iostream>
#include <fstream>
#include <boost/numeric/ublas/matrix.hpp>

using namespace boost::numeric::ublas;

class gravity_calc {
private:
	double GM = 398600.4415;
	double a = 6378137;
public:
	void get_stokes_coef(int n, matrix<double>& N, matrix<double>& M);
	void Legendre(int n, double lat, matrix<double>& L);
	double geopotential(double r, double lat, double lon, int n, matrix<double> N, matrix<double> M);
	int semifactorial(int n);
};


/* boost random/normal_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: normal_distribution.hpp 11696 2001-11-14 21:53:38Z jmaurer $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP

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

namespace boost {

// deterministic polar method, uses trigonometric functions
template<class UniformRandomNumberGenerator, class RealType = double>
class normal_distribution
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  explicit normal_distribution(base_type & rng, const result_type& mean = 0,
                               const result_type& sigma = 1)
    : _rng(rng), _mean(mean), _sigma(sigma), _valid(false)
  {
    assert(sigma >= 0);
  }

  // compiler-generated copy constructor is NOT fine, need to purge cache
  normal_distribution(const normal_distribution& other)
    : _rng(other._rng), _mean(other._mean), _sigma(other._sigma), _valid(false)
  {
  }
  // uniform_01 cannot be assigned, neither can this class

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::sqrt; using std::log; using std::sin; using std::cos;
#endif
    if(!_valid) {
      _r1 = _rng();
      _r2 = _rng();
      _cached_rho = sqrt(-2 * log(1.0-_r2));
      _valid = true;
    } else {
      _valid = false;
    }
    // Can we have a boost::mathconst please?
    const double pi = 3.14159265358979323846;

    return _cached_rho * (_valid ? cos(2*pi*_r1) : sin(2*pi*_r1)) * _sigma +
      _mean;
  }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const normal_distribution& x,
                         const normal_distribution& y)
  {
    return x._mean == y._mean && x._sigma == y._sigma &&
      x._valid == y._valid && x._rng == y._rng;
  }
#else
  // Use a member function
  bool operator==(const normal_distribution& rhs) const
  {
    return _mean == rhs._mean && _sigma == rhs._sigma &&
      _valid == rhs._valid && _rng == rhs._rng;
  }
#endif
private:
  uniform_01<base_type, RealType> _rng;
  const result_type _mean, _sigma;
  result_type _r1, _r2, _cached_rho;
  bool _valid;
};

} // namespace boost

#endif // BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_SCALAR_MNORMINF_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_SCALAR_MNORMINF_HPP_INCLUDED
#include <nt2/linalg/functions/mnorminf.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/asum1.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/globalasum1.hpp>
#include <nt2/include/functions/iscolumn.hpp>
#include <nt2/include/functions/isrow.hpp>
#include <nt2/include/functions/ismatrix.hpp>
#include <nt2/core/container/dsl/forward.hpp>
#include <nt2/core/functions/table/details/is_definitely_vector.hpp>
#include <boost/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/static_assert.hpp>

//  infinity norm  of a matrix  (maximum row sum)
// TODO optimize mnorminf(trans(a)) as mnorm1(a)

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( mnorminf_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef typename meta::as_real<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return nt2::abs(a0);
    }

  };

  BOOST_DISPATCH_IMPLEMENT  ( mnorminf_, tag::cpu_
                            , (A0)
                            , ((ast_<A0, nt2::container::domain>))
                            )
  {
    typedef typename A0::value_type                   type_t;
    typedef typename meta::as_real<type_t>::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      BOOST_ASSERT_MSG(nt2::ismatrix(a0), "a0 is not a matrix");
      typedef typename details::is_col_vector<typename A0::extent_type>::type choice_t;
      return eval(a0, choice_t());
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::true_ const &) const //static col vector
    {
      return nt2::globalmax(nt2::abs(a0));
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, boost::mpl::false_ const &) const  // not static col vector
    {
      typedef typename details::is_row_vector<typename A0::extent_type>::type choice_t;
      return eval2(a0, choice_t());
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, boost::mpl::true_ const &) const //static row vector
    {
      return nt2::globalasum1(a0);
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, boost::mpl::false_ const &) const //not static vector
    {
      return nt2::globalmax(nt2::asum1(a0, 2));
    }

    BOOST_FORCEINLINE result_type
    eval2(A0 const& a0, nt2::meta::indeterminate_ const &) const
    {
      return  eval(a0, nt2::meta::indeterminate_());
    }

    BOOST_FORCEINLINE result_type
    eval(A0 const& a0, nt2::meta::indeterminate_ const &) const
    {
      if (iscolumn(a0)) // col vector
      {
        return eval(a0, boost::mpl::true_());
      }
      else if (isrow(a0)) //row vector
      {
        return eval2(a0, boost::mpl::true_());
      }
      else  // matrix but not vector
      {
        return eval2(a0, boost::mpl::false_());
      }
    }
  };
} }

#endif


#include <Eigen/Core>
#include <fstream>
#include "json/json.h"
#include <aruco/aruco.h>
#include <aruco/cvdrawingutils.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv/cv.h>
#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc.hpp>

Json::Value mat2json(cv::Mat m)
{
    Json::Value r;
    for(int i = 0; i < m.rows; i++)
    {
        Json::Value q;
      for(int j = 0; j < m.cols; j++)
            q[j] = m.at<double>(i,j);
        r[i] = q;
    }
    return r;
}

Json::Value vec2json(double * p, int n)
{
    Json::Value r;
    for(int i = 0; i < n; i++)
        r[i] = p[i];
    return r;
}

Json::Value mat2json1(Eigen::Matrix4d m)
{
    Json::Value r;
    double *v_ptr = m.data();
    for (int i = 0; i < 16; ++i)
        r.append(Json::Value(v_ptr[i]));
    return r;
}

Json::Value mat2json(const Eigen::Matrix4d & m)
{
    Json::Value r;
    for(int i = 0; i < m.rows(); i++)
    {
        Json::Value q;
      for(int j = 0; j < m.cols(); j++)
            q[j] = m(i,j);
        r[i] = q;
    }
    return r;
}

int main(int argc, char const *argv[])
{
	// first is camera as OpenCV
	// second is file
	// output is array of markers
	bool y_axis_perpendicular = false;

	if(argc < 5)
	{
		std::cerr << "Expected: calibrationfile.yaml command.yaml imagefile dist [outfile|-]\n";
		return -1;
	}

	cv::Mat camera_matrix,dist_coeffs;
	cv::FileStorage camera_calibration_file(argv[1], cv::FileStorage::READ);
    if (!camera_calibration_file.isOpened())
    {
    	std::cerr << "wrong calibration file" << std::endl;
    	return -2;
    }
	camera_calibration_file["rgb_intrinsics"] >> camera_matrix;
	camera_calibration_file["rgb_distortion"] >> dist_coeffs;
	if(!camera_matrix.rows || !dist_coeffs.rows)
	{
		std::cerr << "expected rgb_intrinsics or rgb_distortion in file" << std::endl;
		return -1;
	}

    std::cout << "K:\n" <<camera_matrix << std::endl;
    std::cout << "dist:\n" << dist_coeffs << std::endl;

    cv::Mat frame = cv::imread(argv[3]);
    if(frame.rows == 0)
    {
    	std::cerr << "cannot open image:" << argv[3]<<std::endl;
    	return -3;
    }

    if(atoi(argv[4]) != 0)
    {
        // setZero
        for(int i = 0; i < dist_coeffs.cols; i++)
            dist_coeffs.at<float>(i) = 0;
    }

    aruco::CameraParameters cp(camera_matrix,dist_coeffs, cv::Size(frame.cols,frame.rows));

    cv::FileStorage overlay(argv[2], cv::FileStorage::READ);

    for(int i = 0; i < 10; i++)
    {
        aruco::Marker marker;
        char name[128];
        sprintf(name,"markerpose%d",i);
        cv::Mat pose;
        overlay[name] >> pose;
        if(pose.rows == 0)
            continue;
        int id;
        sprintf(name,"markerid%d",i);
        overlay[name] >> id;
        std::cout << "found marker " << i << " with id " << id << " at " << pose << std::endl;
        sprintf(name,"markersize%d",i);
        double size = 1.0;
        overlay[name] >> size;
        sprintf(name,"mode%d",i);
        int flag = 0;
        overlay[name] >> flag;

        marker.id = id;
        marker.ssize = size;
        marker.resize(4);

        // decompose the matrix? not needed
        // marker.Rvec =
        marker.Tvec.create(3, 1, CV_32FC1);
        marker.Rvec.create(3, 1, CV_32FC1);
        marker.Tvec.at<float>(0,0) = pose.at<double>(0,3);
        marker.Tvec.at<float>(1,0) = pose.at<double>(1,3);
        marker.Tvec.at<float>(2,0) = pose.at<double>(2,3);
        cv::Rodrigues(pose(cv::Rect(0,0,3,3)),marker.Rvec);

/*
void MarkerDetector::distortPoints(vector< cv::Point2f > in, vector< cv::Point2f > &out, const Mat &camMatrix, const Mat &distCoeff) {
    // trivial extrinsics
    cv::Mat Rvec = cv::Mat(3, 1, CV_32FC1, cv::Scalar::all(0));
    cv::Mat Tvec = Rvec.clone();
    // calculate 3d points and then reproject, so opencv makes the distortion internally
    vector< cv::Point3f > cornersPoints3d;
    for (unsigned int i = 0; i < in.size(); i++)
        cornersPoints3d.push_back(cv::Point3f((in[i].x - camMatrix.at< float >(0, 2)) / camMatrix.at< float >(0, 0), // x
                                              (in[i].y - camMatrix.at< float >(1, 2)) / camMatrix.at< float >(1, 1), // y
                                              1)); // z
    cv::projectPoints(cornersPoints3d, Rvec, Tvec, camMatrix, distCoeff, out);
}

cv::undistortPoints(contour2f, contour2f, camMatrix, distCoeff, cv::Mat(), camMatrix);

*/

    cv::Mat ImagePoints(4, 1, CV_32FC2, cv::Scalar::all(0));
        std::vector<cv::Point3f> corners;
        corners.push_back(cv::Point3f(size/2,size/2,0));
        corners.push_back(cv::Point3f(size/2,-size/2,0));
        corners.push_back(cv::Point3f(-size/2,-size/2,0));
        corners.push_back(cv::Point3f(-size/2,size/2,0));
        std::cout << "T  " << marker.Tvec << std::endl;
        std::cout << "R  " << marker.Rvec << std::endl;
        std::cout << "C  " << corners << std::endl;
        std::cout << "K  " << camera_matrix << std::endl;
        std::cout << "DI " << dist_coeffs << std::endl;
        cv::projectPoints(corners,marker.Rvec,marker.Tvec,camera_matrix,dist_coeffs,ImagePoints);
        std::cout << "IP " << ImagePoints << std::endl;
        for(int j = 0; j < 4; j++)
        {
            marker[j].x = ImagePoints.at<cv::Point2f>(j).x;
            marker[j].y = ImagePoints.at<cv::Point2f>(j).y;
        }
        if(flag & 1)
        {
            marker.draw(frame, cv::Scalar(0,0,255), 2);
        }
        if(flag & 2)
        {
            aruco::CvDrawingUtils::draw3dAxis(frame, marker, cp);
        }

    }
    if(argc < 6 || strcmp(argv[5],"-") == 0)
    {
        cv::imshow("ciao",frame);
        cv::waitKey(0);
    }
    else
        cv::imwrite(argv[5],frame);
    return 0;
}