#pragma once
#include <Eigen/Core>

#include "DualIntervals.hpp"

using namespace Eigen;

#ifdef USEDOUBLE
    using NType = double;
#else
    using NType = DI;
#endif

typedef Matrix<NType, Dynamic, Dynamic, RowMajor> Mat;
typedef Matrix<NType, 1, Dynamic, RowMajor> RowVec;
typedef Matrix<Interval, Dynamic, Dynamic, RowMajor> IntervalMat;
typedef Matrix<double, Dynamic, Dynamic, RowMajor> DoubleMat;

////////////////////////////////////////////////////////////////////////////////
//
// TENSOR FUNCTIONS
//
////////////////////////////////////////////////////////////////////////////////
// note: tensors are implemented as arrays (3D) or multi-dimensional arrays (4D+) of matrices

/*
 * 2D-Convolution with variable stride, padding, number of channels/kernels, and optional bias
 * Note: Assumes square image and kernel
 * Note: Bias is a tied bias (i.e., each kernel shares the same bias) -- pass in NULL if no bias
 * Note: delete[] must be called on the returned value explicitly
 *
 * img[n] represents an image with n channels (e.g., 3 for RGB), like a 3D-Tensor
 * kernels[i][j] represents i kernels, each with j channels (note: j must equal n), like a 4D-Tensor
 * stride represents how much the kernel is "moved" when doing the convolution
 * padding represents how much img is padded (with zeros) by on all sides
 */
template <int num_channels, int num_kernels>
Mat* Conv2d(Mat* img, Mat** kernels, RowVec* biases, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();            // side length of the square image
    int kernel_length = kernels[0][0].rows();  // side length of the kernel's square sliding window
    int kernel_size = kernel_length * kernel_length;
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    // img im2col, see here: https://leonardoaraujosantos.gitbook.io/artificial-inteligence/machine_learning/deep_learning/convolution_layer/making_faster
    // note: implemention is the transpose of what is described in the link above
    Mat resized_img(new_img_length * new_img_length, num_channels * kernel_size);
    for (int c = 0; c < num_channels; ++c) {
        int curr_resized_row = 0;
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                Mat b = padded_img[c].block(row, col, kernel_length, kernel_length);
                Map<RowVec> b_flattened(b.data(), 1, kernel_size);
                resized_img.block(curr_resized_row, c * kernel_size, 1, kernel_size) = b_flattened;
                ++curr_resized_row;
            }
        }
    }
    resized_img.transposeInPlace();

    // kernel im2col
    Mat resized_kernel(num_kernels, num_channels * kernel_size);
    for (int k = 0; k < num_kernels; ++k) {
        for (int c = 0; c < num_channels; ++c) {
            Map<RowVec> k_flattened(kernels[k][c].data(), 1, kernel_size);
            resized_kernel.block(k, c * kernel_size, 1, kernel_size) = k_flattened;
        }
    }

    // convolution as big matrix multiplication now!
    Mat res = resized_kernel * resized_img;

    // col2im, reshape back to correct dimensions, and apply bias (if applicable)
    if (biases) {
        res.colwise() += biases->transpose();  // adds bias value to each row of res (corresponding to each kernel)
    }

    Mat* res_resized = new Mat[num_kernels];
    for (int k = 0; k < num_kernels; ++k) {
        res_resized[k] = Map<Mat>(res.row(k).data(), new_img_length, new_img_length);
    }

    return res_resized;
}

/*
 * Max-Pooling with variable stride, padding, and number of channels/kernel sizes
 * Note: Assumes square image and kernel
 * Note: delete[] must be called on the returned value explicitly
 *
 * img[n] represents an image with n channels (e.g., 3 for RGB), like a 3D-Tensor
 * kernels_length represents the side length of the kernel (in the first two dimensions)
 * padding represents how much img is padded (with zeros) by on all sides
 * stride represents how much the kernel is "moved" when doing the pooling
 */
template <int num_channels>
Mat* MaxPool2d(Mat img[num_channels], int kernel_length, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    Mat* pooled_img = new Mat[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        int curr_row = 0, curr_col = 0;
        pooled_img[c] = Mat(new_img_length, new_img_length);
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                pooled_img[c](curr_row, curr_col) = padded_img[c].block(row, col, kernel_length, kernel_length).maxCoeff();
                ++curr_col;
                if (curr_col >= new_img_length) {
                    curr_col = 0;
                    ++curr_row;
                }
            }
        }
    }

    return pooled_img;
}

/*
 * Average-Pooling with variable stride, padding, and number of channels/kernel sizes
 * Everything the same as max-pooling, except we take the mean of the values in each window
 */
