/*

channel.cpp

solve for flow rates in a rectangular channel

Nx/Ny = number of nodes along a side

g++ -I/usr/local/include/eigen3 ...

*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <random>
#include <vector>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class TwoVect {

public:

  double x;
  double y;

  TwoVect () {
    x=0.0; y=0.0;
  };

  TwoVect (double x0, double y0) {
    x=x0; y=y0;
  };

  void setV(double x0, double y0) {
    x=x0; y=y0;
  }

  double mag() {
    return  sqrt(x*x + y*y);
  }

 double operator*(const TwoVect & other);

 TwoVect operator+(const TwoVect & other);

 TwoVect operator-(const TwoVect & other);

 friend std::ostream &operator<<(std::ostream &out, TwoVect v0) {
  out << v0.x << " " << v0.y;
  return out;
 }

}; // end of class TwoVect

TwoVect TwoVect::operator-(const TwoVect & other) {
  return TwoVect(x-other.x,y-other.y);
}

TwoVect TwoVect::operator+(const TwoVect & other) {
  return TwoVect(x+other.x,y+other.y);
}

double TwoVect::operator*(const TwoVect & other) {
  // dot product
  return x*other.x + y*other.y;
}

TwoVect operator*(const double & lhs, const TwoVect & rhs) {
  TwoVect v0 = TwoVect(lhs*rhs.x,lhs*rhs.y);
  return v0;
}

TwoVect operator*(const TwoVect & lhs, const double & rhs) {
  TwoVect v0 = TwoVect(rhs*lhs.x,rhs*lhs.y);
  return v0;
}

TwoVect operator/(const TwoVect & lhs, const double & rhs) {
  TwoVect v0 = TwoVect(lhs.x/rhs,lhs.y/rhs);
  return v0;
}

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

typedef int node;
typedef int element;
TwoVect *rNode = NULL;               // vector of node coordinates
typedef Matrix<int,Dynamic,3> eMat;  // custom Eigen matrix type
eMat nL;                             // track nodes associated with element 0,1,2 = CCW nodes

double area(element e)  {
  double a3 = rNode[nL(e,1)].x - rNode[nL(e,0)].x;
  double a2 = rNode[nL(e,0)].x - rNode[nL(e,2)].x;
  double b3 = rNode[nL(e,0)].y - rNode[nL(e,1)].y;
  double b2 = rNode[nL(e,2)].y - rNode[nL(e,0)].y;
  return 0.5*(a3*b2 - a2*b3);
}

TwoVect cm(element e) {
  // center of mass of element e
  return (rNode[nL(e,0)] + rNode[nL(e,1)] + rNode[nL(e,2)])/3.0;
}

int main(void) {
  int verbose=0;
  int Nx,Ny;
  double hw,Pz,Vz;
  ofstream ofs,ofs2,ofs3,ofs4;
  ofs.open("channel.dat");
  ofs2.open("c2.dat");
  ofs3.open("c3.dat");
  ofs4.open("c4.dat");
  cout << " input Nx, Ny, H/W [0.5], Pz/mu [-6], Vz [0] " << endl;
  cin >> Nx >> Ny >> hw >> Pz >> Vz;
  cout << " enter 1 for verbose mode " << endl;
  cin >> verbose;
  int Nnodes = Nx*Ny;
  int Nelements = (Nx-1)*(Ny-1)*2;
  cout << " number of nodes: " << Nnodes << endl;
  cout << " number of elements: " << Nelements << endl;

  VectorXd f(Nnodes),w(Nnodes);
  MatrixXd K(Nnodes,Nnodes);
  nL = eMat(Nelements,3);

  // define rectangle nodes
  rNode = new TwoVect[Nnodes];
  node nodeNumber = 0;
  for (int iy=0;iy<Ny;iy++) {
  for (int ix=0;ix<Nx;ix++) {
     double x = (double) ix/(Nx-1);
     double y = (double) iy/(Ny-1);
     y *= hw;                        // scale to desired proportions
     rNode[nodeNumber].setV(x,y);
     nodeNumber++;
  }}

  //label elements and specify their nodes
  if (verbose) cout << endl << " element and associated nodes: " << endl << endl;
  for (int iy=0;iy<Ny-1;iy++) {
  for (int ix=0;ix<Nx-1;ix++) {
     node i = iy*(Nx-1)+ix;
     element eNumber = 2*i;
     node j = ix + Nx*iy;
     nL(eNumber,0) = j;
     nL(eNumber,1) = j+Nx+1;
     nL(eNumber,2) = j+Nx;
     if (verbose) cout << eNumber << " " << nL(eNumber,0) << " " << nL(eNumber,1) << " " <<
             nL(eNumber,2) << endl;
     eNumber++;
     nL(eNumber,0) = j;
     nL(eNumber,1) = j+1;
     nL(eNumber,2) = j+1+Nx;
     if (verbose) cout << eNumber << " " << nL(eNumber,0) << " " << nL(eNumber,1) << " " <<
             nL(eNumber,2) << endl;
  }}

  if (verbose) {
    cout << " ----------- " << endl;
    // print mesh
     for (element e=0;e<Nelements;e++) {
        ofs << rNode[nL(e,0)] << " " << rNode[nL(e,1)] << endl;
        ofs << rNode[nL(e,1)] << " " << rNode[nL(e,2)] << endl;
        ofs << rNode[nL(e,2)] << " " << rNode[nL(e,0)] << endl;
        ofs2 << e << " " << cm(e) << " " << area(e) << endl;
     }
  }

  // assemble FEM stiffness matrix, K
  for (element e=0;e<Nelements;e++) {
     double beta[3],gamma[3];
     for (int i=0;i<3;i++) {
       int j = (i+1)%3;
       int k = (i+2)%3;
       beta[i]  = rNode[nL(e,j)].y - rNode[nL(e,k)].y;
       gamma[i] = rNode[nL(e,k)].x - rNode[nL(e,j)].x;
       if (verbose) cout << e << " " << i << " " << beta[i] << " " << gamma[i] << endl;
     }
     for (int i=0;i<3;i++) {
     for (int j=0;j<3;j++) {
        node I = nL(e,i);
        node J = nL(e,j);
        K(I,J) += (beta[i]*beta[j] + gamma[i]*gamma[j])/(4.0*area(e));
     }}
  }

  if (verbose) {
    cout << " ----------- " << endl;
    cout << "K " << endl;
    cout << "det(K) = " << K.determinant() << endl;
    for (node i=0;i<Nnodes;i++) {
    for (node j=0;j<Nnodes;j++) {
      cout << i << " " << j << " " << K(i,j) << endl;
    }}
    cout << " ----------- " << endl;
  }

  // assemble force vector  [pressure gradient/viscosity  = dp/dz / mu  == Pz]
  for (element e=0;e<Nelements;e++) {
     double ff = -Pz*area(e)/3.0;
     f(nL(e,0)) += ff;  // sum force contribution to nodes
     f(nL(e,1)) += ff;
     f(nL(e,2)) += ff;
  }
  if (verbose) {
    cout << " f " << endl;
    for (node i=0;i<Nnodes;i++) cout << i << " " << f(i) << endl;
    cout << " ----------- " << endl;
  }

  // boundary conditions    [trick to force w(...) = f(...) = 0]
  // we need to identify the nodes on the boundary !

  for (node n=0;n<Nx;n++) {
     if (verbose) cout << " boundary nodes: " << n << " " << n + Nx*(Ny-1) << endl;
     K(n,n) *= 1e12;
     K(n+Nx*(Ny-1),n+Nx*(Ny-1)) *= 1e12;
     f(n) = 0.0*K(n,n);                                 // bottom
     f(n+Nx*(Ny-1)) = Vz*K(n+Nx*(Ny-1),n+Nx*(Ny-1));    // top
  }
  for (node n=1;n<Ny-1;n++) {
     if (verbose) cout << " boundary nodes: " << n*Nx << " " << n*Nx + Nx - 1 << endl;
     K(n*Nx,n*Nx) *= 1e12;
     K(n*Nx+Nx-1,n*Nx+Nx-1) *= 1e12;
     f(n*Nx) = 0.0*K(n*Nx,n*Nx);             // left
     f(n*Nx+Nx-1) = 0.0;                     // right
  }

  int num=0;
  for (node n=0;n<Nnodes;n++) {
  for (node m=0;m<Nnodes;m++) {
     if (K(n,m) != 0.0) num++;
  }}
  cout << --num << " nonzero elements in K " << endl;

  // obtain solution
  w = K.inverse()*f;

  // store solution
  int nx = 1;
  for (node n=0;n<Nnodes;n++) {
    ofs4 << rNode[n] << " " << w(n) << endl;
    nx++;
    if (nx > Nx) {
       ofs4 << "  " << endl;
       nx=1;
    }
  }

  // integrate to obtain flow rate  [= int dx dy w(x,y)]
  double in = 0.0;
  for (element e=0;e<Nelements;e++) {
     in +=area(e)*(w(nL(e,0)) + w(nL(e,1)) + w(nL(e,2)));
  }
  in /= 3.0;
  cout << " flow rate = " << in << endl;

  delete [] rNode;
  ofs.close();
  ofs2.close();
  ofs3.close();
  ofs4.close();
  cout << " mesh coords in channel.dat " << endl;
  cout << " e cm(e) area(e) in c2.dat " << endl;
  cout << " r_node w(r_node)  in c4.dat" << endl;
  return 0;
}


///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2018 - 2022.                 //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

// This Miller-Rabin primality test is loosely based on
// an adaptation of some code from Boost.Multiprecision.
// The Boost.Multiprecision code can be found here:
// https://www.boost.org/doc/libs/1_78_0/libs/multiprecision/doc/html/boost_multiprecision/tut/primetest.html

#include <ctime>
#include <random>
#include <sstream>
#include <string>

#include <boost/version.hpp>

#if !defined(BOOST_VERSION)
#error BOOST_VERSION is not defined. Ensure that <boost/version.hpp> is properly included.
#endif

#if ((BOOST_VERSION >= 107900) && !defined(BOOST_MP_STANDALONE))
#define BOOST_MP_STANDALONE
#endif

#if (BOOST_VERSION < 108000)
#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
#endif

#if (defined(__GNUC__) && !defined(__clang__) && (__GNUC__ >= 12))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wrestrict"
#endif

#if (BOOST_VERSION < 108000)
#if (defined(__clang__) && (__clang_major__ > 9)) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <boost/multiprecision/miller_rabin.hpp>
#include <boost/multiprecision/uintwide_t_backend.hpp>

#include <examples/example_uintwide_t.h>

namespace local_miller_rabin {

template<typename UnsignedIntegralType>
auto lexical_cast(const UnsignedIntegralType& u) -> std::string
{
  std::stringstream ss;

  ss << u;

  return ss.str();
}

} // namespace local_miller_rabin

#if defined(WIDE_INTEGER_NAMESPACE)
auto WIDE_INTEGER_NAMESPACE::math::wide_integer::example008a_miller_rabin_prime() -> bool
#else
auto math::wide_integer::example008a_miller_rabin_prime() -> bool
#endif
{
  #if defined(WIDE_INTEGER_NAMESPACE)
  using boost_wide_integer_type =
    boost::multiprecision::number<boost::multiprecision::uintwide_t_backend<static_cast<WIDE_INTEGER_NAMESPACE::math::wide_integer::size_t>(UINT32_C(512))>,
                                  boost::multiprecision::et_off>;
  #else
  using boost_wide_integer_type =
    boost::multiprecision::number<boost::multiprecision::uintwide_t_backend<static_cast<math::wide_integer::size_t>(UINT32_C(512))>,
                                  boost::multiprecision::et_off>;
  #endif

  // This example uses wide_integer's uniform_int_distribution to select
  // prime candidates. These prime candidates are subsequently converted
  // (via string-streaming) to Boost.Multiprecision integers.

  #if defined(WIDE_INTEGER_NAMESPACE)
  using local_wide_integer_type = WIDE_INTEGER_NAMESPACE::math::wide_integer::uintwide_t              <static_cast<WIDE_INTEGER_NAMESPACE::math::wide_integer::size_t>(std::numeric_limits<boost_wide_integer_type>::digits)>;
  using local_distribution_type = WIDE_INTEGER_NAMESPACE::math::wide_integer::uniform_int_distribution<static_cast<WIDE_INTEGER_NAMESPACE::math::wide_integer::size_t>(std::numeric_limits<boost_wide_integer_type>::digits)>;
  #else
  using local_wide_integer_type = math::wide_integer::uintwide_t              <static_cast<math::wide_integer::size_t>(std::numeric_limits<boost_wide_integer_type>::digits)>;
  using local_distribution_type = math::wide_integer::uniform_int_distribution<static_cast<math::wide_integer::size_t>(std::numeric_limits<boost_wide_integer_type>::digits)>;
  #endif

  using random_engine1_type = std::mt19937;
  using random_engine2_type = std::linear_congruential_engine<std::uint32_t, UINT32_C(48271), UINT32_C(0), UINT32_C(2147483647)>; // NOLINT(cppcoreguidelines-avoid-magic-numbers,readability-magic-numbers)

  const auto seed_start = std::clock();

  random_engine1_type gen1(static_cast<typename random_engine1_type::result_type>(seed_start));
  random_engine2_type gen2(static_cast<typename random_engine2_type::result_type>(seed_start));

  // Select prime candidates from a range of 10^150 ... max(uint512_t)-1.
  WIDE_INTEGER_CONSTEXPR local_wide_integer_type
    dist_min
    (
      "1"
      "00000000000000000000000000000000000000000000000000"
      "00000000000000000000000000000000000000000000000000"
      "00000000000000000000000000000000000000000000000000"
    );

  WIDE_INTEGER_CONSTEXPR local_wide_integer_type
    dist_max
    (
      (std::numeric_limits<local_wide_integer_type>::max)() - 1
    );

  local_distribution_type
    dist
    {
      dist_min,
      dist_max
    };

  boost_wide_integer_type p0;
  boost_wide_integer_type p1;

  auto dist_func =
    [&dist, &gen1]() // NOLINT(modernize-use-trailing-return-type)
    {
      const auto n = dist(gen1);

      return boost_wide_integer_type(local_miller_rabin::lexical_cast(n));
    };

  for(;;)
  {
    p0 = dist_func();

    const auto p0_is_probably_prime = boost::multiprecision::miller_rabin_test(p0, 25U, gen2);

    if(p0_is_probably_prime)
    {
      break;
    }
  }

  auto seed_next = std::clock();

  while(seed_next == seed_start)
  {
    seed_next = std::clock();
  }

  gen1.seed(static_cast<typename random_engine1_type::result_type>(seed_next));

  for(;;)
  {
    p1 = dist_func();

    const auto p1_is_probably_prime = boost::multiprecision::miller_rabin_test(p1, 25U, gen2);

    if(p1_is_probably_prime)
    {
      break;
    }
  }

  const boost_wide_integer_type gd = gcd(p0, p1);

  const auto result_is_ok = (   (p0  > boost_wide_integer_type(local_miller_rabin::lexical_cast(dist_min)))
                             && (p1  > boost_wide_integer_type(local_miller_rabin::lexical_cast(dist_min)))
                             && (p0 != 0U)
                             && (p1 != 0U)
                             && (p0 != p1)
                             && (gd == 1U));

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if defined(WIDE_INTEGER_STANDALONE_EXAMPLE008A_MILLER_RABIN_PRIME)

#include <iomanip>
#include <iostream>

auto main() -> int // NOLINT(bugprone-exception-escape)
{
  #if defined(WIDE_INTEGER_NAMESPACE)
  const auto result_is_ok = WIDE_INTEGER_NAMESPACE::math::wide_integer::example008a_miller_rabin_prime();
  #else
  const auto result_is_ok = math::wide_integer::example008a_miller_rabin_prime();
  #endif

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;

  return (result_is_ok ? 0 : -1);
}

#endif

#if (BOOST_VERSION < 108000)
#if (defined(__clang__) && (__clang_major__ > 9)) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif
#endif

#if (defined(__GNUC__) && !defined(__clang__) && (__GNUC__ >= 12))
#pragma GCC diagnostic pop
#endif

#if (BOOST_VERSION < 108000)
#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif
#endif


#include "geometry.h"
#include <Eigen/LU>

using namespace Eigen;
namespace marvel{
double clo_surf_vol(const MatrixXd &nods, const MatrixXi &surf){
  //TODO:check if the surface is closed and manifold
  double volume = 0;
  for(size_t i = 0; i < surf.cols(); ++i){
    Matrix3d tet;
    for(size_t j = 0; j < 3; ++j){
      tet.row(j) = nods.col(surf(j, i));
    }
    //TODO:check
    volume += tet.determinant();
  }

  return volume;

}

int build_bdbox(const MatrixXd &nods, MatrixXd & bdbox){
  //simple bounding box
  //bounding box is a dimension * 2 matrix, whose first column is minimal value and second column is maximal value.
  bdbox = nods.col(0)*MatrixXd::Ones(1, 2);
  for(size_t i = 0; i < nods.cols(); ++i){
    for(size_t j = 0; j < nods.rows(); ++j){
      if(bdbox(j, 0) > nods(j, i))
        bdbox(j, 0) = nods(j ,i);
      if(bdbox(j, 1) < nods(j, i))
        bdbox(j, 1) = nods(j ,i);
    }
  }
  return 0;
}

}//namespace : marvel


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXf A = MatrixXf::Random(4,4);
RealSchur<MatrixXf> schur(4);
schur.compute(A, /* computeU = */ false);
cout << "The matrix T in the decomposition of A is:" << endl << schur.matrixT() << endl;
schur.compute(A.inverse(), /* computeU = */ false);
cout << "The matrix T in the decomposition of A^(-1) is:" << endl << schur.matrixT() << endl;

  return 0;
}


