// Copyright (c) 2014, NICTA.
// This file is licensed under the General Public License version 3 or later.
//!
//! Contains the implementation for the thermal forward model.
//!
//! \file fwdmodel/thermal.cpp
//! \author Lachlan McCalman
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#include <glog/logging.h>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-compare"
#include "thermal.hpp"
#pragma GCC diagnostic pop

#include "boost/multi_array.hpp"
#include "world/voxelise.hpp"
#include <iostream>
#include <algorithm>
#include <Eigen/SparseCore>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>

#include <chrono>
typedef std::chrono::high_resolution_clock hrc;

namespace obsidian
{
  namespace fwd
  {
    boost::multi_array<double, 3> fillAndPad(const Eigen::VectorXd& values, const ThermalSpec& spec)
    {
      // The true grid size
      uint nx = spec.voxelisation.xResolution;
      uint ny = spec.voxelisation.yResolution;
      uint nz = spec.voxelisation.zResolution;
      // Make sure we have the right amount of data
      CHECK_EQ(nx * ny * nz, values.size());

      uint bigx = nx + 2 + 2; // top and bottom padding
      uint bigy = ny + 2 + 2; // top and bottom padding
      uint bigz = nz + 1 + 2; // top and bottom padding
      // The multiarrays we'll use
      boost::multi_array<double, 3> volume(boost::extents[bigx][bigy][bigz]);
      // fill in the values to the multiarray
      for (uint i = 0; i < bigx; i++)
      {
        for (uint j = 0; j < bigy; j++)
        {
          for (uint k = 0; k < bigz; k++)
          {
            // first we contrain these to only index the internal region
            // ie no padding. Then we remove the padding offsets so we're
            // actually indexing into the original values matrix
            uint smalli = std::min(std::max(i, (uint) 2), (bigx - 1) - 2) - 2;
            uint smallj = std::min(std::max(j, (uint) 2), (bigy - 1) - 2) - 2;
            uint smallk = std::min(std::max(k, (uint) 1), (bigz - 1) - 2) - 1;
            // Compute the 1D offset into our values matrix
            uint c = smalli * ny * nz + smallj * nz + smallk;
            volume[i][j][k] = values(c);
          }
        }
      }
      return volume;
    }