template <int num_channels>
Mat* AvgPool2d(Mat img[num_channels], int kernel_length, int stride = 1, int padding = 0) {
    int img_length = img[0].rows();
    int new_img_length = std::floor(((img_length + 2 * padding - kernel_length) / stride) + 1);

    // pad img
    Mat padded_img[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

    Mat* pooled_img = new Mat[num_channels];
    for (int c = 0; c < num_channels; ++c) {
        int curr_row = 0, curr_col = 0;
        pooled_img[c] = Mat(new_img_length, new_img_length);
        for (int row = 0; row + kernel_length <= img_length + 2 * padding; row += stride) {
            for (int col = 0; col + kernel_length <= img_length + 2 * padding; col += stride) {
                pooled_img[c](curr_row, curr_col) = padded_img[c].block(row, col, kernel_length, kernel_length).mean();
                ++curr_col;
                if (curr_col >= new_img_length) {
                    curr_col = 0;
                    ++curr_row;
                }
            }
        }
    }

    return pooled_img;
}

/*
 * Pad an image by "padding" amount on all sides
 */
template <int num_channels>
Mat* Pad(Mat img[num_channels], int padding) {
    int img_length = img[0].rows();

	Mat* padded_img = new Mat[num_channels];;
    for (int c = 0; c < num_channels; ++c) {
        padded_img[c] = Mat::Zero(img_length + 2 * padding, img_length + 2 * padding);
        padded_img[c].block(padding, padding, img_length, img_length) = img[c];
    }

	return padded_img;
}

/*
 * Flattens a 3D-Tensor into a single row vector, so that it can be fed into a fully-connected layer
 */
template <int num_channels>
RowVec Flatten(Mat img[num_channels]) {
    int img_size = img[0].rows() * img[0].cols();
    RowVec flattened_img(1, num_channels * img_size);
    for (int c = 0; c < num_channels; ++c) {
        flattened_img.block(0, c * img_size, 1, img_size) = Map<RowVec>(img[c].data(), 1, img_size);
    }
    return flattened_img;
}

// IMPORTANT: use as an in-place operation (output will replace input)
template <int num_channels>
Mat* Relu(Mat* x) {
    for (int c = 0; c < num_channels; ++c) {
        #ifdef USEDOUBLE
            x[c] = x[c].unaryExpr([](double a) { return std::max(0., a); });
        #else
            x[c] = x[c].unaryExpr(std::ref(relu));
        #endif
    }
    return x;
}

////////////////////////////////////////////////////////////////////////////////
//
// MATRIX FUNCTIONS
//
////////////////////////////////////////////////////////////////////////////////

Mat Exp(const Mat& x) {
    return ((x.array()).exp()).matrix();
}

Mat Log(const Mat& x) {
    return ((x.array()).log()).matrix();
}

Mat Sqrt(const Mat& x) {
    return ((x.array()).sqrt()).matrix();
}

Mat Sin(const Mat& x) {
    return ((x.array()).sin()).matrix();
}

Mat Cos(const Mat& x) {
    return ((x.array()).cos()).matrix();
}

Mat Tanh(const Mat& x) {
    return ((x.array()).tanh()).matrix();
}

Mat Atan(const Mat& x) {
    return ((x.array()).atan()).matrix();
}

Mat Logistic(const Mat& x) {
    return (((x.array() * 0.5).tanh()) * 0.5 + 0.5).matrix();
}

Mat Softmax(const Mat& x) {
    Mat temp = Exp(x);
    Mat::Scalar total = temp.sum();
    return temp / total;
}

Mat Relu(const Mat& x) {
    #ifdef USEDOUBLE
        return x.unaryExpr([](double a) { return std::max(0., a); });
    #else
        return x.unaryExpr(std::ref(relu));
    #endif
}

Mat Dropout(const Mat& mask, const Mat& x) {
    return (mask.array() * x.array()).matrix();
}

template <class MatType>
double Norm1(const MatType& array) {
    size_t num_rows = array.rows();
    size_t num_cols = array.cols();

    double norm = 0.;
    for (size_t j = 0; j < num_cols; ++j) {
        double sum = 0.;
        for (size_t i = 0; i < num_rows; ++i) {
            #ifdef USEDOUBLE
                sum += std::abs(array(i, j));
            #else
                Interval ij = array(i, j);
                sum += std::max(std::abs(ij.lower()), std::abs(ij.upper()));
            #endif
        }
        norm = std::max(norm, sum);
    }
    return norm;
}

template <class MatType>
double NormInf(const MatType& array) {
    size_t num_rows = array.rows();
    size_t num_cols = array.cols();

    double norm = 0.;
    for (size_t i = 0; i < num_rows; ++i) {
        double sum = 0.;
        for (size_t j = 0; j < num_cols; ++j) {
            #ifdef USEDOUBLE
                sum += std::abs(array(i, j));
            #else
                Interval ij = array(i, j);
                sum += std::max(std::abs(ij.lower()), std::abs(ij.upper()));
            #endif
        }
        norm = std::max(norm, sum);
    }
    return norm;
}

void Print(const Mat& x) {
    for (size_t i = 0; i < x.rows(); i++) {
        for (size_t j = 0; j < x.cols(); j++) {
            std::cout << x(i, j) << ", ";
        }
        std::cout << std::endl;
    }
}


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

/**
 * \file Sturm.hpp
 * \brief Class for evaluating Sturm chains on polynomials, and bracketing the
 *        real roots.
 */

#ifndef OPENGV_STURM_HPP_
#define OPENGV_STURM_HPP_

#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{
/**
 * \brief The namespace of the math tools.
 */
namespace math
{

/**
 * Sturm is initialized over polynomials of arbitrary order, and used to compute
 * the real roots of the polynomial.
 */
class Sturm
{

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /** A pair of values bracketing a real root */
  typedef std::pair<double,double> bracket_t;

  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p(0,0)*x^n + p(0,1)*x^(n-1) ...).
   */
  Sturm( const Eigen::MatrixXd & p );
  /**
   * \brief Contructor.
   * \param[in] p The polynomial coefficients (poly = p[0]*x^n + p[1]*x^(n-1) ...).
   */
  Sturm( const std::vector<double> & p );
  /**
   * \brief Destructor.
   */
  virtual ~Sturm();

  /**
   * \brief Finds the roots of the polynomial.
   * \return An array with the real roots of the polynomial.
   */
  std::vector<double> findRoots();
  /**
   * \brief Finds brackets for the real roots of the polynomial.
   * \return An array of brackets for the real roots of the polynomial.
   */
  std::vector<bracket_t> bracketRoots();
  /**
   * \brief Evaluates the Sturm chain for a bracket.
   * \param[in] leftBound The left value of the bracket.
   * \param[in] rightBound The right value of the bracket.
   * \return The number of real roots in the given bracket.
   */
  size_t evaluateChain( double leftBound, double rightBound );

private:
  /**
   * \brief Internal function used for composing the Sturm chain
   * \param[in] p1 First polynomial.
   * \param[in] p2 Second polynomial.
   * \param[out] r The negated remainder of the polynomial division p1/p2.
   */
  void computeNegatedRemainder(
      const Eigen::MatrixXd & p1,
      const Eigen::MatrixXd & p2,
      Eigen::MatrixXd & r );
  /**
   * \brief Internal function used for composing an initial bracket for all
   *        the roots of the polynomial.
   * \return The maximum of the absolute values of the bracket-values (That's
   *         what the Lagrangian bound is able to find).
   */
  double computeLagrangianBound();

  /** A matrix containing the coefficients of the Sturm-chain of the polynomial */
  Eigen::MatrixXd _C;
  /** The dimension _C, which corresponds to (polynomial order+1) */
  size_t _dimension;
};

}
}

#endif /* OPENGV_STURM_HPP_ */


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_OccupationNumberEvaluator.hpp
//! \author Alex Robinson
//! \brief  The occupation number evaluator declaration
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_OCCUPATION_NUMBER_EVALUATOR_HPP
#define DATA_GEN_OCCUPATION_NUMBER_EVALUATOR_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>

// FRENSIE Includes
#include "Utility_TabularDistribution.hpp"

namespace DataGen{

//! The occupation number evaluator
class OccupationNumberEvaluator
{

public:

  //! Constructor
  OccupationNumberEvaluator(
		   const Teuchos::Array<double>& electron_momentum_projections,
		   const Teuchos::Array<double>& compton_profile,
		   const double norm_constant_precision = 1e-6 );

  //! Destructor
  ~OccupationNumberEvaluator()
  { /* ... */ }

  //! Return the normalization constant used with the Compton profile
  double getComptonProfileNormConstant() const;

  //! Evaluate the compton profile
  double evaluateComptonProfile(
			     const double electron_momentum_projection ) const;

  //! Return the occupation number at a given electron momentum projection
  double evaluateOccupationNumber( const double electron_momentum_projection,
				   const double precision = 1e-6 ) const;

private:

  // The compton profile normalization constant (rounding error issue)
  double d_compton_profile_norm_constant;

  // The compton profile
  boost::scoped_ptr<const Utility::TabularDistribution<Utility::LogLin> >
  d_compton_profile;
};

} // end DataGen namespace

#endif // end DATA_GEN_OCCUPATION_NUMBER_EVALUATOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_OccupationNumberEvaluator.hpp
//---------------------------------------------------------------------------//


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/array.hpp>
#include <array>
#include <utility>
#include <random>
#include <iostream>
#include <ctime>
#include <cstdint>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  struct edge_properties
  {
    int weight;
  };

  typedef boost::adjacency_list<boost::listS, boost::vecS,
    boost::undirectedS> graph;

  graph g{edges.begin(), edges.end(), 4};

  boost::array<int, 4> predecessors;

  std::mt19937 gen{static_cast<uint32_t>(std::time(0))};
  boost::random_spanning_tree(g, gen,
    boost::predecessor_map(predecessors.begin()).
    root_vertex(bottomLeft));

  int p = topRight;
  while (p != -1)
  {
    std::cout << p << '\n';
    p = predecessors[p];
  }
}


/*
 *  cholesky_boost.cpp
 *  Jan07
 *  Created by  Adwait Joshi
 *  Project: Arcee
 *
 *  Copyright (c) 2007-2008 The Trustees of Indiana University. All rights reserved.
 *
 */

#include <iostream>
#include <string>
#include <boost/test/minimal.hpp>
#include <boost/tuple/tuple.hpp>

#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/sub_matrix.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>
#include <boost/numeric/mtl/recursion/base_case_test.hpp>
#include <boost/numeric/mtl/recursion/for_each.hpp>

//#include "base_Cases_Boost.h"
#include "base_Cases_Boost_new.h"

using namespace mtl;
using namespace std;

typedef dense2D<double> matrix_type;
//typedef morton_dense<double,  0x55555555> matrix_type;
const int order=4200 ;
const int basecasesize =32;
recursion::bound_test_static<basecasesize>    is_base;