#include <NTL/mat_poly_ZZ.h>
#include <NTL/mat_poly_ZZ_p.h>
#include <NTL/mat_poly_lzz_p.h>

#include <NTL/new.h>

NTL_START_IMPL

static
long CharPolyBound(const mat_ZZ& a)
// This bound is computed via interpolation
// through complex roots of unity.

{
   long n = a.NumRows();
   long i;
   ZZ res, t1, t2;

   set(res);

   for (i = 0; i < n; i++) {
      InnerProduct(t1, a[i], a[i]);
      abs(t2, a[i][i]);
      mul(t2, t2, 2);
      add(t2, t2, 1);
      add(t1, t1, t2);
      if (t1 > 1) {
         SqrRoot(t1, t1);
         add(t1, t1, 1);
      }
      mul(res, res, t1);
   }

   return NumBits(res);
}

void CharPoly(ZZX& gg, const mat_ZZ& a, long deterministic)
{
   long n = a.NumRows();
   if (a.NumCols() != n)
      LogicError("CharPoly: nonsquare matrix");

   if (n == 0) {
      set(gg);
      return;
   }

   if (n == 1) {
      ZZ t;
      SetX(gg);
      negate(t, a(1, 1));
      SetCoeff(gg, 0, t);
      return;
   }

   long bound = 2 + CharPolyBound(a);

   zz_pBak bak;
   bak.save();

   ZZ_pBak bak1;
   bak1.save();

   ZZX g;
   ZZ prod;

   clear(g);
   set(prod);

   long i;

   long instable = 1;

   long gp_cnt = 0;

   for (i = 0; ; i++) {
      if (NumBits(prod) > bound)
         break;

      if (!deterministic &&
          !instable && bound > 1000 && NumBits(prod) < 0.25*bound) {
         long plen = 90 + NumBits(max(bound, MaxBits(g)));

         ZZ P;

         GenPrime(P, plen, 90 + 2*NumBits(gp_cnt++));

         ZZ_p::init(P);
         mat_ZZ_p A;
         ZZ_pX G;
         conv(A, a);
         CharPoly(G, A);

         if (CRT(g, prod, G))
            instable = 1;
         else
            break;
      }

      zz_p::FFTInit(i);

      mat_zz_p A;
      zz_pX G;
      conv(A, a);
      CharPoly(G, A);
      instable = CRT(g, prod, G);
   }

   gg = g;

   bak.restore();
   bak1.restore();
}