    boost::multi_array<double, 3> eastWest(const boost::multi_array<double, 3>& pad)
    {
      // The padded grid size
      uint padx = pad.shape()[0];
      uint pady = pad.shape()[1];
      uint padz = pad.shape()[2];
      // new size -- one off all dims and 1 further off the summing dimension
      uint nx = (padx - 1) - 1;
      uint ny = (pady - 1);
      uint nz = (padz - 1);
      boost::multi_array<double, 3> ew(boost::extents[nx][ny][nz]);
      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          for (uint k = 0; k < nz; k++)
          {
            double t1 = pad[i + 1][j][k];
            double t2 = pad[i + 1][j + 1][k];
            double t3 = pad[i + 1][j][k + 1];
            double t4 = pad[i + 1][j + 1][k + 1];
            ew[i][j][k] = (t1 + t2 + t3 + t4) / 4.0;
          }
        }
      }
      return ew;
    }

    boost::multi_array<double, 3> cells(const boost::multi_array<double, 3>& pad, uint axis)
    {
      // The padded grid size
      uint padx = pad.shape()[0];
      uint pady = pad.shape()[1];
      uint padz = pad.shape()[2];
      // bools to switch which axis we're dealing with
      CHECK(axis < 3);
      uint isx = axis == 0;
      uint isy = axis == 1;
      uint isz = axis == 2;
      // new size -- one off all dims and 1 further off the summing dimension
      uint nx = (padx - 1) - isx;
      uint ny = (pady - 1) - isy;
      uint nz = (padz - 1) - isz;
      boost::multi_array<double, 3> ew(boost::extents[nx][ny][nz]);

      // Compute our indexing offsets
      Eigen::MatrixXi indices(4, 3);
      indices << 1, 0, 0, 1, 1, 0, 1, 0, 1, 1, 1, 1;
      uint xcol = isx * 0 + isy * 1 + isz * 2;
      uint ycol = isx * 2 + isy * 0 + isz * 1;
      uint zcol = isx * 1 + isy * 2 + isz * 0;

      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          for (uint k = 0; k < nz; k++)
          {
            double t1 = pad[i + indices(0, xcol)][j + indices(0, ycol)][k + indices(0, zcol)];
            double t2 = pad[i + indices(1, xcol)][j + indices(1, ycol)][k + indices(1, zcol)];
            double t3 = pad[i + indices(2, xcol)][j + indices(2, ycol)][k + indices(2, zcol)];
            double t4 = pad[i + indices(3, xcol)][j + indices(3, ycol)][k + indices(3, zcol)];
            ew[i][j][k] = (t1 + t2 + t3 + t4) / 4.0;
          }
        }
      }
      return ew;
    }

    boost::multi_array<double, 3> isocells(const boost::multi_array<double, 3>& pad)
    {
      // The padded grid size
      uint padx = pad.shape()[0];
      uint pady = pad.shape()[1];
      uint padz = pad.shape()[2];
      uint nx = padx - 1;
      uint ny = pady - 1;
      uint nz = padz - 1;
      boost::multi_array<double, 3> a(boost::extents[nx][ny][nz]);

      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          for (uint k = 0; k < nz; k++)
          {
            bool px0 = i > 0;
            bool px1 = i < nx - 1;
            bool py0 = j > 0;
            bool py1 = j < ny - 1;
            bool pz0 = k > 0;
            bool pz1 = k < nz - 1;
            bool t1On = (px0 && py0 && pz0);
            bool t2On = (px0 && py0 && pz1);
            bool t3On = (px0 && py1 && pz0);
            bool t4On = (px0 && py1 && pz1);
            bool t5On = (px1 && py0 && pz0);
            bool t6On = (px1 && py0 && pz1);
            bool t7On = (px1 && py1 && pz0);
            bool t8On = (px1 && py1 && pz1);
            double t1 = t1On * pad[i][j][k];
            double t2 = t2On * pad[i][j][k + 1];
            double t3 = t3On * pad[i][j + 1][k];
            double t4 = t4On * pad[i][j + 1][k + 1];
            double t5 = t5On * pad[i + 1][j][k];
            double t6 = t6On * pad[i + 1][j][k + 1];
            double t7 = t7On * pad[i + 1][j + 1][k];
            double t8 = t8On * pad[i + 1][j + 1][k + 1];
            double norm = (t1On + t2On + t3On + t4On + t5On + t6On + t7On + t8On);
            a[i][j][k] = (t1 + t2 + t3 + t4 + t5 + t6 + t7 + t8) / (8.0 * norm);
          }
        }
      }
      return a;
    }

    boost::multi_array<double, 3> temp(const boost::multi_array<double, 3>& eastwest, const boost::multi_array<double, 3>& northsouth,
                                       const boost::multi_array<double, 3>& updown, const boost::multi_array<double, 3>& sCells,
                                       const Eigen::MatrixXd& tempZ0, const Eigen::MatrixXd& zLowBound, bool zLowBoundIsHeatFlow,
                                       double xSize, double ySize, double zSize)
    {
      //remove the padding from sCells
      uint nx = sCells.shape()[0] - 2;
      uint ny = sCells.shape()[1] - 2;
      uint nz = sCells.shape()[2] - 2;
      uint n = nx * ny * nz; // total n

      double dx = xSize / double(nx - 1); // original number of cells
      double dy = ySize / double(ny - 1); // original number of cells
      double dz = zSize / double(nz);

      Eigen::SparseMatrix<double> A(n, n);  // big sparse matrix
      std::vector<Eigen::Triplet<double>> coeffs;
      Eigen::VectorXd b = Eigen::VectorXd::Zero(n); // constant term

      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          for (uint k = 0; k < nz; k++)
          {
            uint row = i * ny * nz + j * nz + k;
            uint col = row;
            b(row) = -1 * sCells[i + 1][j + 1][k + 1]; //ignores the outer layer of sCells

            double val_west = eastwest[i][j + 1][k + 1];
            double val_east = eastwest[i + 1][j + 1][k + 1];
            double val_north = northsouth[i + 1][j][k + 1];
            double val_south = northsouth[i + 1][j + 1][k + 1];
            double val_up = updown[i + 1][j + 1][k];
            double val_down = updown[i + 1][j + 1][k + 1];

            double z_up_contrib = val_up / (dz * dz);
            double z_down_contrib = val_down / (dz * dz);
            double y_north_contrib = val_north / (dy * dy);
            double y_south_contrib = val_south / (dy * dy);
            double x_west_contrib = val_west / (dx * dx);
            double x_east_contrib = val_east / (dx * dx);

            double sum = -1 * (z_up_contrib + z_down_contrib + y_north_contrib + y_south_contrib + x_west_contrib + x_east_contrib);
            // Assign to a
            coeffs.push_back( { row, col, sum });
            // Z-axis contribution
            // Top
            if (k == 0)
            {
              b(row) = b(row) - z_up_contrib * tempZ0(i, j);
            } else
            {
              col = i * ny * nz + j * nz + k - 1;
              coeffs.push_back( { row, col, z_up_contrib });
            }

            // Bottom
            if (k == nz - 1)
            {
              if (zLowBoundIsHeatFlow)
              {
                col = row;
                coeffs.push_back( { row, col, z_down_contrib });
                b(row) = b(row) - zLowBound(i, j) / dz;
              } else
              {
                b(row) = b(row) - z_down_contrib * zLowBound(i, j);
              }
            } else
            {
              col = i * ny * nz + j * nz + k + 1;
              coeffs.push_back( { row, col, z_down_contrib });
            }

            // Y-axis contribution
            if (j == 0)
            {
              col = i * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, y_north_contrib });
            } else
            {
              col = i * ny * nz + (j - 1) * nz + k;
              coeffs.push_back( { row, col, y_north_contrib });
            }
            if (j == ny - 1)
            {
              col = i * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, y_south_contrib });
            } else
            {
              col = i * ny * nz + (j + 1) * nz + k;
              coeffs.push_back( { row, col, y_south_contrib });
            }
            // X-axis contribution
            if (i == 0)
            {
              col = i * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, x_west_contrib });
            } else
            {
              col = (i - 1) * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, x_west_contrib });
            }
            if (i == nx - 1)
            {
              col = i * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, x_east_contrib });
            } else
            {
              col = (i + 1) * ny * nz + j * nz + k;
              coeffs.push_back( { row, col, x_east_contrib });
            }

          }
        }
      }

      A.setFromTriplets(coeffs.begin(), coeffs.end());

      // solve for -A
      A = -1 * A;
      b = -1 * b;

      Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
      solver.setTolerance(1.0e-4); // tested using demo3dworld - should be +- 0.5 degrees under typical use @ 40*24*32
      solver.compute(A);

      if (solver.info() != Eigen::Success)
      {
        LOG(ERROR)<< "Matrix decomposition failed";
      }
      Eigen::VectorXd tvec(n);
      tvec = solver.solve(b);
      if (solver.info() != Eigen::Success)
      {
        LOG(ERROR)<< "Linear system could not be solved";
      }
      boost::multi_array<double, 3> t(boost::extents[nx][ny][nz + 1]);
      // add the surface temperature

      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          t[i][j][0] = tempZ0(i, j);
        }
      }
      // count from z=1
      uint c = 0;
      for (uint i = 0; i < nx; i++)
      {
        for (uint j = 0; j < ny; j++)
        {
          for (uint k = 1; k < nz + 1; k++)
          {
            t[i][j][k] = tvec(c);
            c++;
          }
        }
      }
      return t;
    }

    uint clip(int in, uint maxX)
    {
      uint x = (in > 0) * in;
      x = maxX - (x < maxX) * (maxX - x);
      return (uint) x;
    }

    Eigen::VectorXd evalAtLocations(const boost::multi_array<double, 3>& tempVox, const Eigen::MatrixXd& locations,
                                    const std::pair<double, double>& xSize, const std::pair<double, double>& ySize,
                                    const std::pair<double, double>& zSize)
    {
      uint nx = tempVox.shape()[0];
      uint ny = tempVox.shape()[1];
      uint nz = tempVox.shape()[2];
      double x0 = xSize.first;
      double y0 = ySize.first;
      double z0 = zSize.first;
      double deltaX = xSize.second - x0;
      double deltaY = ySize.second - y0;
      double deltaZ = zSize.second - z0;
      Eigen::VectorXd temps(locations.rows());
      for (uint i = 0; i < locations.rows(); i++) //AAA
      {
        // Clip rather than check
        double xx = (double) (nx - 1) * (locations(i, 0) - x0) / deltaX;
        double yy = (double) (ny - 1) * (locations(i, 1) - y0) / deltaY;
        double zz = (double) (nz - 1) * (locations(i, 2) - z0) / deltaZ;

        // Clip so we get continuous edge conditions after the image
        uint x1 = clip(xx, nx - 1);
        uint x2 = clip(xx + 1, nx - 1);
        uint y1 = clip(yy, ny - 1);
        uint y2 = clip(yy + 1, ny - 1);
        uint z1 = clip(zz, nz - 1);
        uint z2 = clip(zz + 1, nz - 1);

        double alpha2 = xx - (double) x1;
        double beta2 = yy - (double) y1;
        double gamma2 = zz - (double) z1;
        double alpha1 = 1.0 - alpha2;
        double beta1 = 1.0 - beta2;
        double gamma1 = 1.0 - gamma2;

        /*Eigen::VectorXi pos(6);
         pos << xx,yy,zz, (alpha1*(double)x1 + alpha2*(double)x2),(beta1*(double)y1 + beta2*(double)y2),(gamma1*(double)z1 + gamma2*(double)z2);
         std::cout << pos.transpose() <<"\n";
         */

        // who needs loops?
        temps(i) = alpha1 * beta1 * gamma1 * tempVox[x1][y1][z1] + alpha1 * beta1 * gamma2 * tempVox[x1][y1][z2]
            + alpha1 * beta2 * gamma1 * tempVox[x1][y2][z1] + alpha1 * beta2 * gamma2 * tempVox[x1][y2][z2]
            + alpha2 * beta1 * gamma1 * tempVox[x2][y1][z1] + alpha2 * beta1 * gamma2 * tempVox[x2][y1][z2]
            + alpha2 * beta2 * gamma1 * tempVox[x2][y2][z1] + alpha2 * beta2 * gamma2 * tempVox[x2][y2][z2]; //ouch
      }
      return temps;
    }

    template<>
    ThermalCache generateCache<ForwardModel::THERMAL>(const std::vector<world::InterpolatorSpec>& boundaryInterpolation,
                                                      const WorldSpec& worldSpec, const ThermalSpec& thermSpec)
    {
      const VoxelSpec& thermVox = thermSpec.voxelisation;
      world::Query thermQuery(boundaryInterpolation, worldSpec, thermVox.xResolution, thermVox.yResolution, thermVox.zResolution,
                              world::SamplingStrategy::noAA);
      return
      { boundaryInterpolation, thermQuery, worldSpec.xBounds, worldSpec.yBounds, worldSpec.zBounds};
    }

    template<>
    ThermalResults forwardModel<ForwardModel::THERMAL>(const ThermalSpec& spec, const ThermalCache& cache, const WorldParams& world)
    {
      Eigen::MatrixXd cond = world::getVoxels(cache.boundaryInterpolation, world, cache.query, RockProperty::ThermalConductivity); // conductivity shouldnt go to zero...
      Eigen::MatrixXd prod = world::getVoxels(cache.boundaryInterpolation, world, cache.query, RockProperty::ThermalProductivity);

      auto volume = fillAndPad(world::flatten(cond), spec);
      auto eastwest = cells(volume, 0);
      auto northsouth = cells(volume, 1);
      auto updown = cells(volume, 2);
      auto sVolume = fillAndPad(world::flatten(prod), spec);
      auto sCells = isocells(sVolume);

      uint nx = spec.voxelisation.xResolution;
      uint ny = spec.voxelisation.yResolution;
      Eigen::MatrixXd tempZ0 = Eigen::MatrixXd::Ones(nx + 1, ny + 1) * spec.surfaceTemperature;
      Eigen::MatrixXd zLowBound = Eigen::MatrixXd::Ones(nx + 1, ny + 1) * spec.lowerBoundary;
      bool isHeatFlow = spec.lowerBoundaryIsHeatFlow;
      double xSize = cache.xBounds.second - cache.xBounds.first;
      double ySize = cache.yBounds.second - cache.yBounds.first;
      double zSize = cache.zBounds.second - cache.zBounds.first;
      boost::multi_array<double, 3> tempVox = temp(eastwest, northsouth, updown, sCells, tempZ0, zLowBound, isHeatFlow, xSize, ySize,
                                                   zSize);

      ThermalResults results;
      results.readings = evalAtLocations(tempVox, spec.locations, cache.xBounds, cache.yBounds, cache.zBounds);
      return results;
    }

  }
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_
//
// Comparison operators for cpp_int_backend:
//
#ifndef BOOST_MP_CPP_INT_MISC_HPP
#define BOOST_MP_CPP_INT_MISC_HPP