int  callnum = 0, basehit = 0;
int docholcall=0;
int schurcall=0;
int trischurcall=0;
int trisolvecall=0;

template <typename Matrix>
void print_matrix(Matrix& matrix){
	for (int i=0 ; i<matrix.num_rows(); i++ ){
		for(int j=0; j<matrix.num_cols();  j++ ){
		   cout.fill (' '); cout.width (8); cout.precision (5); cout.flags (ios_base::left);
		   cout << showpoint <<  matrix[i][j] <<"  ";
		}
		cout << endl;
	}
	return;
}

template <typename Matrix>
void fill_matrix(Matrix& matrix){
    typename traits::row<Matrix>::type                                 row(matrix);
    typename traits::col<Matrix>::type                                 col(matrix);
    typename traits::value<Matrix>::type                               value(matrix);
    typedef  glas::tag::nz                                          tag;
    typedef typename traits::range_generator<tag, Matrix>::type        cursor_type;

    double x= 1.0;
    for(int i=0;i<matrix.num_rows();i++) {
       for(int j=0;j<=i;j++){
         if(i!=j){
	          matrix[i][j]=x; matrix[j][i]=x;
	          x=x+1.0;
	       }
       }
    }

    double rowsum;
    for(int i=0;i<matrix.num_rows();i++){
       rowsum=0.0;
       for(int j=0;j<matrix.num_cols();j++){
         if(i!=j){
	          rowsum += matrix[i][j];
	       }
       }
       matrix[i][i]=rowsum*2;
    }
}

template <typename Recursator>
void schur(Recursator E, Recursator W, Recursator N)
{
  if (E.is_empty() || W.is_empty() || N.is_empty())
    return;

  if(is_base(E)){
     typename Recursator::matrix_type  base_E(E.get_value()), base_W(W.get_value()),base_N(N.get_value());
     schur_base(base_E, base_W, base_N);
  }
  else{
    schur(     E.north_east(),W.north_west()     ,N.south_west()     );
    schur(     E.north_east(),     W.north_east(),     N.south_east());

    schur(E.north_west()     ,     W.north_east(),     N.north_east());
    schur(E.north_west()     ,W.north_west()     ,N.north_west()     );

    schur(E.south_west()     ,W.south_west()     ,N.north_west()     );
    schur(E.south_west()     ,     W.south_east(),     N.north_east());

    schur(     E.south_east(),     W.south_east(),     N.south_east());
    schur(     E.south_east(),W.south_west()     ,N.south_west()     );
  }
}

template <typename Recursator>
void tri_solve(Recursator S, Recursator N)
{
  if (S.is_empty())
    return;

  if(is_base(S)){
     typename Recursator::matrix_type  base_S(S.get_value()), base_N(N.get_value());
     tri_solve_base(base_S, base_N);
  }
  else{
  tri_solve(S.north_west()     ,N.north_west()     );

      schur(     S.north_east(),S.north_west()     ,N.south_west()     );

  tri_solve(     S.north_east(),     N.south_east());

  tri_solve(S.south_west()     ,N.north_west()     );

      schur(     S.south_east(),S.south_west()     ,N.south_west()     );

  tri_solve(     S.south_east(),     N.south_east());
  }
}

template <typename Recursator>
void tri_schur(Recursator E, Recursator W)
{
  if (E.is_empty() || W.is_empty())
    return;

  if(is_base(W)){
     typename Recursator::matrix_type  base_E(E.get_value()), base_W(W.get_value());
     tri_schur_base(base_E, base_W);
  }
  else{
      schur(     E.south_west(),     W.south_west(),    W.north_west());

      schur(     E.south_west(),     W.south_east(),    W.north_east());

  tri_schur(E.south_east()     ,     W.south_east());

  tri_schur(E.south_east()     ,W.south_west()     );

  tri_schur(     E.north_west(),     W.north_east());

  tri_schur(     E.north_west(),W.north_west()     );
  }
}

template <typename Recursator>
void
do_cholesky (Recursator recursator)
{
  if (recursator.is_empty())
    return;

  if (is_base (recursator)){
      typename Recursator::matrix_type  base_matrix(recursator.get_value());
      do_cholesky_base (base_matrix);
  }
  else{

    do_cholesky(recursator.north_west()     );

      tri_solve(recursator.south_west()     ,recursator.north_west()     );

      tri_schur(     recursator.south_east(),recursator.south_west()     );

    do_cholesky(     recursator.south_east());

  }
}

int test_main(int argc, char* argv[])
{

  //    cout << "=====================\n" << "Morton-ordered matrix\n" << "=====================\n\n";
  matrix_type matrix(order,order);
  time_t starttime,endtime;
  struct tm *timeinfo;

  time (&starttime);
  timeinfo = localtime (&starttime);

  printf("----------order = %d      Basecase = %d  -------------------->Load start: %s",
	 order, basecasesize, asctime (timeinfo));

  fill_matrix(matrix);
  // test_sub_matrix(matrix);
  recursion::mat::recursator<matrix_type> recursator(matrix);
  // print_matrix(matrix);
  time (&starttime);
  timeinfo = localtime (&starttime);
  printf("----------order = %d      Basecase = %d  -------------------->Start date and time are: %s",
     order, basecasesize, asctime (timeinfo));

  do_cholesky(recursator);

  //    cout << "\n=============================\n"	 <<   "Again with cholesky\n"	 <<   "=============================\n\n";

  time (&endtime);
  timeinfo = localtime (&endtime);
  printf("----------order = %d      Basecase = %d  ------------------->End date and time are: %s",
     order, basecasesize, asctime (timeinfo));
  //printf ("\nRec calls: %d    Basehits: %d\n", callnum, basehit);

  printf
    ("\nTOTAL TIME TAKEN for order = %d      Basecase = %d  : %d  secs\n\n",
     order, basecasesize, endtime - starttime);

   //cout << "\n\n\n\n\n\n";

 // print_matrix(matrix);
 /*     cout << "\n=============================\n"	;

		for(int i=0 ; i<matrix.num_rows();  i++ ){
			for (int j=i+1; j<matrix.num_cols(); j++ )
	    matrix[i][j]=0;
	    }

   print_matrix(matrix);
   cout  <<      "=============================\n\n";
    verify_matrix(matrix);
   cout  <<      "=============================\n\n";
   print_matrix(matrix);*/

     //printf("Rec Calls:\ndocholcall: %d\nschurcall: %d\ntrischurcall:%d\ntrisolvecall:%d\n", 	 docholcall, schurcall, trischurcall,trisolvecall);
     //printf("\nbasecase calls\ndocholhits:%d\nschurhits:%d\ntrischurhits:%d\ntrisolvehits:%d\n",	   docholeskyhits , schurhits , trischurhits, trisolvehits);

    return 0;
}


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

#include <nt2/include/functor.hpp>
#include <nt2/include/functions/sqr_abs.hpp>
#include <boost/simd/toolbox/constant/constants/zero.hpp>