NTL_END_IMPL


#include <boost/test/unit_test.hpp>
#include "vec.h"

BOOST_AUTO_TEST_SUITE(vec)

BOOST_AUTO_TEST_CASE(perpendiculars)
{
	for (int i = 0; i < 1000; i++)
	{
		math::vec<3> v(math::scalar(rand() * 2) / RAND_MAX - 1, math::scalar(rand() * 2) / RAND_MAX - 1,
			math::scalar(rand() * 2) / RAND_MAX - 1);

		math::vec<3> xy = v.perpendicular_xy();
		math::vec<3> yz = v.perpendicular_yz();
		math::vec<3> xz = v.perpendicular_xz();

		BOOST_CHECK (math::abs(xy & v) < math::EPSILON);
		BOOST_CHECK (math::abs(yz & v) < math::EPSILON);
		BOOST_CHECK (math::abs(xz & v) < math::EPSILON);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include "FlexKalman/EigenQuatExponentialMap.h"
#include <Eigen/Eigen>
#include <fstream>

#include "unifiedvideoinertial/CSV.h"
#include "unifiedvideoinertial/CSVCellGroup.h"

using namespace videotracker::util;

int main() {
    CSV csv;
    for (int i = 0; i < 100; ++i) {

        auto rotVec = Eigen::Vector3d{i / 2000., 0, 0};
        auto full_exp = flexkalman::util::quat_exp(rotVec);
        auto small_angle = flexkalman::util::small_angle_quat_exp(rotVec);
        csv.row() << cellGroup(rotVec) << cellGroup("smallAngle.", small_angle)
                  << cellGroup("fullExp.", full_exp);
    }
    std::ofstream os{"data.csv"};
    csv.output(os);
    return 0;
}


#include <boost/numeric/ublas/matrix.hpp>

int main()
{
    using namespace boost::numeric::ublas;

    int nSize;
    std::cout << "Enter matrix size (N): ";
    std::cin >> nSize;

    identity_matrix<int> oMatrix( nSize );

    for ( unsigned int y = 0; y < oMatrix.size2(); y++ )
    {
        for ( unsigned int x = 0; x < oMatrix.size1(); x++ )
        {
            std::cout << oMatrix(x,y) << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}


/*! \file demo_2d_legend_lines_markers.cpp
\brief Demonstration of some simple 2D plot features.
\details Uses some simple math functions to generate curves.
This demonstrates plotting some simple math functions with most of the 2-D defaults,
just changing a few details, from demo_2d_simple,
to show use of legend for both markers and lines.
The detailed output shows the plot settings for each plot.
See default_2d_plot.cpp for using \b all defaults.
See also demo_2d_plot.cpp for use of more of the very many options.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A. Bristow 2007, 2008, 2012, 2018, 2020

// 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)

#include <boost/svg_plot/svg_2d_plot.hpp>
using boost::svg::svg_2d_plot;

//#include <boost/svg_plot/show_2d_settings.hpp>
// Only needed for showing which settings in use.
#include <boost/quan/unc_init.hpp>

#include <iostream>
// using std::cout;
// using std::endl;
// using std::boolalpha;
#include <map>
using std::map;
// #include <cmath>
// using std::sqrt;
#include <string>
// using std::string;

// using namespace boost::svg;
// may be *very convenient* if using any SVG named colors,
// to avoid writing
//using boost::svg::red;
//using boost::svg::yellow;
//using boost::svg::orange;
//using boost::svg::blue;
//// and other enum options used:
//using boost::svg::square;
//// for every color used.

// Three simple functions ued to generate some X and Y data-series.

double e(double x)
{
  return x /2;
}

double f(double x)
{
  return std::sqrt(x);
}

double g(double x)
{
  return -2 + x*x;
}

double h(double x)
{
  return -1 + 2 * x;
}

int main()
{
  using namespace boost::svg; // Convenient so that all colors can be named.
  try
  {
    // Some containers for (sorted) sample data.
    std::map<double, double> data0;  //
    std::map<double, double> data1;
    std::map<double, double> data2;
    std::map<double, double> data3;
    std::map<double, double> data4;

    const double n = 3.;

    for(double i = -1; i <=n; i += 1.)
    { // Compute several data points for each function.
      data0[i] = i;
      data1[i] = f(i);
      data2[i] = g(i);
      data3[i] = h(i);
      data4[i] = e(i);
     // List if desired:
      // std::cout << i << ' '<< data1[i] << ' ' << data2[i] << ' ' << data3[i] << ' ' << data3[i] << std::endl;
    }

    svg_2d_plot my_plot;
    // Uses most defaults, but some scale settings are usually sensible as defaults are -10 to +10.

    my_plot.title("demo_2d_legend_lines_markers");
    std::cout << " my_plot.title() " << my_plot.title() << std::endl;
    my_plot
      .x_label("X-axis") // Note chaining of setting functions.
      .y_label("Y-axis")
      .y_range(-n, +n)
      .x_range(-n, +n)
      ///.x_autoscale(true) // hangs!
      //.y_autoscale(true) // hangs!
      .legend_on(true) // If you want to show legend and any lines and/or markers.
      // https://www.w3schools.com/colors/colors_names.asp
      .legend_background_color(lightyellow)
      .legend_border_color(lightgrey)
      .plot_background_color(whitesmoke)
      .legend_title("Functions")
     ;

    // You can check and show features and options.
    std::cout << "Plot title is \"" <<  my_plot.title() << "\"." << std::endl;
    std::cout << "Legend header is \"" <<  my_plot.legend_title() << "\"." << std::endl;

    // Add the three data series to the plot in turn:
    // Add any options to the data series.
    // Plot color settings.  See https://www.w3schools.com/colors/colors_names.asp

    my_plot.plot(data1, "x").stroke_color(black).fill_color(white); // Some are NaN because x < 0.

   // my_plot.plot(data1, "sqrt(x)"); // But nicer to use Unicode symbol √ for square root.
    my_plot.plot(data1, "√x").stroke_color(red).fill_color(yellow); // Some are NaN because x < 0.

    ////my_plot.plot(data2, "-2 + x^2").fill_color(orange).size(5); // but nicer to use Unicode symbol for superscript 2.
    my_plot.plot(data2, "-2 + x²").stroke_color(magenta).fill_color(cyan).size(5);

    my_plot.plot(data3, "-1 + 2x").stroke_color(red).fill_color(green).line_on(true).line_color(blue).shape(square);

    my_plot.plot(data4, "x / 2").bezier_on(true).line_on(true).line_color(green).shape(none);

    my_plot.write("./demo_2d_legend_lines_markers.svg");

    //show_2d_plot_settings(my_plot); // For diagnostic purposes only.
  }
  catch(const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n  " << e.what() << std::endl;
  }
  return 0;
} // int main()

  /*
  my_plot.title() demo_2d_legend_lines_markers
  Plot title is demo_2d_legend_lines_markers

  SVG 2-D plot settings
  axes_on true
  background_border_width 2
  background_border_color RGB(255,255,0)
  background_color RGB(255,255,255)
  image_border_margin() 3
  image_border_width() 2
  coord_precision 3
  copyright_date
  copyright_holder
  description
  document_title ""
  x_size 500
  image y_size 400
  image_filename
  legend_on true
  legend_place 2
  legend_top_left 367.3, 47.5, legend_bottom_right 497.5, 145.5
  legend_background_color RGB(255,255,255)
  legend_border_color RGB(255,255,0)
  legend_color blank
  legend_title ""
  legend_title_font_size 14
  legend_font_weight
  legend_width 130.2
  legend_lines true
  limit points stroke color RGB(119,136,153)
  limit points fill color RGB(250,235,215)
  license_on false
  license_reproduction permits
  license_distribution permits
  license_attribution requires
  license_commercialuse permits
  plot_background_color RGB(255,255,255)
  plot_border_color RGB(119,136,153)
  plot_border_width 2
  plot_window_on true
  plot_window_x 60.7, 353.3
  plot_window_x_left 60.7
  plot_window_x_right 353.3
  plot_window_y 47.5, 346.5
  plot_window_y_top 47.5
  plot_window_y_bottom 346.5
  title_on true
  title ""
  title_color blank
  title_font_alignment 2
  title_font_decoration
  title_font_family Lucida Sans Unicode
  title_font_rotation 0
  title_font_size 18
  title_font_stretch
  title_font_style
  title_font_weight
  x_values_on false
  x_values_font_size 12
  x_values_font_family Lucida Sans Unicode
  x_values_precision 3
  x_values_ioflags 200 iosFormatFlags (0x200) dec.
  y_values_precision 3
  y_values_font_size() 3
  y_values_ioflags 200 iosFormatFlags (0x200) dec.
  y_values_color blank
  y_values_font_family() Lucida Sans Unicode
  y_values_font_size() 12
  x_max 10
  x_min -10
  x_autoscale false
  y_autoscale false
  xy_autoscale false
  x_autoscale_check_limits true
  x_axis_on true
  x_axis_color() RGB(0,0,0)
  x_axis_label_color blank
  x_values_color blank
  x_axis_width 1
  x_label_on true
  x_label "X-axis"
  x_label_color blank
  x_label_font_family Lucida Sans Unicode
  x_label_font_size 14
  x_label_units
  x_label_units_on false
  x_major_labels_side left
  x_major_label_rotation 0
  x_major_grid_color RGB(200,220,255)
  x_major_grid_on false
  x_major_grid_width 1
  x_major_interval 2
  x_major_tick 2
  x_major_tick_color RGB(0,0,0)
  x_major_tick_length 5
  x_major_tick_width 2
  x_minor_interval 0
  x_minor_tick_color RGB(0,0,0)
  x_minor_tick_length 2
  x_minor_tick_width 1
  x_minor_grid_on false
  x_minor_grid_color RGB(200,220,255)
  x_minor_grid_width 0.5
  x_range() -10, 10
  x_num_minor_ticks 4
  x_ticks_down_on true
  x_ticks_up_on false
  x_ticks_on_window_or_axis bottom
  y_axis_position y_axis_position intersects X-axis (X range includes zero)
  x_axis_position x_axis_position intersects Y axis (Y range includes zero)
  x_plusminus_on false
  x_plusminus_color blank
  x_df_on false
  x_df_color RGB(0,0,0)
  x_prefix
  x_separator
  x_suffix
  xy_values_on false
  y_label_on "true"
  y_label_axis Y-axis
  y_axis_color RGB(0,0,0)
  y_axis_label_color blank
  y_axis_on true
  axes_on true
  y_axis_value_color RGB(0,0,0)
  y_axis_width 1
  y_label Y-axis
  y_label_color blank
  y_label_font_family Lucida Sans Unicode
  y_label_font_size 14
  y_label_on true
  y_label_units
  y_label_units_on false
  y_label_width 0
  y_major_grid_on false
  y_major_grid_color RGB(200,220,255)
  y_major_grid_width 1
  y_major_interval 2
  y_major_labels_side bottom
  y_major_label_rotation 0
  y_major_tick_color RGB(0,0,0)
  y_major_tick_length  5
  y_major_tick_width  2
  y_minor_grid_on false
  y_minor_grid_color  RGB(200,220,255)
  y_minor_grid_width 0.5
  y_minor_interval 0
  y_minor_tick_color RGB(0,0,0)
  y_minor_tick_length 2
  y_minor_tick_width 1
  y_range() -10, 10
  y_num_minor_ticks
  y_ticks_left_on true
  y_ticks_right_on false
  y_ticks_on_window_or_axis left
  y_max 10
  y_min -10
  y_values_on false
  y_plusminus_on false
  y_plusminus_color blank
  x_addlimits_on false
  x_addlimits_color RGB(0,0,0)
  y_df_on false
  y_df_color RGB(0,0,0)
  y_prefix ""
  y_separator ""
  y_suffix ""
  confidence alpha 0.05
  data lines width 2
  Press any key to continue . . .
  Output:

  */


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

int main () {
    using namespace boost::numeric::ublas;
    unsigned i;

    vector<double> v (6);

    // populando vetor
    for (i = 0; i < v.size (); ++ i)
        v (i) = i;

    std::cout << "v=\n" << v << std::endl;

    // multiplicacao por escalar
    vector<double> w = 3.0 * v;
    std::cout << "w=\n" << w << std::endl;

    // soma de vetores
    vector<double> x = v + w;
    std::cout << "x=\n" << x << std::endl;

}


#include "cellogram/point_source_detection.h"

#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <Eigen/Dense>
#include <igl/list_to_matrix.h>
#include <igl/Timer.h>

bool load_img(const std::string & path, Eigen::MatrixXd &res)
{
	std::fstream file;
	file.open(path.c_str());

	if (!file.good())
	{
		std::cerr << "Failed to open file : " << path << std::endl;
		file.close();
		return false;
	}

	std::string s;
	std::vector<std::vector<double>> matrix;

	while (getline(file, s))
	{
		std::stringstream input(s);
		double temp;
		matrix.emplace_back();

		std::vector<double> &currentLine = matrix.back();

		while (input >> temp)
			currentLine.push_back(temp);
	}

	if (!igl::list_to_matrix(matrix, res))
	{
		std::cerr << "list to matrix error" << std::endl;
		file.close();
		return false;
	}
	return true;
}

int main(int argc, char** argv) {
	Eigen::MatrixXd img;
	const double sigma = 2;
	Eigen::MatrixXd V;
	Eigen::MatrixXd V_std;
	Eigen::VectorXd pval_Ar;

	const std::string root = DATA_DIR;

	load_img(root + "1-1.txt", img);

	double min = img.minCoeff();
	double max = img.maxCoeff();

	Eigen::MatrixXd imgNorm;

	imgNorm = (img.array() - min) / (max - min);

	cellogram::DetectionParams params;

	cellogram::point_source_detection(imgNorm, sigma, 0.5, V, params);

	std::cout << V << std::endl;
	std::cout << params.std_x << std::endl;

	return 0;
}


/* Tags: Delaunay Triangulation, Union-Find, Connected Components, Closest Neighbor

  Key idea: Close relation to Golden Eye problem:
            1:1 correspondence between points being connected (connected components in EMST)
                <==>
                we can move between the disks of the points
                (for a radius >= 2 * minimal distance betw. them)
            Can copy code from the EMST template, and adapt:
            * two UF structures, one for given initial radius with s,
              one for radius needed to get k
            * for first problem (max bones):
              for each component of tree, store the number of bones reachable
              (i.e. 4 *distance to closest point <= s)
              This can be done via a simple array, as UF comp. have index <= n
              Upon merging, add together the numbers. At the end (when all edges <= s considerd)
              look for max
            * for second problem (min. radius b to get k):
              Have additional edges from each bone to closest point with edge weight 4 * dist.
              Sort as usual, then do same procedure as above (i.e. UF) until one comp has >= k bones
              Then the latest edge considered has minimal radius needed

              Why 4 * dist? ->
              for correct b, need to consider radius over all edges, but for edges between trees
              we have that dist = radius / 2. When we sort, we would thus mix up scale
*/

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <boost/pending/disjoint_sets.hpp>
#include <vector>
#include <tuple>
#include <algorithm>
#include <iostream>

// Epic kernel is enough, no constructions needed, provided the squared distance
// fits into a double (!)
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
// we want to store an index with each vertex
typedef std::size_t                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  Delaunay;

// As edges are not explicitly represented in the triangulation, we extract them
// from the triangulation to be able to sort and process them. We store the
// indices of the two endpoints, first the smaller, second the larger, and third
// the squared length of the edge. The i-th entry, for i=0,... of a tuple t can
// be accessed using std::get<i>(t).
typedef std::tuple<Index,Index,double> Edge;
typedef std::vector<Edge> EdgeV;

std::ostream& operator<<(std::ostream& o, const Edge& e) {
  return o << std::get<0>(e) << " " << std::get<1>(e) << " " << std::get<2>(e);
}

void testcase() {
  Index n, m, k;
  long s;
  std::cin >> n >> m >> s >> k;

  // read points: first, we read all points and store them into a vector,
  // together with their indices
  typedef std::pair<K::Point_2,Index> IPoint;
  std::vector<IPoint> oak_trees;
  oak_trees.reserve(n);
  for (Index i = 0; i < n; ++i) {
    int x, y;
    std::cin >> x >> y;
    oak_trees.emplace_back(K::Point_2(x, y), i);
  }
  Delaunay t;
  t.insert(oak_trees.begin(), oak_trees.end());

  // store the number of bones that are in reach for each oak tree
  std::vector<Index> bones_per_comp(n, 0);
  std::vector<IPoint> points;
  points.reserve(m);
  EdgeV edges_ext;
  edges_ext.reserve(m); // there can be no more in a planar graph

  for (Index i = 0; i < m; ++i) {
    int x, y;
    std::cin >> x >> y;
    auto p = K::Point_2(x, y);
    auto v = t.nearest_vertex(p);
    double dist = CGAL::squared_distance(p, v->point());
    if(4 * K::FT(dist) <= s) {
      bones_per_comp[v->info()] += 1;
    }
    points.emplace_back(p, i);
    edges_ext.emplace_back(v->info(), n + i, 4 * dist);
  }

  // extract edges and sort by (squared) length
  // This step takes O(n log n) time (for the sorting).
  EdgeV edges;
  edges.reserve(3*n + m); // there can be no more in a planar graph

  for (auto e = t.finite_edges_begin(); e != t.finite_edges_end(); ++e) {
    Index i1 = e->first->vertex((e->second+1)%3)->info();
    Index i2 = e->first->vertex((e->second+2)%3)->info();
    // ensure smaller index comes first
    if (i1 > i2) std::swap(i1, i2);
    edges.emplace_back(i1, i2, t.segment(e).squared_length());
  }
  std::sort(edges.begin(), edges.end(),
        [](const Edge& e1, const Edge& e2) -> bool {
          return std::get<2>(e1) < std::get<2>(e2);
            });

  boost::disjoint_sets_with_storage<> uf(n);
  Index n_components = n;
  // ... and process edges in order of increasing length
  for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
    // determine components of endpoints
    Index c1 = uf.find_set(std::get<0>(*e));
    Index c2 = uf.find_set(std::get<1>(*e));
    double squared_dist = std::get<2>(*e);
    if(squared_dist <= s) {
        if (c1 != c2) {
            // this edge connects two different components => merge number of bones
            auto b1 = bones_per_comp[c1];
            auto b2 = bones_per_comp[c2];
            bones_per_comp[c1] = 0;
            bones_per_comp[c2] = 0;
            uf.link(c1, c2);
            Index c3 = uf.find_set(std::get<0>(*e));
            bones_per_comp[c3] = b1 + b2;
            if (--n_components == 1) break;
        }
    } else break;
  }

  std::cout << *std::max_element(bones_per_comp.begin(), bones_per_comp.end()) << " ";

  // find minimal readius for k bones
  // by considering edges to bones as well
  // -- for code simplicity, redo whole union-find procedure
  std::vector<Index> bones(n + m, 0);
  for (Index i = 0; i < m; ++i) {
    edges.push_back(edges_ext[i]);
    bones[n + i] = 1;
  }
  std::sort(edges.begin(), edges.end(),
        [](const Edge& e1, const Edge& e2) -> bool {
          return std::get<2>(e1) < std::get<2>(e2);
            });

  boost::disjoint_sets_with_storage<> uf_ext(n + m);
  K::FT b_squared = 0;
  // ... and process edges in order of increasing length
  for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
    // determine components of endpoints
    Index i1 = std::get<0>(*e);
    Index i2 = std::get<1>(*e);
    Index c1 = uf_ext.find_set(i1);
    Index c2 = uf_ext.find_set(i2);
    double squared_dist = std::get<2>(*e);
    if (c1 != c2) {
        // this edge connects two different components => merge number of bones
        auto b1 = bones[c1];
        auto b2 = bones[c2];
        bones[c1] = 0;
        bones[c2] = 0;
        uf_ext.link(c1, c2);
        Index c3 = uf_ext.find_set(std::get<0>(*e));
        bones[c3] = b1 + b2;
        if (b1 + b2 >= k) {
          b_squared = squared_dist;
          break;
        }
    }
  }
  std::cout << b_squared << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(0);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


#include <iostream>
#include <math.h>

#include <boost/format.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <LEDA/core/string.h>
#include <LEDA/system/misc.h>
//#include <LEDA/graph/max_flow.h>

using namespace boost;
using namespace std;

//Type & Struct Definitions ___________________

// Bundled properties for graph
struct bundleVertex {
    int id;
    int excess;
    int distance;
};

struct bundleEdge {
    int residual;
};

struct bundleGraph;

// Define graph type
typedef adjacency_list<vecS, vecS, bidirectionalS, bundleVertex, bundleEdge, bundleGraph> Graph;

//Define graph traits type
typedef graph_traits<Graph> Traits;

//Define descriptor for vertices
typedef Traits::vertex_descriptor Vertex;
//Define descriptor for edges
typedef Traits::edge_descriptor Edge;

struct bundleGraph {
    Vertex source;
    Vertex sink;
};

// Function Declarations ___________________

// Create example graph used for visualising
Graph createExampleGraph();
// Print results to terminal
void printResults(Graph &G);
// Create graph visualisation dot language file
void visualise(Graph &G);

//Excess Scale functions -------
void ExcessScaleMaxFlow(Graph &G, Vertex source, Vertex sink);

// Handle preprocessing operations
void preprocess(Graph &G, Vertex source, Vertex sink);
// Calculate initial delta = 2 ^ log(maxCapacity)
float calcInitialDelta(Graph &G);
// Utility function for selecting node with smallest distance among nodes with large excess
Vertex selectNode(Graph &G, Vertex source, Vertex sink, float delta);
// implements push of flow or relabel of distance of current node
void pushRelabel(Graph &G, Vertex current, float delta);

// Custom Property Writer for Graphviz
template <class IdMap, class ExcessMap, class DistanceMap>
class vertex_writer {
public:
    vertex_writer(IdMap i, ExcessMap e, DistanceMap d) : im(i), em(e), dm(d) {}
    template <class Vertex>
    void operator()(ostream &out, const Vertex& v) const {
        out << "[label=\"ID:" << im[v] << "\nExcess: "<< em[v] <<"\nDistance: "<< dm[v] << "\"]";
    }
private:
    IdMap im;
    ExcessMap em;
    DistanceMap dm;
};
// Function for passing in the maps to custom property writer
template <class IdMap, class ExcessMap, class DistanceMap>
inline vertex_writer<IdMap, ExcessMap, DistanceMap>
make_vertex_writer(IdMap i, ExcessMap e, DistanceMap d) {
    return vertex_writer<IdMap, ExcessMap, DistanceMap>(i, e, d);
}

int main() {

    Graph exampleG = createExampleGraph();

    Vertex source = exampleG[graph_bundle].source;
    Vertex sink = exampleG[graph_bundle].sink;

    /*Algorithm exectution and timing*/

    cout<<"\t\tEXCESS SCALE MAX FLOW ALGORITHM"<<endl;
    cout << "Case:\tExample Graph"<<endl;
    float time = leda::used_time();
    ExcessScaleMaxFlow(exampleG, source, sink);
    cout << "\nImplementation Time: " << leda::used_time(time) << endl;
    //MAX_FLOW_T(const graph& G, node s, node t, const edge_array< NT> & cap)
    //cout << "BGL Time: " << leda::used_time(time) << endl<<endl;

    visualise(exampleG);  // dot -Tpng ./cmake-build-debug/graph.dot > exampleFinal.png

    return 0;
}

// Function Definitions ___________________
Graph createExampleGraph() {

    // Graph instantiation
    Graph G;

    // Graph creation
    Vertex v1 = add_vertex(bundleVertex{1, 0, 2}, G);
    Vertex v2 = add_vertex(bundleVertex{2, 0, 1}, G);
    Vertex v3 = add_vertex(bundleVertex{3, 0, 1}, G);
    Vertex v4 = add_vertex(bundleVertex{4, 0, 0}, G);

    add_edge(v1, v2, bundleEdge{2}, G);
    add_edge(v1, v3, bundleEdge{4}, G);
    add_edge(v2, v3, bundleEdge{3}, G);
    add_edge(v2, v4, bundleEdge{1}, G);
    add_edge(v3, v4, bundleEdge{5}, G);

    G[graph_bundle].source = v1;
    G[graph_bundle].sink = v4;

    return G;
}

void printResults(Graph &G) {

    print_graph(G, get(&bundleVertex::id, G));

    Graph::vertex_iterator vertexIt, vertexEnd;
    tie(vertexIt, vertexEnd) = vertices(G);
    cout << endl << endl;
    for (; vertexIt != vertexEnd; ++vertexIt) {
        cout << "excess of node "<< G[*vertexIt].id << " : " << G[*vertexIt].excess << endl;
    }

    //TODO: MAX FLOW IS EXCESS VALUE AT SINK...handle that
}

void visualise(Graph &G) {
    ofstream dot("graph.dot");
    write_graphviz(dot, G, make_vertex_writer(get(&bundleVertex::id, G),
                                              get(&bundleVertex::excess, G),
                                              get(&bundleVertex::distance, G)),
                           make_label_writer(get(&bundleEdge::residual, G)));
}

void ExcessScaleMaxFlow(Graph &G, Vertex source, Vertex sink) {

    // Preprocessing
    preprocess(G, source, sink);

    // Calculate initial delta = 2 ^ log(maxCapacity)
    float delta = calcInitialDelta(G);

    // Core update loop
    while(delta >= 1) {

        // select node with smallest distance among nodes with large excess
        Vertex currentNode = selectNode(G, source, sink, delta);
        while(currentNode != 0) {
            // push flow or relabel distance of current node
            pushRelabel(G, currentNode, delta);
            // select another node from updated graph
            currentNode = selectNode(G, source, sink, delta);
        }
        delta /= 2;  // update delta if no other selectable node
    }

}

void preprocess(Graph &G, Vertex source, Vertex sink) {

    // Store initial distances in this vector
    std::vector<int> distances(num_vertices(G));

    // Computation of exact distance labels via backward BFS from sink
    reverse_graph<Graph> R = make_reverse_graph(G);
    breadth_first_search(R,
                         sink,
                         visitor(make_bfs_visitor(record_distances(make_iterator_property_map(distances.begin(),
                                                                                get(vertex_index, G)),
                                                                   on_tree_edge()))));
    Graph::vertex_iterator vertexIt, vertexEnd;
    tie(vertexIt, vertexEnd) = vertices(G);
    for (; vertexIt != vertexEnd; ++vertexIt) {
        G[*vertexIt].distance = distances[*vertexIt];
    }

    // Saturate source's adjacent edges
    Graph::out_edge_iterator outedgeIt, outedgeEnd;
    tie(outedgeIt, outedgeEnd) = out_edges(source, G);
    for(; outedgeIt != outedgeEnd; ++outedgeIt) {
        Vertex src = boost::source(*outedgeIt, G);
        Vertex target = boost::target(*outedgeIt, G);
        int saturation = G[*outedgeIt].residual;

        add_edge(target, src, bundleEdge{saturation}, G);  // add reverse of saturated edge
        remove_edge(*outedgeIt, G); // remove saturated edge

        G[target].excess += saturation;  // update target node's excess property
    }

    // Update source node's distance label to num_vertices
    G[source].distance = (int) num_vertices(G);
}

float calcInitialDelta(Graph &G) {

    std::vector<int> capacities(num_edges(G));

    Graph::edge_iterator edgeIt, edgeEnd;
    tie(edgeIt, edgeEnd) = edges(G);
    for (; edgeIt!= edgeEnd; ++edgeIt) {
        capacities.push_back(G[*edgeIt].residual);
    }
    int maxCap = *max_element(capacities.begin(), capacities.end());
    return (float) pow(2, ceil(log2(maxCap)));
}

Vertex selectNode(Graph &G, Vertex source, Vertex sink, float delta) {

    double min = std::numeric_limits<double>::infinity();
    Vertex minNode = 0;

    Graph::vertex_iterator vertexIt, vertexEnd;
    tie(vertexIt, vertexEnd) = vertices(G);
    for (; vertexIt != vertexEnd; ++vertexIt) {

        // if node has large excess (cannot be source or sink)
        if(G[*vertexIt].excess >= delta/2 && *vertexIt != source && *vertexIt != sink) {
            if(G[*vertexIt].excess < min) {
                min = G[*vertexIt].excess;
                minNode = *vertexIt;
            }
        }
    }
    //return node with min excess amongst nodes with large excess
    return minNode;
}

void pushRelabel(Graph &G, Vertex current, float delta) {

    // stores candidate distances of adj nodes for raising current node's distance label to
    std::vector<int> adjDist;

    Graph::out_edge_iterator outedgeIt, outedgeEnd;
    tie(outedgeIt, outedgeEnd) = out_edges(current, G);
    for(; outedgeIt != outedgeEnd; ++outedgeIt) {
        Vertex target = boost::target(*outedgeIt, G);

        // if network contains an admissable arc
        if(G[current].distance == G[target].distance + 1) {
            int pushFlow = 0;
            if(delta - G[target].excess > 0) {
                pushFlow = std::min({G[current].excess, G[*outedgeIt].residual, (int) delta - G[target].excess});  // last arg ensures that excess doesnt exceed delta
            } else {
                pushFlow = std::min(G[current].excess, G[*outedgeIt].residual);
            }

            // implement Push
            G[current].excess -= pushFlow;
            G[target].excess += pushFlow;
            G[*outedgeIt].residual -= pushFlow;
            if(G[*outedgeIt].residual == 0)  // saturation
                remove_edge(*outedgeIt, G); // remove saturated edge
            bool found;
            Edge reverse;
            boost::tie(reverse, found) = edge(target, current, G);
            if(found) {
                G[reverse].residual += pushFlow;
            } else {
                add_edge(target, current, bundleEdge{pushFlow}, G);  // add reverse edge
            }

            return;
        } else {
            if(G[*outedgeIt].residual > 0)
                adjDist.push_back(G[target].distance + 1);
        }
    }

    // implement Relabel
    int minDist = *min_element(adjDist.begin(), adjDist.end());
    G[current].distance = minDist;

    return;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

typedef std::complex<double>      cdouble;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    //using mtl::conj;
    const unsigned                    xd= 2, yd= 5, n= xd * yd;
    A.change_dim(n, n);
    laplacian_setup(A, xd, yd);

    A*= cdouble(1, -1);
    std::cout << name << "\nconj(A) is\n" << with_format(mtl::conj(A), 7, 1) << "\n";

    mtl::dense_vector<cdouble> x(n),Ax(n);
    x=cdouble(1,2);

    // Ax= mtl::mat::conj(A) * x;
    Ax= mtl::conj(A) * x;
    std::cout << "conj(A) * x is " << Ax << "\n";

    Ax=trans(A) * x;
    std::cout << "trans(A) * x is " << Ax << "\n";

    Ax=hermitian(A) * x;
    std::cout << "hermitian(A) * x is " << Ax << "\n";
}

int main(int, char**)
{
    mtl::compressed2D<cdouble>             crc;

    test(crc, "Compressed row major complex");

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010-2011 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "lapack_common.h"
#include <Eigen/Cholesky>

// POTRF computes the Cholesky factorization of a real symmetric positive definite matrix A.
EIGEN_LAPACK_FUNC(potrf,(char* uplo, int *n, RealScalar *pa, int *lda, int *info))
{
  *info = 0;
        if(UPLO(*uplo)==INVALID) *info = -1;
  else  if(*n<0)                 *info = -2;
  else  if(*lda<std::max(1,*n))  *info = -4;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"POTRF", &e, 6);
  }

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  MatrixType A(a,*n,*n,*lda);
  int ret;
  if(UPLO(*uplo)==UP) ret = internal::llt_inplace<Scalar, Upper>::blocked(A);
  else                ret = internal::llt_inplace<Scalar, Lower>::blocked(A);

  if(ret>=0)
    *info = ret+1;

  return 0;
}

// POTRS solves a system of linear equations A*X = B with a symmetric
// positive definite matrix A using the Cholesky factorization
// A = U**T*U or A = L*L**T computed by DPOTRF.
EIGEN_LAPACK_FUNC(potrs,(char* uplo, int *n, int *nrhs, RealScalar *pa, int *lda, RealScalar *pb, int *ldb, int *info))
{
  *info = 0;
        if(UPLO(*uplo)==INVALID) *info = -1;
  else  if(*n<0)                 *info = -2;
  else  if(*nrhs<0)              *info = -3;
  else  if(*lda<std::max(1,*n))  *info = -5;
  else  if(*ldb<std::max(1,*n))  *info = -7;
  if(*info!=0)
  {
    int e = -*info;
    return xerbla_(SCALAR_SUFFIX_UP"POTRS", &e, 6);
  }

  Scalar* a = reinterpret_cast<Scalar*>(pa);
  Scalar* b = reinterpret_cast<Scalar*>(pb);
  MatrixType A(a,*n,*n,*lda);
  MatrixType B(b,*n,*nrhs,*ldb);

  if(UPLO(*uplo)==UP)
  {
    A.triangularView<Upper>().adjoint().solveInPlace(B);
    A.triangularView<Upper>().solveInPlace(B);
  }
  else
  {
    A.triangularView<Lower>().solveInPlace(B);
    A.triangularView<Lower>().adjoint().solveInPlace(B);
  }

  return 0;
}


/* =========== */
/*  Libraries  */
/* =========== */
/* System Libraries */
#include <iostream>
#include <chrono>

/* OpenCV Libraries */
#include <opencv2/opencv.hpp>
// #include <opencv2/core/core.hpp>
// #include <opencv2/features2d/features2d.hpp>
// #include <opencv2/highgui/highgui.hpp>
// #include <opencv2/calib3d/calib3d.hpp>

/* Sophus Libraries */
#include <sophus/se3.hpp>

/* Boost Libraries */
#include <boost/format.hpp>

/* Pangolin Libraries */
#include <pangolin/pangolin.h>

/* Custom Libraries */
#include "../../../common/libUtils_basic.h"
#include "../../../common/libUtils_eigen.h"
#include "../../../common/libUtils_opencv.h"

using namespace std;
using namespace cv;

/* Global Variables */
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

string left_file = "../../images/left.png"; // FIXME: I think this image is the 000000.png
string disparity_file = "../../images/disparity.png";
boost::format fmt_others("../../images/%06d.png"); // Other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// Camera Intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
double b = 0.573; // Baseline (Stereo Camera)

/* =================== */
/*  Class Declaration  */
/* =================== */

/**
 * @brief Class for accumulate Jacobians in parallel
 *
 */
class JacobianAccumulator {
public:
    // Constructor
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_, //FIXME: Use &?
        Sophus::SE3d &T21_) : img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_)
    {
        p2_proj = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /* Accumulate jacobians in a range */
    void accumulate_jacobian(const cv::Range &range);

    /* Get Hessian Matrix */
    Matrix6d getHessian() const { return H; }

    /* Get Bias Vector */
    Vector6d getBias() const { return b; }

    /* Get Total Cost */
    double getTotalCost() const { return cost; }

    /* Get Projected Points */
    VecVector2d getProjectedPoints() const { return p2_proj; }

    /* Reset H, b, and cost */
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0.0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref; //FIXME: Use &?
    Sophus::SE3d &T21;

    VecVector2d p2_proj;            // Projected Points
    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0.0;
};

/* ===================== */
/*  Function Prototypes  */
/* ===================== */
/**
 * @brief Pose estimation using Direct Method (Multi-layer)
 *
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref, //FIXME: Use &?
    Sophus::SE3d &T21);

/**
 * @brief Pose estimation using Direct Method (Single-layer)
 *
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref, //FIXME: Use &?
    Sophus::SE3d &T21);

/** Bilinear Interpolation
 * @brief Get a grayscale value from reference image (bilinear interpolation)
 *
 * @param img input image
 * @param x x-coordinate of the center pixel
 * @param y y-coordinate of the center pixel
 * @return the interpolated value of this pixel
 */
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    /* Boundary check */
    if (x < 0)
        x = 0; // Avoid negative x-axis coordinates
    if (y < 0)
        y = 0; // Avoid negative y-axis coordinates
    if (x >= img.cols)
        x = img.cols - 1; // Avoid positive x-axis coordinates outside image width
    if (y >= img.rows)
        y = img.rows - 1; // Avoid positive y-axis coordinates outside image height

    uchar *data = &img.data[int(y) * img.step + int(x)];

    float xx = x - floor(x);
    float yy = y - floor(y);

    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]);
}