#include <boost/multiprecision/detail/bitscan.hpp> // lsb etc

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4702)
#endif

namespace boost{ namespace multiprecision{ namespace backends{

template <class R, class CppInt>
void check_in_range(const CppInt& val, const mpl::int_<checked>&)
{
   typedef typename boost::multiprecision::detail::canonical<R, CppInt>::type cast_type;
   if(val.sign())
   {
      if(val.compare(static_cast<cast_type>((std::numeric_limits<R>::min)())) < 0)
         BOOST_THROW_EXCEPTION(std::overflow_error("Could not convert to the target type - -value is out of range."));
   }
   else
   {
      if(val.compare(static_cast<cast_type>((std::numeric_limits<R>::max)())) > 0)
         BOOST_THROW_EXCEPTION(std::overflow_error("Could not convert to the target type - -value is out of range."));
   }
}
template <class R, class CppInt>
inline void check_in_range(const CppInt& /*val*/, const mpl::int_<unchecked>&) BOOST_NOEXCEPT {}

inline void check_is_negative(const mpl::true_&) BOOST_NOEXCEPT {}
inline void check_is_negative(const mpl::false_&)
{
   BOOST_THROW_EXCEPTION(std::range_error("Attempt to assign a negative value to an unsigned type."));
}

template <class Integer>
inline Integer negate_integer(Integer i, const mpl::true_&) BOOST_NOEXCEPT
{
   return -i;
}
template <class Integer>
inline Integer negate_integer(Integer i, const mpl::false_&) BOOST_NOEXCEPT
{
   return ~(i-1);
}

template <class R, unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<is_integral<R>::value && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, void>::type
   eval_convert_to(R* result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& backend) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   typedef mpl::int_<Checked1> checked_type;
   check_in_range<R>(backend, checked_type());