namespace nt2 {  namespace tag
  {
    /*!
     * \brief Define the tag expm_ of functor expm
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct cov_ : tag::formal_
    {
       typedef tag::formal_ parent;
    };
  }
  /**
   * @brief compute covariance matrix expression
   *
   * If x is a vector,  cov(x) returns the variance
   * For matrices, where each row is an observation, and each column a variable,
   * cov(x) is the covariance matrix.  diag(cov(x)) is a vector of
   * variances for each column, and sqrt(diag(cov(x))) is a vector
   * of standard deviations.
   * cov(x,y), where x and y are matrices with the same number of elements,
   * is equivalent to cov(horzcat(x(_) y(_))).
   *
   * cov(x) or cov(x,y) normalizes by (n-1) if n>1, where n is the number of
   * observations.  this makes cov(x) the best unbiased estimate of the
   * covariance matrix if the observations are from a normal distribution.
   * for n=1, cov normalizes by n.
   *
   *  cov(x,1) or cov(x,y,1) normalizes by n and produces the second
   * moment matrix of the observations about their mean.  cov(x,y,0) is
   * the same as cov(x,y) and cov(x,0) is the same as cov(x).
   *
   * the mean is removed from each column before calculating the
   * result.
   *
  **/

  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::cov_       , cov, 1)
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::cov_       , cov, 2)
  NT2_FUNCTION_IMPLEMENTATION(nt2::tag::cov_       , cov, 3)
}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_POW_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POW_ABS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing pow_abs capabilities

    Computes \f$|x|^y\f$.

    @par Semantic:

    For every parameters of floating type  T:

    @code
    T r = pow_abs(x, y);
    @endcode

    is similar to:

    @code
    T r = pow(abs(x), y);
    @endcode

    @see pow, abs

  **/
  const boost::dispatch::functor<tag::pow_abs_> pow_abs = {};
} }
#endif

#include <boost/simd/function/scalar/pow_abs.hpp>
#include <boost/simd/function/simd/pow_abs.hpp>

#endif


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

// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2018-2020.
// Modifications copyright (c) 2018-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP

#include <type_traits>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/core/static_assert.hpp>
#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/vector_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_quaternion_concept.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_matrix_concept.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

namespace boost { namespace geometry {

namespace detail { namespace transform_geometrically {

template <typename Box, typename Vector, std::size_t Dimension>
struct box_vector_cartesian
{
    BOOST_GEOMETRY_STATIC_ASSERT(
        (Dimension > 0),
        "Dimension has to be greater than 0.",
        Box);

    static inline void apply(Box & box, Vector const& vector)
    {
        box_vector_cartesian<Box, Vector, Dimension-1>::apply(box, vector);
        set<min_corner, Dimension-1>(box, get<min_corner, Dimension-1>(box) + get<Dimension-1>(vector));
        set<max_corner, Dimension-1>(box, get<max_corner, Dimension-1>(box) + get<Dimension-1>(vector));
    }
};

template <typename Box, typename Vector>
struct box_vector_cartesian<Box, Vector, 1>
{
    static inline void apply(Box & box, Vector const& vector)
    {
        set<min_corner, 0>(box, get<min_corner, 0>(box) + get<0>(vector));
        set<max_corner, 0>(box, get<max_corner, 0>(box) + get<0>(vector));
    }
};

}} // namespace detail::transform

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename Geometry, typename Transform,
          typename GTag = typename tag<Geometry>::type,
          typename TTag = typename tag<Transform>::type>
struct transform_geometrically
{
    BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
        "Not implemented for this Geometry.",
        Geometry, Transform);
};

// Point translation by Vector
template <typename Point, typename Vector>
struct transform_geometrically<Point, Vector, point_tag, vector_tag>
{
    BOOST_CONCEPT_ASSERT( (concepts::Point<Point>) );
    BOOST_CONCEPT_ASSERT( (concepts::Vector<Vector>) );

    static inline void apply(Point & point, Vector const& vector)
    {
        typedef std::is_same
            <
                typename traits::coordinate_system<Point>::type,
                cs::cartesian
            > is_cartesian;
        apply(point, vector, is_cartesian());
    }

    static inline void apply(Point & point, Vector const& vector, std::true_type /*is_cartesian*/)
    {
        for_each_coordinate(point, detail::point_operation<Vector, std::plus>(vector));
    }

    static inline void apply(Point & point, Vector const& vector, std::false_type /*is_cartesian*/)
    {
        BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
            "Not implemented for this coordinate system.",
            typename traits::coordinate_system<Point>::type);
    }
};

// Box translation by Vector
template <typename Box, typename Vector>
struct transform_geometrically<Box, Vector, box_tag, vector_tag>
{
    typedef typename traits::point_type<Box>::type point_type;

    BOOST_CONCEPT_ASSERT( (concepts::Point<point_type>) );
    BOOST_CONCEPT_ASSERT( (concepts::Vector<Vector>) );

    static inline void apply(Box & box, Vector const& vector)
    {
        typedef std::is_same
            <
                typename traits::coordinate_system<point_type>::type,
                cs::cartesian
            > is_cartesian;
        apply(box, vector, is_cartesian());
    }

    static inline void apply(Box & box, Vector const& vector, std::true_type /*is_cartesian*/)
    {
        geometry::detail::transform_geometrically::box_vector_cartesian<
            Box, Vector, traits::dimension<point_type>::value
        >::apply(box, vector);
    }

    static inline void apply(Box & box, Vector const& vector, std::false_type /*is_cartesian*/)
    {
        BOOST_GEOMETRY_STATIC_ASSERT_FALSE(
            "Not implemented for this coordinate system.",
            typename traits::coordinate_system<point_type>::type);
    }
};

// Vector rotation by Quaternion
template <typename Vector, typename RotationQuaternion>
struct transform_geometrically<Vector, RotationQuaternion, vector_tag, rotation_quaternion_tag>
{
    static inline void apply(Vector & v, RotationQuaternion const& r)
    {
        concepts::check_concepts_and_equal_dimensions<Vector, RotationQuaternion const>();

        detail::algebra::quaternion_rotate(v, r);
    }
};