/* ====== */
/*  Main  */
/* ====== */
/* This program demonstrates how to implement a sparse direct method of binocular cameras (i.e, vSLAM with Known Depth!). */
int main(int argc, char **argv) {
    cout << "[direct_method] Hello!" << endl << endl;

    /* Load the images */
    cv::Mat left_img = cv::imread(left_file, CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat disparity_img = cv::imread(disparity_file, CV_LOAD_IMAGE_GRAYSCALE);

    /* Initialization */
    // Let's randomly pick pixels in the first image and generate some 3D points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // Generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        // Don't pick pixels close to the boarder
        int x = rng.uniform(boarder, left_img.cols - boarder);
        int y = rng.uniform(boarder, left_img.rows - boarder);

        // Use the camera intrinsics parameters to get the depth from the disparity image.
        // Note: Remember that in OpenCV, the coordinates axes for retriving the pixel value are inverted.
        int disparity = disparity_img.at<uchar>(y, x); // Disp(x, y)
        double depth = (fx * b) / disparity;           // D(x, y)

        depth_ref.push_back(depth);           // {D(x, y)_i}
        pixels_ref.push_back(Vector2d(x, y)); // {[x, y]_i}
    }

    // Estimates 01~05.png's pose using this information.
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) { // [1, 5]
        cv::Mat img = cv::imread((fmt_others % i).str(), CV_LOAD_IMAGE_GRAYSCALE);

        // Try single layer by uncommenting the following line
        DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        // DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);  //TODO
    }

    /* --------- */
    /*  Results  */
    /* --------  */
    /* Display Images */
    // imshow("image1", image1);
    // imshow("image2", image2);
    imshow("left", left_img);
    imshow("disparity", disparity_img);
    cout << "\nPress 'ESC' to exit the program..." << endl;
    waitKey(0);

    cout << "Done." << endl;

    return 0;
}