   *result = static_cast<R>(backend.limbs()[0]);
   unsigned shift = cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   for(unsigned i = 1; (i < backend.size()) && (shift < static_cast<unsigned>(std::numeric_limits<R>::digits)); ++i)
   {
      *result += static_cast<R>(backend.limbs()[i]) << shift;
      shift += cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   }
   if(backend.sign())
   {
      check_is_negative(boost::is_signed<R>());
      *result = negate_integer(*result, boost::is_signed<R>());
   }
}

template <class R, unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<is_floating_point<R>::value && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, void>::type
   eval_convert_to(R* result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& backend) BOOST_NOEXCEPT_IF(is_arithmetic<R>::value)
{
   typename cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::const_limb_pointer p = backend.limbs();
   unsigned shift = cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   *result = static_cast<R>(*p);
   for(unsigned i = 1; i < backend.size(); ++i)
   {
      *result += static_cast<R>(std::ldexp(static_cast<long double>(p[i]), shift));
      shift += cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   }
   if(backend.sign())
      *result = -*result;
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
BOOST_MP_FORCEINLINE typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, bool>::type
   eval_is_zero(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val) BOOST_NOEXCEPT
{
   return (val.size() == 1) && (val.limbs()[0] == 0);
}
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
BOOST_MP_FORCEINLINE typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, int>::type
   eval_get_sign(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val) BOOST_NOEXCEPT
{
   return eval_is_zero(val) ? 0 : val.sign() ? -1 : 1;
}
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
BOOST_MP_FORCEINLINE typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_abs(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   result = val;
   result.sign(false);
}

//
// Get the location of the least-significant-bit:
//
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, unsigned>::type
   eval_lsb(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a)
{
   using default_ops::eval_get_sign;
   if(eval_get_sign(a) == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(a.sign())
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }

   //
   // Find the index of the least significant limb that is non-zero:
   //
   unsigned index = 0;
   while(!a.limbs()[index] && (index < a.size()))
      ++index;
   //
   // Find the index of the least significant bit within that limb:
   //
   unsigned result = boost::multiprecision::detail::find_lsb(a.limbs()[index]);

   return result + index * cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
}

//
// Get the location of the most-significant-bit:
//
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, unsigned>::type
   eval_msb(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a)
{
   using default_ops::eval_get_sign;
   if(eval_get_sign(a) == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(a.sign())
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }

   //
   // Find the index of the most significant bit that is non-zero:
   //
   return (a.size() - 1) * cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits + boost::multiprecision::detail::find_msb(a.limbs()[a.size() - 1]);
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, bool>::type
   eval_bit_test(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val, unsigned index) BOOST_NOEXCEPT
{
   unsigned offset = index / cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   unsigned shift = index % cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   limb_type mask = shift ? limb_type(1u) << shift : limb_type(1u);
   if(offset >= val.size())
      return false;
   return val.limbs()[offset] & mask ? true : false;
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_bit_set(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val, unsigned index)
{
   unsigned offset = index / cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   unsigned shift = index % cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   limb_type mask = shift ? limb_type(1u) << shift : limb_type(1u);
   if(offset >= val.size())
   {
      unsigned os = val.size();
      val.resize(offset + 1, offset + 1);
      if(offset >= val.size())
         return;  // fixed precision overflow
      for(unsigned i = os; i <= offset; ++i)
         val.limbs()[i] = 0;
   }
   val.limbs()[offset] |= mask;
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_bit_unset(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val, unsigned index) BOOST_NOEXCEPT
{
   unsigned offset = index / cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   unsigned shift = index % cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   limb_type mask = shift ? limb_type(1u) << shift : limb_type(1u);
   if(offset >= val.size())
      return;
   val.limbs()[offset] &= ~mask;
   val.normalize();
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_bit_flip(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val, unsigned index)
{
   unsigned offset = index / cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   unsigned shift = index % cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::limb_bits;
   limb_type mask = shift ? limb_type(1u) << shift : limb_type(1u);
   if(offset >= val.size())
   {
      unsigned os = val.size();
      val.resize(offset + 1, offset + 1);
      if(offset >= val.size())
         return;  // fixed precision overflow
      for(unsigned i = os; i <= offset; ++i)
         val.limbs()[i] = 0;
   }
   val.limbs()[offset] ^= mask;
   val.normalize();
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_qr(
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& x,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& y,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& q,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& r) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   divide_unsigned_helper(&q, x, y, r);
   q.sign(x.sign() != y.sign());
   r.sign(x.sign());
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_qr(
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& x,
      limb_type y,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& q,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& r) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   divide_unsigned_helper(&q, x, y, r);
   q.sign(x.sign());
   r.sign(x.sign());
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1, class U>
inline typename enable_if_c<is_integral<U>::value>::type eval_qr(
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& x,
      U y,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& q,
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& r) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   using default_ops::eval_qr;
   cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> t(y);
   eval_qr(x, t, q, r);
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1, class Integer>
inline typename enable_if_c<is_unsigned<Integer>::value && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, Integer>::type
   eval_integer_modulus(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& x, Integer val)
{
   if((sizeof(Integer) <= sizeof(limb_type)) || (val <= (std::numeric_limits<limb_type>::max)()))
   {
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> d;
      divide_unsigned_helper(static_cast<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>*>(0), x, static_cast<limb_type>(val), d);
      return d.limbs()[0];
   }
   else
   {
      return default_ops::eval_integer_modulus(x, val);
   }
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1, class Integer>
BOOST_MP_FORCEINLINE typename enable_if_c<is_signed<Integer>::value && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, Integer>::type
   eval_integer_modulus(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& x, Integer val)
{
   return eval_integer_modulus(x, boost::multiprecision::detail::unsigned_abs(val));
}

inline limb_type integer_gcd_reduce(limb_type u, limb_type v)
{
   do
   {
      if(u > v)
         std::swap(u, v);
      if(u == v)
         break;
      v -= u;
      v >>= boost::multiprecision::detail::find_lsb(v);
   } while(true);
   return u;
}

inline double_limb_type integer_gcd_reduce(double_limb_type u, double_limb_type v)
{
   do
   {
      if(u > v)
         std::swap(u, v);
      if(u == v)
         break;
      if(v <= ~static_cast<limb_type>(0))
      {
         u = integer_gcd_reduce(static_cast<limb_type>(v), static_cast<limb_type>(u));
         break;
      }
      v -= u;
      while((static_cast<unsigned>(v) & 1u) == 0)
         v >>= 1;
   } while(true);
   return u;
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_gcd(
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a,
      limb_type v)
{
   using default_ops::eval_lsb;
   using default_ops::eval_is_zero;
   using default_ops::eval_get_sign;

   int shift;

   cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> u(a);

   int s = eval_get_sign(u);

   /* GCD(0,x) := x */
   if(s < 0)
   {
      u.negate();
   }
   else if(s == 0)
   {
      result = v;
      return;
   }
   if(v == 0)
   {
      result = u;
      return;
   }

   /* Let shift := lg K, where K is the greatest power of 2
   dividing both u and v. */

   unsigned us = eval_lsb(u);
   unsigned vs = boost::multiprecision::detail::find_lsb(v);
   shift = (std::min)(us, vs);
   eval_right_shift(u, us);
   if(vs)
      v >>= vs;

   do
   {
      /* Now u and v are both odd, so diff(u, v) is even.
      Let u = min(u, v), v = diff(u, v)/2. */
      if(u.size() <= 2)
      {
         if(u.size() == 1)
            v = integer_gcd_reduce(*u.limbs(), v);
         else
         {
            double_limb_type i;
            i = u.limbs()[0] | (static_cast<double_limb_type>(u.limbs()[1]) << sizeof(limb_type) * CHAR_BIT);
            v = static_cast<limb_type>(integer_gcd_reduce(i, static_cast<double_limb_type>(v)));
         }
         break;
      }
      eval_subtract(u, v);
      us = eval_lsb(u);
      eval_right_shift(u, us);
   }
   while(true);

   result = v;
   eval_left_shift(result, shift);
}
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1, class Integer>
inline typename enable_if_c<is_unsigned<Integer>::value && (sizeof(Integer) <= sizeof(limb_type)) && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_gcd(
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a,
      const Integer& v)
{
   eval_gcd(result, a, static_cast<limb_type>(v));
}
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1, class Integer>
inline typename enable_if_c<is_signed<Integer>::value && (sizeof(Integer) <= sizeof(limb_type)) && !is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_gcd(
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a,
      const Integer& v)
{
   eval_gcd(result, a, static_cast<limb_type>(v < 0 ? -v : v));
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<!is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_gcd(
      cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a,
      const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& b)
{
   using default_ops::eval_lsb;
   using default_ops::eval_is_zero;
   using default_ops::eval_get_sign;

   if(a.size() == 1)
   {
      eval_gcd(result, b, *a.limbs());
      return;
   }
   if(b.size() == 1)
   {
      eval_gcd(result, a, *b.limbs());
      return;
   }

   int shift;

   cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> u(a), v(b);

   int s = eval_get_sign(u);

   /* GCD(0,x) := x */
   if(s < 0)
   {
      u.negate();
   }
   else if(s == 0)
   {
      result = v;
      return;
   }
   s = eval_get_sign(v);
   if(s < 0)
   {
      v.negate();
   }
   else if(s == 0)
   {
      result = u;
      return;
   }

   /* Let shift := lg K, where K is the greatest power of 2
   dividing both u and v. */

   unsigned us = eval_lsb(u);
   unsigned vs = eval_lsb(v);
   shift = (std::min)(us, vs);
   eval_right_shift(u, us);
   eval_right_shift(v, vs);

   do
   {
      /* Now u and v are both odd, so diff(u, v) is even.
      Let u = min(u, v), v = diff(u, v)/2. */
      s = u.compare(v);
      if(s > 0)
         u.swap(v);
      if(s == 0)
         break;
      if(v.size() <= 2)
      {
         if(v.size() == 1)
            u = integer_gcd_reduce(*v.limbs(), *u.limbs());
         else
         {
            double_limb_type i, j;
            i = v.limbs()[0] | (static_cast<double_limb_type>(v.limbs()[1]) << sizeof(limb_type) * CHAR_BIT);
            j = (u.size() == 1) ? *u.limbs() : u.limbs()[0] | (static_cast<double_limb_type>(u.limbs()[1]) << sizeof(limb_type) * CHAR_BIT);
            u = integer_gcd_reduce(i, j);
         }
         break;
      }
      eval_subtract(v, u);
      vs = eval_lsb(v);
      eval_right_shift(v, vs);
   }
   while(true);

   result = u;
   eval_left_shift(result, shift);
}
//
// Now again for trivial backends:
//
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
BOOST_MP_FORCEINLINE typename enable_if_c<is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value>::type
   eval_gcd(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& b) BOOST_NOEXCEPT
{
   *result.limbs() = boost::math::gcd(*a.limbs(), *b.limbs());
}
// This one is only enabled for unchecked cpp_int's, for checked int's we need the checking in the default version:
template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
BOOST_MP_FORCEINLINE typename enable_if_c<is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value && (Checked1 == unchecked)>::type
   eval_lcm(cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& b) BOOST_NOEXCEPT_IF((is_non_throwing_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value))
{
   *result.limbs() = boost::math::lcm(*a.limbs(), *b.limbs());
   result.normalize(); // result may overflow the specified number of bits
}

inline void conversion_overflow(const mpl::int_<checked>&)
{
   BOOST_THROW_EXCEPTION(std::overflow_error("Overflow in conversion to narrower type"));
}
inline void conversion_overflow(const mpl::int_<unchecked>&){}

template <class R, unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<
            is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value
            && is_signed_number<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value
         >::type
   eval_convert_to(R* result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val)
{
   typedef typename common_type<R, typename cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::local_limb_type>::type common_type;
   if(std::numeric_limits<R>::is_specialized && (static_cast<common_type>(*val.limbs()) > static_cast<common_type>((std::numeric_limits<R>::max)())))
   {
      conversion_overflow(typename cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::checked_type());
      *result = (std::numeric_limits<R>::max)();
   }
   else
      *result = static_cast<R>(*val.limbs());
   if(val.isneg())
   {
      check_is_negative(mpl::bool_<boost::is_signed<R>::value || boost::is_floating_point<R>::value>());
      *result = negate_integer(*result, mpl::bool_<boost::is_signed<R>::value || boost::is_floating_point<R>::value>());
   }
}

template <class R, unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<
            is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value
            && is_unsigned_number<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value
         >::type
   eval_convert_to(R* result, const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& val)
{
   typedef typename common_type<R, typename cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::local_limb_type>::type common_type;
   if(std::numeric_limits<R>::is_specialized && (static_cast<common_type>(*val.limbs()) > static_cast<common_type>((std::numeric_limits<R>::max)())))
   {
      conversion_overflow(typename cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>::checked_type());
      *result = (std::numeric_limits<R>::max)();
   }
   else
      *result = static_cast<R>(*val.limbs());
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, unsigned>::type
   eval_lsb(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a)
{
   using default_ops::eval_get_sign;
   if(eval_get_sign(a) == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(a.sign())
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }
   //
   // Find the index of the least significant bit within that limb:
   //
   return boost::multiprecision::detail::find_lsb(*a.limbs());
}

template <unsigned MinBits1, unsigned MaxBits1, cpp_integer_type SignType1, cpp_int_check_type Checked1, class Allocator1>
inline typename enable_if_c<is_trivial_cpp_int<cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1> >::value, unsigned>::type
   eval_msb(const cpp_int_backend<MinBits1, MaxBits1, SignType1, Checked1, Allocator1>& a)
{
   using default_ops::eval_get_sign;
   if(eval_get_sign(a) == 0)
   {
      BOOST_THROW_EXCEPTION(std::range_error("No bits were set in the operand."));
   }
   if(a.sign())
   {
      BOOST_THROW_EXCEPTION(std::range_error("Testing individual bits in negative values is not supported - results are undefined."));
   }
   //
   // Find the index of the least significant bit within that limb:
   //
   return boost::multiprecision::detail::find_msb(*a.limbs());
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

}}} // namespaces

#endif


#include <jni.h>
#include <android/log.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <Eigen/Geometry>
#include <g2o/core/hyper_graph_action.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/types/slam3d/parameter_se3_offset.h>

#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>

#include <g2o/core/hyper_dijkstra.h>

using namespace std;

#define  LOG_TAG	"G2O"
#define  LOGI(...)	__android_log_print(ANDROID_LOG_INFO, LOG_TAG, __VA_ARGS__)
#define  LOGE(...)	__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)

/**
 * Stop iterating based on the gain which is (oldChi - currentChi) / currentChi.
 *
 * If the gain is larger than zero and below the threshold, then the optimizer is stopped.
 * Typically usage of this action includes adding it as a postIteration action, by calling
 * addPostIterationAction on a sparse optimizer.
 */
class ConvergenceCheckAction : public g2o::HyperGraphAction
{
	public:
		ConvergenceCheckAction(g2o::SparseOptimizer* opt, double error_th = 1e-8)
        : m_optimizer(opt), m_gain_th(error_th), m_last_chi2(0.), m_stop_flag(false)
		{}

		void reset(g2o::SparseOptimizer* opt = NULL, double error_th = -1.)
		{
			if(opt != NULL)
				m_optimizer = opt;
			if(error_th >= 0)
				m_gain_th = error_th;
			m_last_chi2 = 0.;
			m_stop_flag = false;
		}

		virtual g2o::HyperGraphAction* operator()(const g2o::HyperGraph* graph, g2o::HyperGraphAction::Parameters* = 0)
		{
			m_optimizer->computeActiveErrors();
			const double current_chi2 = m_optimizer->activeChi2();
			const double gain = m_last_chi2/current_chi2 - 1.0;
			if((gain >= 0 && gain < m_gain_th) || current_chi2 == 0.0)
			{
				// tell the optimizer to stop
				if(m_optimizer->forceStopFlag() != NULL)
					*(m_optimizer->forceStopFlag()) = true;
				else
				{
					m_stop_flag = true;
					m_optimizer->setForceStopFlag(&m_stop_flag);
				}
			}
			m_last_chi2 = current_chi2;
			return this;
		}

    private:
		g2o::SparseOptimizer* m_optimizer;
		double m_gain_th, m_last_chi2;
		bool m_stop_flag;
};

/** col-0 = point-0; col-1 = point-1 */
typedef Eigen::Matrix<double,3,2> XYZPairMeasurement;

class EdgeSE3XYZPair : public g2o::BaseBinaryEdge<3, XYZPairMeasurement, g2o::VertexSE3, g2o::VertexSE3>
{
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

		EdgeSE3XYZPair()
		: offset0_(NULL), offset1_(NULL), cache0_(NULL), cache1_(NULL), weight_(1)
		{
			jac_buffer_.setZero();
			resizeParameters(2);
			installParameter(offset0_, 0);
			installParameter(offset1_, 1);
		}

		inline void setWeight(double w)
		{
			weight_ = w;
		}

		inline void setPointPointMetric()
		{
			plane_plane_metric_ = false;
			_information.setIdentity();
		}

		inline void setPointPlaneMetric(const Eigen::Vector3d& normal0, double e = 1e-3)
		{
			plane_plane_metric_ = false;
			Eigen::Matrix3d info;
			info << e, 0, 0,
					0, e, 0,
					0, 0, 1;
			Eigen::Matrix3d rot0 = computeRotation(normal0);
			_information = rot0.transpose()*info*rot0;
		}

		inline void setPlanePlaneMetric(const Eigen::Vector3d& normal0, const Eigen::Vector3d& normal1, double e = 1e-3)
		{
			plane_plane_metric_ = true;
			Eigen::Matrix3d info;
			info << 1, 0, 0,
					0, 1, 0,
					0, 0, e;

			Eigen::Matrix3d rot = computeRotation(normal0);
			cov0_ = rot.transpose()*info*rot;
			rot = computeRotation(normal1);
			cov1_ = rot.transpose()*info*rot;
		}

		double weight() const
		{
			return weight_;
		}

		const Eigen::Matrix3d& cov0() const
		{
			return cov0_;
		}

		const Eigen::Matrix3d& cov1() const
		{
			return cov1_;
		}

		//TODO meglio spostare poi il tutto in un altro file!

		virtual bool read(std::istream& is)
		{
			int pid0, pid1;
			is >> pid0 >> pid1;
			if(!setParameterId(0, pid0))
				return false;
			if(!setParameterId(1, pid1))
				return false;
			if(!is.good())
				return false;

			is >> _measurement(0,0) >> _measurement(1,0) >> _measurement(2,0)
		       >> _measurement(0,1) >> _measurement(1,1) >> _measurement(2,1);
			if(!is.good())
				return false;

			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					is >> _information(i,j);
			if(!is.good())
				return false;

			int plpl;
			is >> weight_ >> plpl;
			plane_plane_metric_ = (plpl == 1 ? true : false);
			if(!is.good())
				return false;

			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					is >> cov0_(i,j);
			if(!is.good())
				return false;

			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					is >> cov1_(i,j);
			return true;
		}

		virtual bool write(std::ostream& os) const
		{
			os << offset0_->id() << " " << offset1_->id() << " ";
			os << _measurement(0,0) << " " << _measurement(1,0) << " " << _measurement(2,0) << " "
		       << _measurement(0,1) << " " << _measurement(1,1) << " " << _measurement(2,1) << " ";
			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					os << _information(i,j) << " ";
			os << weight_ << " " << (plane_plane_metric_ ? 1 : 0) << " ";
			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					os << cov0_(i,j) << " ";
			for (unsigned i = 0; i < 3; ++i)
				for (unsigned j = 0; j < 3; ++j)
					os << cov1_(i,j) << " ";

			return os.good();
		}

		virtual void computeError()
		{
			if(plane_plane_metric_)
			{
				const Eigen::Isometry3d n2n = cache0_->w2n() * cache1_->n2w();
				_error = weight_ * ((n2n * _measurement.col(1)) - _measurement.col(0));
				_information = (cov0_ + n2n.rotation() * cov1_ * n2n.rotation().transpose()).inverse();
			}
			else
				_error = weight_ * ((cache0_->w2n() * (cache1_->n2w() * _measurement.col(1))) - _measurement.col(0));
		}

		virtual bool setMeasurementData(const double* d)
		{
			_measurement = Eigen::Map<const XYZPairMeasurement>(d);
			return true;
		}

		virtual bool getMeasurementData(double* d) const
		{
			Eigen::Map<XYZPairMeasurement> v(d);
			v = _measurement;
			return true;
		}

		virtual void linearizeOplus()
		{
			const g2o::VertexSE3* vp0 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
			const g2o::VertexSE3* vp1 = static_cast<const g2o::VertexSE3*>(_vertices[1]);

			if (!vp0->fixed())
			{
				const Eigen::Vector3d p1t = cache0_->w2l() *  (cache1_->n2w() * _measurement.col(1));
				jac_buffer_(1,0) = 2.0*p1t[2];
				jac_buffer_(2,0) = -2.0*p1t[1];
				jac_buffer_(0,1) = -2.0*p1t[2];
				jac_buffer_(2,1) = 2.0*p1t[0];
				jac_buffer_(0,2) = 2.0*p1t[1];
				jac_buffer_(1,2) = -2.0*p1t[0];

				_jacobianOplusXi.block<3,3>(0,0) = -1. * offset0_->inverseOffset().rotation();
				_jacobianOplusXi.block<3,3>(0,3) = offset0_->inverseOffset().rotation() * jac_buffer_;
			}

			if (!vp1->fixed())
			{
				const Eigen::Vector3d p1o = offset1_->offset() * _measurement.col(1);
				jac_buffer_(1,0) = -2.0*p1o[2];
				jac_buffer_(2,0) = 2.0*p1o[1];
				jac_buffer_(0,1) = 2.0*p1o[2];
				jac_buffer_(2,1) = -2.0*p1o[0];
				jac_buffer_(0,2) = -2.0*p1o[1];
				jac_buffer_(1,2) = 2.0*p1o[0];

				_jacobianOplusXj.block<3,3>(0,0) = cache0_->w2n().rotation() * vp1->estimate().rotation();
				_jacobianOplusXj.block<3,3>(0,3) = _jacobianOplusXj.block<3,3>(0,0) * jac_buffer_;
			}
		}

		virtual int measurementDimension() const
		{
			return 6;
		}

	private:
		inline Eigen::Matrix3d computeRotation(const Eigen::Vector3d& normal) const
		{
			Eigen::Matrix3d rot;
			rot.row(2) = normal;
			rot.row(1) = (Eigen::Vector3d::UnitY() - normal(1)*normal).normalized();
			rot.row(0) = normal.cross(rot.row(1));
			return rot;
		}

		virtual bool resolveCaches()
		{
			assert(offset0_ && offset1_);

			g2o::ParameterVector pv(2);
			pv[0] = offset0_;
			resolveCache(cache0_, (g2o::OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
			pv[1] = offset1_;
			resolveCache(cache1_, (g2o::OptimizableGraph::Vertex*)_vertices[1], "CACHE_SE3_OFFSET", pv);
			return (cache0_ && cache1_);
		}

		g2o::ParameterSE3Offset *offset0_, *offset1_;
		g2o::CacheSE3Offset  *cache0_, *cache1_;

		double weight_;
		bool plane_plane_metric_;
		Eigen::Matrix3d cov0_, cov1_;
		Eigen::Matrix3d jac_buffer_;
};

extern "C"
{
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_initNativeGraph(JNIEnv *env, jobject obj);
	JNIEXPORT jdouble JNICALL Java_it_unibo_slam_graph_GraphBackend_getChi2Native(JNIEnv *env, jobject obj);
	JNIEXPORT jdouble JNICALL Java_it_unibo_slam_graph_GraphBackend_getWeightedMeanChi2Native(JNIEnv *env, jobject obj);
	JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_getNumberOfEdgesNative(JNIEnv *env, jobject obj);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_clearNative(JNIEnv *env, jobject obj);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_setFixedViewNative(JNIEnv *env, jobject obj,
			jint viewId, jboolean fixed);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_addVertexWithEstimateNative(JNIEnv *env, jobject obj,
			jint poseId, jdoubleArray estimate);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_setVertexEstimateNative(JNIEnv *env, jobject obj,
			jint poseId, jdoubleArray estimate);
	JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_graph_GraphBackend_getVertexEstimateNative(JNIEnv *env, jobject obj,
			jint poseId);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_removeVertexNative(JNIEnv *env, jobject obj,
			jint poseId);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_addEdgeNative(JNIEnv *env, jobject obj,
			jint referenceId, jint matchingId, jdoubleArray referenceVx, jdoubleArray matchingVx, jdouble score);
	JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_optimizeNative(JNIEnv *env, jobject obj,
			jint maximumNumberOfIterations);
	JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_relativeOptimizeNative(JNIEnv *env, jobject obj,
			jintArray vSet, jint size, jint maximumNumberOfIterations);
	JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_createRBASubsetNative(JNIEnv *env, jobject obj,
			jint originId, jint ring, jobject vSet, jobject fSet);
}

// Global variables accessible only within this source file
namespace
{
	g2o::SparseOptimizer optimizer;
	ConvergenceCheckAction action = ConvergenceCheckAction(&optimizer, 1E-10);
	double edgeWeights = 0.0;
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_initNativeGraph(JNIEnv *env, jobject obj)
{
	action = ConvergenceCheckAction(&optimizer, 1E-10);
	edgeWeights = 0.0;

	typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 0> > BlockSolver_6_0;

	BlockSolver_6_0::LinearSolverType* linearSolver = new g2o::LinearSolverPCG<BlockSolver_6_0::PoseMatrixType>();
	BlockSolver_6_0* solverPointer = new BlockSolver_6_0(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(solverPointer);

	// Set the algorithm and the termination action
	optimizer.setAlgorithm(optimizationAlgorithm);
	optimizer.addPostIterationAction(&action);

	g2o::ParameterSE3Offset* offset = new g2o::ParameterSE3Offset();
	offset->setId(0);
	optimizer.addParameter(offset);
}

JNIEXPORT jdouble JNICALL Java_it_unibo_slam_graph_GraphBackend_getChi2Native(JNIEnv *env, jobject obj)
{
	return optimizer.activeChi2();
}

JNIEXPORT jdouble JNICALL Java_it_unibo_slam_graph_GraphBackend_getWeightedMeanChi2Native(JNIEnv *env, jobject obj)
{
	assert(edgeWeights > 0);
	return optimizer.activeChi2() / edgeWeights;
}

JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_getNumberOfEdgesNative(JNIEnv *env, jobject obj)
{
	return optimizer.edges().size();
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_clearNative(JNIEnv *env, jobject obj)
{
	optimizer.clear();
	edgeWeights = 0.0;
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_setFixedViewNative(JNIEnv *env, jobject obj,
		jint viewId, jboolean fixed)
{
	g2o::SparseOptimizer::Vertex* v = optimizer.vertex(viewId);
	if (v != NULL)
		v->setFixed(fixed);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_addVertexWithEstimateNative(JNIEnv *env, jobject obj,
		jint poseId, jdoubleArray estimate)
{
	double tempEstimate[16];
	env->GetDoubleArrayRegion(estimate, 0, 16, tempEstimate);

	Eigen::Isometry3d estimateEigen(Eigen::Matrix4d::Map(tempEstimate));

	g2o::VertexSE3* vx = new g2o::VertexSE3();
	vx->setId(poseId);
	vx->setEstimate(estimateEigen);
	optimizer.addVertex(vx);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_setVertexEstimateNative(JNIEnv *env, jobject obj,
		jint poseId, jdoubleArray estimate)
{
	double tempEstimate[16];
	env->GetDoubleArrayRegion(estimate, 0, 16, tempEstimate);

	Eigen::Isometry3d estimateEigen(Eigen::Matrix4d::Map(tempEstimate));

	static_cast<g2o::VertexSE3*>(optimizer.vertex(poseId))->setEstimate(estimateEigen);
}

JNIEXPORT jdoubleArray JNICALL Java_it_unibo_slam_graph_GraphBackend_getVertexEstimateNative(JNIEnv *env, jobject obj,
		jint poseId)
{
	jdoubleArray vertexEstimateArray;
	vertexEstimateArray = env->NewDoubleArray(16);
	double *tempVertexEstimateArray;

	Eigen::Isometry3d vertexEstimate = static_cast<g2o::VertexSE3*>(optimizer.vertex(poseId))->estimate();
	tempVertexEstimateArray = vertexEstimate.data();
	env->SetDoubleArrayRegion(vertexEstimateArray, 0, 16, tempVertexEstimateArray);
	return vertexEstimateArray;
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_removeVertexNative(JNIEnv *env, jobject obj,
		jint poseId)
{
	g2o::SparseOptimizer::Vertex* vx = optimizer.vertex(poseId);

	for(g2o::HyperGraph::EdgeSet::const_iterator it = vx->edges().begin(); it != vx->edges().end(); ++it)
		edgeWeights -= static_cast<const EdgeSE3XYZPair*>(*it)->weight();

	optimizer.removeVertex(vx);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_addEdgeNative(JNIEnv *env, jobject obj,
		jint referenceId, jint matchingId, jdoubleArray referenceVx, jdoubleArray matchingVx, jdouble score)
{
	double tempReferenceVx[3];
	double tempMatchingVx[3];
	env->GetDoubleArrayRegion(referenceVx, 0, 3, tempReferenceVx);
	env->GetDoubleArrayRegion(matchingVx, 0, 3, tempMatchingVx);

	Eigen::Vector3d referenceVxEigen(tempReferenceVx[0], tempReferenceVx[1], tempReferenceVx[2]);
	Eigen::Vector3d matchingVxEigen(tempMatchingVx[0], tempMatchingVx[1], tempMatchingVx[2]);

	g2o::SparseOptimizer::Vertex* referenceVertex = optimizer.vertex(referenceId);
	g2o::SparseOptimizer::Vertex* matchingVertex = optimizer.vertex(matchingId);

	EdgeSE3XYZPair* edge = new EdgeSE3XYZPair();
	edge->setParameterId(0, 0);
	edge->setParameterId(1, 0);
	edge->vertices()[0] = referenceVertex;
	edge->vertices()[1] = matchingVertex;

	XYZPairMeasurement measurement;
	measurement.col(0) = referenceVxEigen;
	measurement.col(1) = matchingVxEigen;

	edge->setMeasurement(measurement);
	if (score > 0)
		edge->setWeight(score);
	edge->setPointPointMetric();
	optimizer.addEdge(edge);

	edgeWeights += edge->weight();
}

JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_optimizeNative(JNIEnv *env, jobject obj,
		jint maximumNumberOfIterations)
{
	action.reset();
	optimizer.initializeOptimization(-1);
	return optimizer.optimize(maximumNumberOfIterations);
}

JNIEXPORT jint JNICALL Java_it_unibo_slam_graph_GraphBackend_relativeOptimizeNative(JNIEnv *env, jobject obj,
		jintArray vSet, jint size, jint maximumNumberOfIterations)
{
	int vSetArray[size];
	env->GetIntArrayRegion(vSet, 0, size, vSetArray);

	g2o::HyperGraph::VertexSet vSetG2O;
	for (int i = 0; i < size; i++)
		vSetG2O.insert(optimizer.vertex(vSetArray[i]));

	action.reset();
	optimizer.initializeOptimization(vSetG2O, -1);
	return optimizer.optimize(maximumNumberOfIterations);
}

JNIEXPORT void JNICALL Java_it_unibo_slam_graph_GraphBackend_createRBASubsetNative(JNIEnv *env, jobject obj,
		jint originId, jint ring, jobject vSet, jobject fSet)
{
	//TODO verificare se i metodi sono corretti

	jclass vSetClass = env->GetObjectClass(vSet);
	jmethodID vSetMethod = env->GetMethodID(vSetClass, "add", "(Ljava/lang/Object;)Z");
	if (vSetMethod == 0)
	{
		LOGE("VSET METHOD NOT FOUND");
		return;
	}

	jclass fSetClass = env->GetObjectClass(fSet);
	jmethodID fSetMethod = env->GetMethodID(fSetClass, "add", "(Ljava/lang/Object;)Z");
	if (fSetMethod == 0)
	{
		LOGE("FSET METHOD NOT FOUND");
		return;
	}

	g2o::HyperGraph::Vertex* origin = optimizer.vertex(originId);
	g2o::HyperGraph::VertexSet vSetG2O;
	g2o::HyperGraph::VertexSet fSetG2O;

	vSetG2O.insert(origin);

	g2o::HyperGraph::VertexSet localSets[2];
	localSets[0] = vSetG2O;
	g2o::HyperGraph::VertexSet* lastRing = &localSets[0];
	g2o::HyperGraph::VertexSet* nextRing = &localSets[1];

	for (unsigned cr = 0; cr <= ring; ++cr)
	{
		for (g2o::HyperGraph::VertexSet::const_iterator vit = lastRing->begin(); vit != lastRing->end(); ++vit)
		{
			// Collect next ring
			for (g2o::HyperGraph::EdgeSet::iterator eit = (*vit)->edges().begin(); eit != (*vit)->edges().end(); ++eit)
			{
				for (unsigned i = 0; i < (*eit)->vertices().size(); ++i)
				{
					g2o::HyperGraph::Vertex* vi = (*eit)->vertex(i);
					if (!vSetG2O.count(vi))
					{
						if (cr == ring)
						{
							vSetG2O.insert(vi);
							fSetG2O.insert(vi);
							static_cast<g2o::SparseOptimizer::Vertex*>(vi)->setFixed(true);
						}
						else
							nextRing->insert(vi);
					}
				} // For every vx attached to the periphery
			} // For every peripheral edge
		} // For every peripheral vx

		if (nextRing->empty())
			break;

		vSetG2O.insert(nextRing->begin(), nextRing->end());
		std::swap(lastRing, nextRing);
		nextRing->clear();

	} // For every ring

	jclass integerClass = env->FindClass("java/lang/Integer");
	jmethodID integerInitMethod = env->GetMethodID(integerClass, "<init>", "(I)V");

	//TODO verificare funzionamento - passaggio da int a Integer
	for (g2o::HyperGraph::VertexSet::const_iterator vit = vSetG2O.begin(); vit != vSetG2O.end(); ++vit)
		env->CallBooleanMethod(vSet, vSetMethod, env->NewObject(integerClass, integerInitMethod, (*vit)->id()));
	for (g2o::HyperGraph::VertexSet::const_iterator fit = fSetG2O.begin(); fit != fSetG2O.end(); ++fit)
		env->CallBooleanMethod(fSet, fSetMethod, env->NewObject(integerClass, integerInitMethod, (*fit)->id()));
}

extern "C"
{
	JNIEXPORT void JNICALL Java_it_unibo_slam_main_SlamDunk_initDijkstraNative(JNIEnv *env, jobject obj);
	JNIEXPORT jboolean JNICALL Java_it_unibo_slam_main_SlamDunk_loopClosureCheck(JNIEnv *env, jobject obj,
			jintArray trackedKeyframes, jint size, jint rbaRings);
	JNIEXPORT void JNICALL Java_it_unibo_slam_main_SlamDunk_updateAdjacencyMap(JNIEnv *env, jobject obj,
			jint id);
}

g2o::HyperDijkstra dijkstra = g2o::HyperDijkstra(&optimizer);

JNIEXPORT void JNICALL Java_it_unibo_slam_main_SlamDunk_initDijkstraNative(JNIEnv *env, jobject obj)
{
	dijkstra = g2o::HyperDijkstra(&optimizer);
}

JNIEXPORT jboolean JNICALL Java_it_unibo_slam_main_SlamDunk_loopClosureCheck(JNIEnv *env, jobject obj,
		jintArray trackedKeyframes, jint size, jint rbaRings)
{
	int trackedKeyframesArray[size];
	env->GetIntArrayRegion(trackedKeyframes, 0, size, trackedKeyframesArray);

	std::vector<g2o::HyperGraph::Vertex*> trackedVertices(size);
	for (unsigned k = 0; k < trackedVertices.size(); k++)
		trackedVertices[k] = optimizer.vertex(trackedKeyframesArray[k]);

	g2o::UniformCostFunction edgeCost;
	bool isKeyframe = false;
	for (unsigned k = 0; k < trackedVertices.size() && !isKeyframe; ++k)
	{
		g2o::HyperGraph::Vertex* vx = trackedVertices[k];
		dijkstra.shortestPaths(vx, &edgeCost, rbaRings);
		const g2o::HyperGraph::VertexSet& visited = dijkstra.visited();

		for (unsigned k2 = k; k2 < trackedVertices.size() && !isKeyframe; ++k2)
			isKeyframe = (visited.count(trackedVertices[k2]) == 0);
	}

	return isKeyframe;
}

JNIEXPORT void JNICALL Java_it_unibo_slam_main_SlamDunk_updateAdjacencyMap(JNIEnv *env, jobject obj,
		jint id)
{
	g2o::HyperGraph::Vertex* vx = optimizer.vertex(id);
	g2o::HyperDijkstra::AdjacencyMapEntry entry(vx, 0, 0, std::numeric_limits< double >::max());
	dijkstra.adjacencyMap().insert(std::make_pair(entry.child(), entry));
}