// Vector rotation by Matrix
template <typename Vector, typename RotationMatrix>
struct transform_geometrically<Vector, RotationMatrix, vector_tag, rotation_matrix_tag>
{
    static inline void apply(Vector & v, RotationMatrix const& r)
    {
        concepts::check_concepts_and_equal_dimensions<Vector, RotationMatrix const>();

        // TODO vector_type and convert from Vector
        Vector tmp(v);
        detail::algebra::matrix_rotate(r, tmp, v);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename Geometry, typename Transformation>
inline void transform_geometrically(Geometry & g, Transformation const& t)
{
    dispatch::transform_geometrically<Geometry, Transformation>::apply(g, t);
}

template <typename GeometrySrc, typename Transformation, typename GeometryDst>
inline void transformed_geometrically(GeometrySrc const& gsrc, Transformation const& t, GeometryDst & gdst)
{
    geometry::convert(gsrc, gdst);
    geometry::transform_geometrically(gdst, t);
}

template <typename GeometryDst, typename GeometrySrc, typename Transformation>
inline GeometryDst return_transformed_geometrically(GeometrySrc const& gsrc, Transformation const& t)
{
    GeometryDst res;
    transformed_geometrically(gsrc, t, res);
    return res;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_TRANSFORM_HPP


/*
 * Copyright 2018 Pedro Proenza <p.proenca@surrey.ac.uk> (University of Surrey)
 *
 */

#include <iostream>
#include <cstdio>
#define _USE_MATH_DEFINES
#include <math.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "CAPE/CAPE.h"

bool done = false;
float COS_ANGLE_MAX = cos(M_PI/12);
float MAX_MERGE_DIST = 50.0f;
bool cylinder_detection= true;
CAPE * plane_detector;
std::vector<cv::Vec3b> color_code;

bool loadCalibParameters(std::string filepath, cv:: Mat & intrinsics_rgb, cv::Mat & dist_coeffs_rgb, cv:: Mat & intrinsics_ir, cv::Mat & dist_coeffs_ir, cv::Mat & R, cv::Mat & T){

    cv::FileStorage fs(filepath,cv::FileStorage::READ);
    if (fs.isOpened()){
        fs["RGB_intrinsic_params"]        >> intrinsics_rgb;
        fs["RGB_distortion_coefficients"] >> dist_coeffs_rgb;
        fs["IR_intrinsic_params"]         >> intrinsics_ir;
        fs["IR_distortion_coefficients"]  >> dist_coeffs_ir;
        fs["Rotation"]                    >> R;
        fs["Translation"]                 >> T;
        fs.release();
        return true;
    }else{
        std::cerr << "Calibration file missing" << std::endl;
        return false;
    }
}

void projectPointCloud(cv::Mat & X, cv::Mat & Y, cv::Mat & Z, cv::Mat & U, cv::Mat & V, float fx_rgb, float fy_rgb, float cx_rgb, float cy_rgb, double z_min, Eigen::MatrixXf & cloud_array){

    int width = X.cols;
    int height = X.rows;

    // Project to image coordinates
    cv::divide(X,Z,U,1);
    cv::divide(Y,Z,V,1);
    U = U*fx_rgb + cx_rgb;
    V = V*fy_rgb + cy_rgb;
    // Reusing U as cloud index
    //U = V*width + U + 0.5;

    float * sz, * sx, * sy, * u_ptr, * v_ptr, * id_ptr;
    float z, u, v;
    int id;
    for(int r=0; r< height; r++){
        sx = X.ptr<float>(r);
        sy = Y.ptr<float>(r);
        sz = Z.ptr<float>(r);
        u_ptr = U.ptr<float>(r);
        v_ptr = V.ptr<float>(r);
        for(int c=0; c< width; c++){
            z = sz[c];
            u = u_ptr[c];
            v = v_ptr[c];
            if(z>z_min && u>0 && v>0 && u<width && v<height){
                id = floor(v)*width + u;
                cloud_array(id,0) = sx[c];
                cloud_array(id,1) = sy[c];
                cloud_array(id,2) = z;
            }
        }
    }
}

void organizePointCloudByCell(Eigen::MatrixXf & cloud_in, Eigen::MatrixXf & cloud_out, cv::Mat & cell_map){
    int width = cell_map.cols;
    int height = cell_map.rows;
    int mxn = width*height;
    int mxn2 = 2*mxn;

    int id, it(0);
    int * cell_map_ptr;
    for(int r=0; r< height; r++){
        cell_map_ptr = cell_map.ptr<int>(r);
        for(int c=0; c< width; c++){
            id = cell_map_ptr[c];
            *(cloud_out.data() + id) = *(cloud_in.data() + it);
            *(cloud_out.data() + mxn + id) = *(cloud_in.data() + mxn + it);
            *(cloud_out.data() + mxn2 + id) = *(cloud_in.data() + mxn2 + it);
            it++;
        }
    }
}

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

    std::string sequence;
    int PATCH_SIZE;
    if (argc>1){
        PATCH_SIZE = atoi(argv[1]);
        sequence = argv[2];
    }else{
        PATCH_SIZE = 20;
        sequence = "pipe";
    }

    std::stringstream string_buff;
    std::string data_path = "/home/grvc/programming/CAPE/Data/";
    string_buff << data_path << sequence;

    // Get intrinsics
    cv::Mat K_rgb, K_ir, dist_coeffs_rgb, dist_coeffs_ir, R_stereo, t_stereo;
    std::stringstream calib_path;
    calib_path << string_buff.str() << "/calib_params.xml";
    loadCalibParameters(calib_path.str(), K_rgb, dist_coeffs_rgb, K_ir, dist_coeffs_ir, R_stereo, t_stereo);
    float fx_ir  = K_ir.at<double>(0,0);  float fy_ir = K_ir.at<double>(1,1);
    float cx_ir  = K_ir.at<double>(0,2);  float cy_ir = K_ir.at<double>(1,2);
    float fx_rgb = K_rgb.at<double>(0,0); float fy_rgb = K_rgb.at<double>(1,1);
    float cx_rgb = K_rgb.at<double>(0,2); float cy_rgb = K_rgb.at<double>(1,2);

    // Read frame 1 to allocate and get dimension
    cv::Mat rgb_img, d_img;
    int width, height;
    std::stringstream image_path;
    std::stringstream depth_img_path;
    std::stringstream rgb_img_path;
    rgb_img_path   << string_buff.str() << "/rgb_0.png";
    depth_img_path << string_buff.str() << "/depth_0.png";

    rgb_img = cv::imread(rgb_img_path.str(),cv::IMREAD_COLOR);

    if(rgb_img.data){
        width = rgb_img.cols;
        height = rgb_img.rows;
    }else{
        std::cout << "Error loading file";
        return -1;
    }

    int nr_horizontal_cells = width/PATCH_SIZE;
    int nr_vertical_cells = height/PATCH_SIZE;

    // Pre-computations for backprojection
    cv::Mat_<float> X_pre(height,width);
    cv::Mat_<float> Y_pre(height,width);
    cv::Mat_<float> U(height,width);
    cv::Mat_<float> V(height,width);
    for (int r=0;r<height; r++){
        for (int c=0;c<width; c++){
            // Not efficient but at this stage doesn t matter
            X_pre.at<float>(r,c) = (c-cx_ir)/fx_ir;
            Y_pre.at<float>(r,c) = (r-cy_ir)/fy_ir;
        }
    }

    // Pre-computations for maping an image point cloud to a cache-friendly array where cell's local point clouds are contiguous
    cv::Mat_<int> cell_map(height,width);

    for (int r=0;r<height; r++){
        int cell_r = r/PATCH_SIZE;
        int local_r = r%PATCH_SIZE;
        for (int c=0;c<width; c++){
            int cell_c = c/PATCH_SIZE;
            int local_c = c%PATCH_SIZE;
            cell_map.at<int>(r,c) = (cell_r*nr_horizontal_cells+cell_c)*PATCH_SIZE*PATCH_SIZE + local_r*PATCH_SIZE + local_c;
        }
    }

    cv::Mat_<float> X(height,width);
    cv::Mat_<float> Y(height,width);
    cv::Mat_<float> X_t(height,width);
    cv::Mat_<float> Y_t(height,width);
    Eigen::MatrixXf cloud_array(width*height,3);
    Eigen::MatrixXf cloud_array_organized(width*height,3);

    cv::namedWindow("Seg");

    // Populate with random color codes
    for(int i=0; i<100;i++){
        cv::Vec3b color;
        color[0]=rand()%255;
        color[1]=rand()%255;
        color[2]=rand()%255;
        color_code.push_back(color);
    }

    // Add specific colors for planes
    color_code[0][0] = 0; color_code[0][1] = 0; color_code[0][2] = 255;
    color_code[1][0] = 255; color_code[1][1] = 0; color_code[1][2] = 204;
    color_code[2][0] = 255; color_code[2][1] = 100; color_code[2][2] = 0;
    color_code[3][0] = 0; color_code[3][1] = 153; color_code[3][2] = 255;
    // Add specific colors for cylinders
    color_code[50][0] = 178; color_code[50][1] = 255; color_code[50][2] = 0;
    color_code[51][0] = 255; color_code[51][1] = 0; color_code[51][2] = 51;
    color_code[52][0] = 0; color_code[52][1] = 255; color_code[52][2] = 51;
    color_code[53][0] = 153; color_code[53][1] = 0; color_code[53][2] = 255;

    // Initialize CAPE
    plane_detector = new CAPE(height, width, PATCH_SIZE, PATCH_SIZE, cylinder_detection, COS_ANGLE_MAX, MAX_MERGE_DIST);

    int i=0;
    while(true){

        // Read frame i
        rgb_img_path.str("");
        rgb_img_path << string_buff.str() << "/rgb_" << i << ".png";
        rgb_img = cv::imread(rgb_img_path.str(),cv::IMREAD_COLOR);

        if (!rgb_img.data)
            break;

        std::cout << "Frame: " << i << std::endl;

        // Read depth image
        depth_img_path.str("");
        depth_img_path << string_buff.str() << "/depth_" << i << ".png";

        d_img = cv::imread(depth_img_path.str(), cv::IMREAD_ANYDEPTH);
        d_img.convertTo(d_img, CV_32F);

        // Backproject to point cloud
        X = X_pre.mul(d_img); Y = Y_pre.mul(d_img);
        cloud_array.setZero();

        // The following transformation+projection is only necessary to visualize RGB with overlapped segments
        // Transform point cloud to color reference frame
        X_t = ((float)R_stereo.at<double>(0,0))*X + ((float)R_stereo.at<double>(0,1))*Y + ((float)R_stereo.at<double>(0,2))*d_img + (float)t_stereo.at<double>(0);
        Y_t = ((float)R_stereo.at<double>(1,0))*X + ((float)R_stereo.at<double>(1,1))*Y + ((float)R_stereo.at<double>(1,2))*d_img + (float)t_stereo.at<double>(1);
        d_img = ((float)R_stereo.at<double>(2,0))*X + ((float)R_stereo.at<double>(2,1))*Y + ((float)R_stereo.at<double>(2,2))*d_img + (float)t_stereo.at<double>(2);

        projectPointCloud(X_t, Y_t, d_img, U, V, fx_rgb, fy_rgb, cx_rgb, cy_rgb, t_stereo.at<double>(2), cloud_array);

        cv::Mat_<cv::Vec3b> seg_rz = cv::Mat_<cv::Vec3b>(height,width,cv::Vec3b(0,0,0));
        cv::Mat_<uchar> seg_output = cv::Mat_<uchar>(height,width,uchar(0));

        // Run CAPE
        int nr_planes, nr_cylinders;
        std::vector<PlaneSeg> plane_params;
        std::vector<CylinderSeg> cylinder_params;
        double t1 = cv::getTickCount();
        organizePointCloudByCell(cloud_array, cloud_array_organized, cell_map);
        plane_detector->process(cloud_array_organized, nr_planes, nr_cylinders, seg_output, plane_params, cylinder_params);
        double t2 = cv::getTickCount();
        double time_elapsed = (t2-t1)/(double)cv::getTickFrequency();
        std::cout<<"Total time elapsed: " << time_elapsed << std::endl;

        /* Uncomment this block to print model params
        for(int p_id=0; p_id<nr_planes;p_id++){
            cout<<"[Plane #"<<p_id<<"] with ";
            cout<<"normal: ("<<plane_params[p_id].normal[0]<<" "<<plane_params[p_id].normal[1]<<" "<<plane_params[p_id].normal[2]<<"), ";
            cout<<"d: "<<plane_params[p_id].d<<endl;
        }

        for(int c_id=0; c_id<nr_cylinders;c_id++){
            cout<<"[Cylinder #"<<c_id<<"] with ";
            cout<<"axis: ("<<cylinder_params[c_id].axis[0]<<" "<<cylinder_params[c_id].axis[1]<<" "<<cylinder_params[c_id].axis[2]<<"), ";
            cout<<"center: ("<<cylinder_params[c_id].centers[0].transpose()<<"), ";
            cout<<"radius: "<<cylinder_params[c_id].radii[0]<<endl;
        }
        */

        // Map segments with color codes and overlap segmented image w/ RGB
        uchar * sCode;
        uchar * dColor;
        uchar * srgb;
        int code;
        for(int r=0; r<  height; r++){
            dColor = seg_rz.ptr<uchar>(r);
            sCode = seg_output.ptr<uchar>(r);
            srgb = rgb_img.ptr<uchar>(r);
            for(int c=0; c< width; c++){
                code = *sCode;
                if (code>0){
                    dColor[c*3] =   color_code[code-1][0]/2 + srgb[0]/2;
                    dColor[c*3+1] = color_code[code-1][1]/2 + srgb[1]/2;
                    dColor[c*3+2] = color_code[code-1][2]/2 + srgb[2]/2;;
                }else{
                    dColor[c*3] =  srgb[0];
                    dColor[c*3+1] = srgb[1];
                    dColor[c*3+2] = srgb[2];
                }
                sCode++; srgb++; srgb++; srgb++;
            }
        }

        // Show frame rate and labels
        cv::rectangle(seg_rz,  cv::Point(0,0),cv::Point(width,20), cv::Scalar(0,0,0),-1);
        std::stringstream fps;
        fps << (int)(1/time_elapsed+0.5) << " fps";
        cv::putText(seg_rz, fps.str(), cv::Point(15,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
        std::cout << "Nr cylinders:" << nr_cylinders << std::endl;
        int cylinder_code_offset = 50;
        // show cylinder labels
        if (nr_cylinders>0){
            std::stringstream text;
            text<<"Cylinders:";

            cv::putText(seg_rz, text.str(), cv::Point(width/2,15), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255,255,255,1));
            for(int j=0;j<nr_cylinders;j++){
                cv::rectangle(seg_rz,  cv::Point(width/2 + 80+15*j,6),cv::Point(width/2 + 90+15*j,16), cv::Scalar(color_code[cylinder_code_offset+j][0],color_code[cylinder_code_offset+j][1],color_code[cylinder_code_offset+j][2]),-1);
            }
        }
        cv::imshow("Seg", seg_rz);
        cv::waitKey(1);
        i++;
    }

    return 0;
}


#include <iostream>
#include <boost/format.hpp>
// Qt headers
#include <QtGui>
#include <CGAL/Qt/GraphicsViewNavigation.h>
#include <QLineF>
#include <QRectF>
#include <QApplication>
#include <QGraphicsScene>
#include <QGraphicsView>
// CGAL headers
#include <CGAL/Qt/PointsGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/utility.h>
// GraphicsView items and event filters (input classes)
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Largest_empty_iso_rectangle_2.h>
// the two base classes
#include <QString>
#include <QFileDialog>
#include <QInputDialog>
#include <QGraphicsRectItem>
#include <QGraphicsLineItem>
using namespace std;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;
typedef CGAL::Triangle_2<K> Triangle_2;

int main(int argc, char **argv)
{
	CGAL::Qt::PointsGraphicsItem<std::vector<Point_2> > * pgi;
	QApplication app(argc, argv);
	QGraphicsScene scene;

	// AREA OF TRIANGLE
	FILE *fp = NULL;
	fp = fopen("../src/input.txt","r");
	int pt1,pt2;
	Point_2 points[3];
	int i=0;

	// DISPLAYING TRIANGL AND ITS AREA
	QPolygonF Triangle;

	while(fscanf(fp,"%d %d",&pt1,&pt2)!=EOF){

		points[i]=Point_2(pt1,pt2);
		Triangle.append(QPointF(pt1,pt2));
		i++;

	}

	// finding area of triangle
	Triangle_2 pgn(points[0],points[1],points[2]);
  	double Area = abs(pgn.area());
  	cout << Area;

	scene.addPolygon(Triangle);
	QString str = QStringLiteral("Area : %1 ").arg(Area);
	scene.addText(str);

    scene.setSceneRect(0,0, 300, 300);

    QGraphicsView* view = new QGraphicsView(&scene);
    CGAL::Qt::GraphicsViewNavigation navigation;
    view->installEventFilter(&navigation);
    view->viewport()->installEventFilter(&navigation);
    view->setRenderHint(QPainter::Antialiasing);

    view->show();

    return app.exec();
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <cstddef>

#include "DataStructures/DenseMatrix.hpp"
#include "DataStructures/DenseVector.hpp"
#include "Informer/Verbosity.hpp"
#include "NumericalAlgorithms/Convergence/Criteria.hpp"
#include "NumericalAlgorithms/Convergence/HasConverged.hpp"
#include "NumericalAlgorithms/Convergence/Reason.hpp"
#include "NumericalAlgorithms/LinearSolver/InnerProduct.hpp"
#include "Options/Options.hpp"
#include "Utilities/Gsl.hpp"

namespace LinearSolver {
namespace gmres::detail {

// Perform an Arnoldi orthogonalization to find a new `operand` that is
// orthogonal to all vectors in `basis_history`. Appends a new column to the
// `orthogonalization_history` that holds the inner product of the intermediate
// `operand` with each vector in the `basis_history` and itself.
template <typename VarsType>
void arnoldi_orthogonalize(
    const gsl::not_null<VarsType*> operand,
    const gsl::not_null<DenseMatrix<double>*> orthogonalization_history,
    const std::vector<VarsType>& basis_history,
    const size_t iteration) noexcept {
  // Resize matrix and make sure the new entries that are not being filled below
  // are zero.
  orthogonalization_history->resize(iteration + 2, iteration + 1);
  for (size_t j = 0; j < iteration; ++j) {
    (*orthogonalization_history)(iteration + 1, j) = 0.;
  }
  // Arnoldi orthogonalization
  for (size_t j = 0; j < iteration + 1; ++j) {
    const double orthogonalization = inner_product(basis_history[j], *operand);
    (*orthogonalization_history)(j, iteration) = orthogonalization;
    *operand -= orthogonalization * basis_history[j];
  }
  (*orthogonalization_history)(iteration + 1, iteration) =
      sqrt(inner_product(*operand, *operand));
  // Avoid an FPE if the new operand norm is exactly zero. In that case the
  // problem is solved and the algorithm will terminate (see Proposition 9.3 in
  // \cite Saad2003). Since there will be no next iteration we don't need to
  // normalize the operand.
  if (UNLIKELY((*orthogonalization_history)(iteration + 1, iteration) == 0.)) {
    return;
  }
  *operand /= (*orthogonalization_history)(iteration + 1, iteration);
}

// Solve the linear least-squares problem `||beta - H * y||` for `y`, where `H`
// is the Hessenberg matrix given by `orthogonalization_history` and `beta` is
// the vector `(initial_residual, 0, 0, ...)` by updating the QR decomposition
// of `H` from the previous iteration with a Givens rotation.
void solve_minimal_residual(
    gsl::not_null<DenseMatrix<double>*> orthogonalization_history,
    gsl::not_null<DenseVector<double>*> residual_history,
    gsl::not_null<DenseVector<double>*> givens_sine_history,
    gsl::not_null<DenseVector<double>*> givens_cosine_history,
    size_t iteration) noexcept;

// Find the vector that minimizes the residual by inverting the upper
// triangular matrix obtained above.
DenseVector<double> minimal_residual_vector(
    const DenseMatrix<double>& orthogonalization_history,
    const DenseVector<double>& residual_history) noexcept;

}  // namespace gmres::detail

namespace Serial {

template <typename VarsType>
struct IdentityPreconditioner {
  VarsType operator()(const VarsType& arg) const noexcept { return arg; }
};

struct NoIterationCallback {
  void operator()(const Convergence::HasConverged& /*has_converged*/) const
      noexcept {}
};

/*!
 * \brief A serial GMRES iterative solver for nonsymmetric linear systems of
 * equations.
 *
 * This is an iterative algorithm to solve general linear equations \f$Ax=b\f$
 * where \f$A\f$ is a linear operator. See \cite Saad2003, chapter 6.5 for a
 * description of the GMRES algorithm and Algorithm 9.6 for this implementation.
 * It is matrix-free, which means the operator \f$A\f$ needs not be provided
 * explicity as a matrix but only the operator action \f$A(x)\f$ must be
 * provided for an argument \f$x\f$.
 *
 * The GMRES algorithm does not require the operator \f$A\f$ to be symmetric or
 * positive-definite. Note that other algorithms such as conjugate gradients may
 * be more efficient for symmetric positive-definite operators.
 *
 * \par Convergence:
 * Given a set of \f$N_A\f$ equations (e.g. through an \f$N_A\times N_A\f$
 * matrix) the GMRES algorithm will converge to numerical precision in at most
 * \f$N_A\f$ iterations. However, depending on the properties of the linear
 * operator, an approximate solution can ideally be obtained in only a few
 * iterations. See \cite Saad2003, section 6.11.4 for details on the convergence
 * of the GMRES algorithm.
 *
 * \par Restarting:
 * This implementation of the GMRES algorithm supports restarting, as detailed
 * in \cite Saad2003, section 6.5.5. Since the GMRES algorithm iteratively
 * builds up an orthogonal basis of the solution space the cost of each
 * iteration increases linearly with the number of iterations. Therefore it is
 * sometimes helpful to restart the algorithm every \f$N_\mathrm{restart}\f$
 * iterations, discarding the set of basis vectors and starting again from the
 * current solution estimate. This strategy can improve the performance of the
 * solver, but note that the solver can stagnate for non-positive-definite
 * operators and is not guaranteed to converge within \f$N_A\f$ iterations
 * anymore. Set the `restart` argument of the constructor to
 * \f$N_\mathrm{restart}\f$ to activate restarting, or set it to zero to
 * deactivate restarting (default behaviour).
 *
 * \par Preconditioning:
 * This implementation of the GMRES algorithm also supports preconditioning.
 * You can provide a linear operator \f$P\f$ that approximates the inverse of
 * the operator \f$A\f$ to accelerate the convergence of the linear solve.
 * The algorithm is right-preconditioned, which allows the preconditioner to
 * change in every iteration ("flexible" variant). See \cite Saad2003, sections
 * 9.3.2 and 9.4.1 for details. This implementation follows Algorithm 9.6 in
 * \cite Saad2003.
 *
 * \par Improvements:
 * Further improvements can potentially be implemented for this algorithm, see
 * e.g. \cite Ayachour2003.
 *
 * \example
 * \snippet NumericalAlgorithms/LinearSolver/Test_Gmres.cpp gmres_example
 */
template <typename VarsType>
struct Gmres {
 private:
  struct ConvergenceCriteria {
    using type = Convergence::Criteria;
    static constexpr OptionString help =
        "Determine convergence of the algorithm";
  };
  struct Restart {
    using type = size_t;
    static constexpr OptionString help = "Iterations to run before restarting";
    static size_t default_value() noexcept { return 0; }
  };
  struct Verbosity {
    using type = ::Verbosity;
    static constexpr OptionString help = "Logging verbosity";
  };

 public:
  static constexpr OptionString help =
      "A serial GMRES iterative solver for nonsymmetric linear systems of\n"
      "equations Ax=b. It will converge to numerical precision in at most N_A\n"
      "iterations, where N_A is the number of equations represented by the\n"
      "linear operator A, but will ideally converge to a reasonable\n"
      "approximation of the solution x in only a few iterations.\n"
      "\n"
      "Restarting: It is sometimes helpful to restart the algorithm every\n"
      "N_restart iterations to speed it up. Note that it can stagnate for\n"
      "non-positive-definite matrices and is not guaranteed to converge\n"
      "within N_A iterations anymore when restarting is activated.\n"
      "Activate restarting by setting the 'Restart' option to N_restart, or\n"
      "deactivate restarting by setting it to zero (default).";
  using options = tmpl::list<ConvergenceCriteria, Verbosity, Restart>;

  Gmres(Convergence::Criteria convergence_criteria, ::Verbosity verbosity,
        size_t restart = 0) noexcept
      // clang-tidy: trivially copyable
      : convergence_criteria_(std::move(convergence_criteria)),  // NOLINT
        verbosity_(std::move(verbosity)),                        // NOLINT
        restart_(restart > 0 ? restart : convergence_criteria_.max_iterations) {
    initialize();
  }

  Gmres() = default;
  Gmres(const Gmres& /*rhs*/) = default;
  Gmres& operator=(const Gmres& /*rhs*/) = default;
  Gmres(Gmres&& /*rhs*/) = default;
  Gmres& operator=(Gmres&& /*rhs*/) = default;
  ~Gmres() = default;

  void initialize() noexcept {
    orthogonalization_history_.reserve(restart_ + 1);
    residual_history_.reserve(restart_ + 1);
    givens_sine_history_.reserve(restart_);
    givens_cosine_history_.reserve(restart_);
    basis_history_.resize(restart_ + 1);
    preconditioned_basis_history_.resize(restart_);
  }

  const Convergence::Criteria& convergence_criteria() const noexcept {
    return convergence_criteria_;
  }

  void pup(PUP::er& p) noexcept {  // NOLINT
    p | convergence_criteria_;
    p | verbosity_;
    p | restart_;
    if (p.isUnpacking()) {
      initialize();
    }
  }

  /*!
   * \brief Iteratively solve the problem \f$Ax=b\f$ for \f$x\f$ where \f$A\f$
   * is the `linear_operator` and \f$b\f$ is the `source`, starting \f$x\f$ at
   * `initial_guess`.
   *
   * Optionally provide a `preconditioner` (see class documentation).
   *
   * \return An instance of `Convergence::HasConverged` that provides
   * information on the convergence status of the completed solve, and the
   * approximate solution \f$x\f$.
   */
  template <typename LinearOperator, typename SourceType,
            typename Preconditioner = IdentityPreconditioner<VarsType>,
            typename IterationCallback = NoIterationCallback>
  std::pair<Convergence::HasConverged, VarsType> operator()(
      LinearOperator&& linear_operator, const SourceType& source,
      const VarsType& initial_guess,
      Preconditioner&& preconditioner = IdentityPreconditioner<VarsType>{},
      IterationCallback&& iteration_callback = NoIterationCallback{}) const
      noexcept;

 private:
  Convergence::Criteria convergence_criteria_{};
  ::Verbosity verbosity_{::Verbosity::Verbose};
  size_t restart_{};

  // Memory buffers to avoid re-allocating memory for successive solves:
  // The `orthogonalization_history_` is built iteratively from inner products
  // between existing and potential basis vectors and then Givens-rotated to
  // become upper-triangular.
  mutable DenseMatrix<double> orthogonalization_history_{};
  // The `residual_history_` holds the remaining residual in its last entry, and
  // the other entries `g` "source" the minimum residual vector `y` in
  // `R * y = g` where `R` is the upper-triangular `orthogonalization_history_`.
  mutable DenseVector<double> residual_history_{};
  // These represent the accumulated Givens rotations up to the current
  // iteration.
  mutable DenseVector<double> givens_sine_history_{};
  mutable DenseVector<double> givens_cosine_history_{};
  // These represent the orthogonal Krylov-subspace basis that is constructed
  // iteratively by Arnoldi-orthogonalizing a new vector in each iteration and
  // appending it to the `basis_history_`.
  mutable std::vector<VarsType> basis_history_{};
  // When a preconditioner is used it is applied to each new basis vector. The
  // preconditioned basis is used to construct the solution when the algorithm
  // has converged.
  mutable std::vector<VarsType> preconditioned_basis_history_{};
};

template <typename VarsType>
template <typename LinearOperator, typename SourceType, typename Preconditioner,
          typename IterationCallback>
std::pair<Convergence::HasConverged, VarsType> Gmres<VarsType>::operator()(
    LinearOperator&& linear_operator, const SourceType& source,
    const VarsType& initial_guess, Preconditioner&& preconditioner,
    IterationCallback&& iteration_callback) const noexcept {
  constexpr bool use_preconditioner =
      not std::is_same_v<Preconditioner, IdentityPreconditioner<VarsType>>;
  constexpr bool use_iteration_callback =
      not std::is_same_v<IterationCallback, NoIterationCallback>;

  auto result = initial_guess;
  Convergence::HasConverged has_converged{};
  size_t iteration = 0;

  while (not has_converged) {
    auto& initial_operand = basis_history_[0] =
        source - linear_operator(result);
    const double initial_residual_magnitude =
        sqrt(inner_product(initial_operand, initial_operand));
    has_converged = Convergence::HasConverged{convergence_criteria_, iteration,
                                              initial_residual_magnitude,
                                              initial_residual_magnitude};
    if (use_iteration_callback) {
      iteration_callback(has_converged);
    }
    if (UNLIKELY(has_converged)) {
      break;
    }
    initial_operand /= initial_residual_magnitude;
    residual_history_.resize(1);
    residual_history_[0] = initial_residual_magnitude;
    for (size_t k = 0; k < restart_; ++k) {
      auto& operand = basis_history_[k + 1];
      if (use_preconditioner) {
        preconditioned_basis_history_[k] = preconditioner(basis_history_[k]);
      }
      operand =
          linear_operator(use_preconditioner ? preconditioned_basis_history_[k]
                                             : basis_history_[k]);
      // Find a new orthogonal basis vector of the Krylov subspace
      gmres::detail::arnoldi_orthogonalize(
          make_not_null(&operand), make_not_null(&orthogonalization_history_),
          basis_history_, k);
      // Least-squares solve for the minimal residual
      gmres::detail::solve_minimal_residual(
          make_not_null(&orthogonalization_history_),
          make_not_null(&residual_history_),
          make_not_null(&givens_sine_history_),
          make_not_null(&givens_cosine_history_), k);
      ++iteration;
      has_converged = Convergence::HasConverged{
          convergence_criteria_, iteration, abs(residual_history_[k + 1]),
          initial_residual_magnitude};
      if (use_iteration_callback) {
        iteration_callback(has_converged);
      }
      if (UNLIKELY(has_converged)) {
        break;
      }
    }
    // Find the vector w.r.t. the constructed orthogonal basis of the Krylov
    // subspace that minimizes the residual
    const auto minres = gmres::detail::minimal_residual_vector(
        orthogonalization_history_, residual_history_);
    // Construct the solution from the orthogonal basis and the minimal residual
    // vector
    for (size_t i = 0; i < minres.size(); ++i) {
      result +=
          minres[i] * gsl::at(use_preconditioner ? preconditioned_basis_history_
                                                 : basis_history_,
                              i);
    }
  }
  // NOLINTNEXTLINE(hicpp-move-const-arg,performance-move-const-arg)
  return {std::move(has_converged), std::move(result)};
}

}  // namespace Serial
}  // namespace LinearSolver


// Loba2d library

// Copyright (c) 2015 Andrii Sydorchuk

// Use, modification and distribution is subject to the MIT License (MIT).

#include <ctime>
#include <iostream>
#include <random>
#include <vector>

#include <boost/polygon/voronoi_builder.hpp>

#include "loba2d/delaunay_index.hpp"

const int RANDOM_SEED = 27;
const int NUM_POINTS = 1000000;

int main() {
  std::mt19937 gen(RANDOM_SEED);
  boost::polygon::voronoi_builder<int> vb;
  loba2d::delaunay_index di;
  std::vector< std::pair<int, int> > input;

  for (int i = 0; i < NUM_POINTS; ++i) {
    int x = gen() & ((1 << 30) - 1);
    int y = gen() & ((1 << 30) - 1);
    input.push_back(std::make_pair(x, y));
    vb.insert_point(x, y);
  }

  double t = clock();
  vb.construct(&di);
  double elapsed = (clock() - t) / 1E6;

  std::cout << "Delaunay graph constructed in: " << elapsed << "(secs)" << std::endl;
  std::cout << "Num. triangles: " << di.triangles().size() << std::endl;

  if (di.num_triangles() <= 10) {
    std::cout << "Triangles coordinates:" << std::endl;
    for (loba2d::delaunay_index::const_triangle_iterator it = di.triangles().begin();
         it != di.triangles().end(); ++it) {
      std::cout << "[";
      for (int i = 0; i < 3; ++i) {
        std::size_t idx = it->source_index(i);
        std::cout << " (" << input[idx].first << ", " << input[idx].second << ")";
      }
      std::cout << " ]" << std::endl;
    }
  }
}