/* ========================= */
/*  Function Implementation  */
/* ========================= */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref, //FIXME: Use &?
    Sophus::SE3d &T21){

    /* Initialization */
    const int iterations = 10;
    double cost = 0.0, lastCost = 0.0;

    Timer t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++)
    {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()), std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.getHessian();
        Vector6d b = jaco_accu.getBias();

        /* ----- Solve! ----- */
        // Solve the Linear System A*x=b, H(x)*∆x = g(x)
        Vector6d update = H.ldlt().solve(b); // ∆x = δξ (Lie Algebra)
        cost = jaco_accu.getTotalCost();

        // Check Solution
        if (std::isnan(update[0]))
        {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }

        /* Stopping Criteria */
        // If the cost increased, the update was not good, then break.
        if (iter > 0 && cost > lastCost)
        { // FIXME: I think the correct one is "cost >= lastCost"
            cout << "\ncost: " << cost << " >= lastCost: " << lastCost << ", break!" << endl;
            break;
        }

        /* ----- Update ----- */ // FIXME: Esta parte estava antes dos if acima, mas acredito que o correto seja fazer o update após os ifs
        // Left multiply T by a disturbance quantity exp(δξ)
        T21 = Sophus::SE3d::exp(update) * T21; // Left perturbation

        if (update.norm() < 1e-3)
        { // Method converged!
            break;
        }

        lastCost = cost;
        cout << "it: " << iter << ",\tcost: " << std::setprecision(12) << cost << ",\tupdate: " << update.transpose() << endl;
    }

    printMatrix<Matrix4d>("T21: ", T21.matrix());
    Timer t2 = chrono::steady_clock::now();

    printElapsedTime("Direct method (Single-layer): ", t1, t2);

    /* Plot the projected pixels */
    Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.getProjectedPoints();

    for (size_t i = 0; i < px_ref.size(); ++i)
    {                               // FIXME: pq ++i, e não i++
        auto p_ref = px_ref[i];     // p1_i
        auto p_cur = projection[i]; // p2^_i

        if (p_cur[0] > 0 && p_cur[1] > 0)
        { // x, y
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]), cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("Current", img2_show);
    cv::waitKey();
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) { // cv::Range(0, px_ref.size())
    /* Parameters */
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {
        /* Compute the projection in the second image */
        Eigen::Vector3d x1_ref = Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);  // p1_i->x1_i, x1 = [X/Z, Y/Z, 1]
        Eigen::Vector3d point_ref = depth_ref[i] * x1_ref;                                                // P1_i = [X, Y, Z]
        Eigen::Vector3d point_cur = T21 * point_ref;                                                      // P2^_i = T21*.P1_i

        if (point_cur[2] < 0) // invalid depth
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx; // u = fx*(X/Z)+cx
        float v = fy * point_cur[1] / point_cur[2] + cy; // v = fy*(Y/Z)+cy

        if (u < half_patch_size || u > img2.cols - half_patch_size ||
            v < half_patch_size || v > img2.rows - half_patch_size)
            continue;

        p2_proj[i] = Eigen::Vector2d(u, v); // p2^ = [u, v]
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
               Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;

        cnt_good++;

        // and compute error and jacobian
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++)
            {
                double error =  GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                                GetPixelValue(img2, u + x, v + y);

                Eigen::Vector2d J_img_pixel; // ∂I2/∂u
                Matrix26d J_pixel_xi;        // ∂u/∂δξ

                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y)));

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                // Total Jacobian, J = −(∂I2/∂u)*(∂u/∂δξ)
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose(); // 6x1

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

    if (cnt_good)
    {
        // Set Hessian, bias and cost_tmp
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}


/*
Copyright 2012-2018 Tamas Bolner

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 <cinttypes>
#include <iostream>
#include <Eigen/Dense>
#include "SimplexSolver.h"
#include "exception.h"

using namespace std;
using namespace Eigen;

int main()
{
    MatrixXd constraints(3, 3);
    VectorXd objectiveFunction(2);

    try {
        /*
            Maximization problem
        */
        objectiveFunction <<    1,
                                2;

        constraints <<      2,    3,    34,
                            1,    5,    45,
                            1,    0,    15;

        SimplexSolver solver1(SIMPLEX_MAXIMIZE, objectiveFunction, constraints);

        if (solver1.hasSolution()) {
            cout << "The maximum is: " << solver1.getOptimum() << endl;
            cout << "The solution is: " << solver1.getSolution().transpose() << endl;
        } else {
            cout << "The linear problem has no solution." << endl;
        }

        cout << endl;

        /*
            Minimization problem
        */
        objectiveFunction <<    3,
                                4;

        constraints <<      2,    1,    8,
                            1,    2,    13,
                            1,    5,    16;

        SimplexSolver solver2(SIMPLEX_MINIMIZE, objectiveFunction, constraints);

        if (solver2.hasSolution()) {
            cout << "The minimum is: " << solver2.getOptimum() << endl;
            cout << "The solution is: " << solver2.getSolution().transpose() << endl;
        } else {
            cout << "The linear problem has no solution." << endl;
        }
    } catch (const FException &ex) {
        ex.Print();
        return 1;
    }

    return 0;
}