#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

#include <pcl/PointIndices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/recognition/linemod.h>
#include <pcl/recognition/color_gradient_modality.h>
#include <pcl/recognition/surface_normal_modality.h>

#include <pcl/point_types.h>

#include <iostream>
#include <sstream>

#include <boost/any.hpp>
#include <boost/program_options.hpp>

#include <opencv2/opencv.hpp>
#include <math.h>
#include <algorithm>

namespace po = boost::program_options;
namespace fs = boost::filesystem;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

void
trainTemplate (const PointCloud::ConstPtr & input,  cv::Mat_<uint8_t> &foreground_mask,
               pcl::LINEMOD & linemod)
{

    std::cout << "Processing color modality" << std::endl;
  pcl::ColorGradientModality<pcl::PointXYZRGB> color_grad_mod;
  color_grad_mod.setInputCloud (input);
  color_grad_mod.processInputData ();

  std::cout << "processing normal modality" << std::endl;

  pcl::SurfaceNormalModality<pcl::PointXYZRGB> surface_norm_mod;
  surface_norm_mod.setInputCloud (input);
  surface_norm_mod.processInputData ();

  std::cout << "processing quantizable" << std::endl;

  std::vector<pcl::QuantizableModality*> modalities (2);
  modalities[0] = &color_grad_mod;
  modalities[1] = &surface_norm_mod;

  size_t min_x (input->width), min_y (input->height), max_x (0), max_y (0);
  pcl::MaskMap mask_map (input->width, input->height);
  int nCols = foreground_mask.cols;
  int nRows = foreground_mask.rows;
  uint8_t* p;
  std::cout << "nRows " << nRows << " nCols" << nCols << std::endl;
  int counter = 0;
  for(size_t i = 0; i < nRows; ++i){
      p = foreground_mask.ptr<uint8_t>(i);
      for (size_t j = 0; j < nCols; ++j){
          mask_map (j,i) = (((int) p[j])==255);
          if(((int) p[j])==255){
            min_x = std::min (min_x, j);
            max_x = std::max (max_x, j);
            min_y = std::min (min_y, i);
            max_y = std::max (max_y, i);
          }
      }
  }

  std::cout << "finished with masks process" << std::endl;

  std::vector<pcl::MaskMap*> masks (2);
  std::cout << "variable declaration " << std::endl;
  masks[0] = &mask_map;
  masks[1] = &mask_map;

  std::cout << "processing region" << std   ::endl;
  pcl::RegionXY region;
  region.x = static_cast<int> (min_x);
  region.y = static_cast<int> (min_y);
  region.width = static_cast<int> (max_x - min_x + 1);
  region.height = static_cast<int> (max_y - min_y + 1);

  printf ("%d %d %d %d\n", region.x, region.y, region.width, region.height);

  linemod.createAndAddTemplate (modalities, masks, region);
}

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

        std::string pathStr = ".";
        typedef pcl::PointXYZRGB PointT;
        std::string mask = "";
        int valid_model = 0;

        po::options_description desc("Single-View Object Instance Recognizer\n======================================\n**Allowed options");
        desc.add_options()
            ("help,h", "produce help message")
            ("mask,m", po::value<std::string>(&mask), "Mask input cloud with given class")
            ("path,p",po::value(&pathStr)->required(),"Path to operate on")
       ;

        std::cout << "Flag 1" << std::endl;
        po::variables_map vm;
        po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).allow_unregistered().run();
        po::store(parsed, vm);
        if (vm.count("help")) { std::cout << desc << std::endl; }
        try { po::notify(vm); }
        catch(std::exception& e) { std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;  }

        // Gather list of pcd
        std::vector<fs::path> filesPath;
        {
            auto it = fs::recursive_directory_iterator(pathStr);
            auto end = fs::recursive_directory_iterator();

            for(; it != end; ++it)
            {
                fs::path path = *it;
                if(fs::is_directory(path) && fs::exists(path / "frame.pcd"))
                    filesPath.push_back(fs::canonical(path));
            }
        }
        std::string maskFilename = "mask_"+mask+".png";

        pcl::LINEMOD linemod;
        int counter = 0;

        for (int i = 0; i < filesPath.size(); i++){
            std::cout << "Recognizing file " << filesPath[i].string() << std::endl;
            PointCloud::Ptr input(new PointCloud);
            PointCloud::Ptr whole_cloud(new PointCloud);

            pcl::io::loadPCDFile((filesPath[i] / "frame.pcd").string(), *input);
            pcl::copyPointCloud(*input, *whole_cloud);
            if(!mask.empty()){
                if(!fs::is_directory(filesPath[i]) || !fs::exists(filesPath[i] / maskFilename)){
                    std::cout << "Class " << mask << " is not present " << std::endl;
                    continue;
                }

                cv::Mat_<uint8_t> mask_box = cv::imread((filesPath[i] / maskFilename).string(), CV_LOAD_IMAGE_GRAYSCALE);
                std::cout << "Read " << (filesPath[i] / maskFilename).string() << std::endl;
                std::cout << "Output has " << input->points.size () << " points." << std::endl;
                pcl::PointIndices indices;
                int nCols = mask_box.cols;
                int nRows = mask_box.rows;
                uint8_t* p;
                std::cout << "nRows " << nRows << " nCols" << nCols << std::endl;
                int counter = 0;
                for(int i = 0; i < nRows; ++i){
                    p = mask_box.ptr<uint8_t>(i);
                    for (int j = 0; j < nCols; ++j){
                        if(((int) p[j])==255){
                            indices.indices.push_back(i*nCols + j);
                            counter++;
                        }
                    }
                }

                pcl::ExtractIndices<PointT> extract_indices;
                extract_indices.setIndices (boost::make_shared<const pcl::PointIndices> (indices));
                extract_indices.setInputCloud (input);
                extract_indices.setKeepOrganized (true);
                extract_indices.filterDirectly(input);
                std::cout << "Output has " << input->points.size () << " points." << std::endl;
                std::cout << "indices size " << indices.indices.size()<< " vs counter" << counter << std::endl;
                pcl::io::savePCDFile((filesPath[i] / ("frame_" + mask +"_ " + std::to_string(valid_model) +"_template.pcd")).string(), *input);
                trainTemplate (whole_cloud, mask_box, linemod);
                std::cout << "finished" << std::endl;

                // Save the LINEMOD template
                std::ofstream file_stream;
                std::string template_sqmmt_filename = (filesPath[i] / ("frame_" + mask +"_template.sqmmt")).string();
                file_stream.open (template_sqmmt_filename.c_str (), std::ofstream::out | std::ofstream::binary);
                linemod.getTemplate (valid_model).serialize (file_stream);
                file_stream.close ();
                valid_model++;

            }

        }

        std::cout << "writing all tamples to " << pathStr <<"/" <<mask <<"_template.lmt"<< std::endl;
        linemod.saveTemplates((pathStr + "/" + mask + "_template.lmt").c_str());
        return 0;
    }


#include <boost/units/systems/si/plane_angle.hpp>


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|

#ifndef LIMBO_TOOLS_RANDOM_GENERATOR_HPP
#define LIMBO_TOOLS_RANDOM_GENERATOR_HPP

#include <Eigen/Core>

#include <cmath>
#include <cstdlib>
#include <ctime>
#include <external/rand_utils.hpp>
#include <list>
#include <mutex>
#include <random>
#include <stdlib.h>
#include <utility>

namespace limbo {
    namespace tools {
        /// @ingroup tools
        /// a mt19937-based random generator (mutex-protected)
        ///
        /// usage :
        /// - RandomGenerator<dist<double>>(0.0, 1.0);
        /// - double r = rgen.rand();
        template <typename D>
        class RandomGenerator {
        public:
            using result_type = typename D::result_type;
            RandomGenerator(result_type a, result_type b, int seed = -1) : _dist(a, b) { this->seed(seed); }

            result_type rand() { return _dist(_rgen); }

            void seed(int seed = -1)
            {
                if (seed >= 0)
                    _rgen.seed(seed);
                else
                    _rgen.seed(randutils::auto_seed_128{}.base());
            }

            void reset() { _dist.reset(); }

            void param(const typename D::param_type& param) { _dist.param(param); }

        private:
            D _dist;
            std::mt19937 _rgen;
        };

        /// @ingroup tools
        using rdist_double_t = std::uniform_real_distribution<double>;
        /// @ingroup tools
        using rdist_int_t = std::uniform_int_distribution<int>;
        /// @ingroup tools
        using rdist_gauss_t = std::normal_distribution<>;

        /// @ingroup tools
        /// Double random number generator
        using rgen_double_t = RandomGenerator<rdist_double_t>;

        /// @ingroup tools
        /// Double random number generator (gaussian)
        using rgen_gauss_t = RandomGenerator<rdist_gauss_t>;

        /// @ingroup tools
        /// integer random number generator
        using rgen_int_t = RandomGenerator<rdist_int_t>;

        /// @ingroup tools
        /// random vector by providing custom RandomGenerator
        template <typename Rng>
        inline Eigen::VectorXd random_vec(int size, Rng& rng)
        {
            Eigen::VectorXd res(size);
            for (int i = 0; i < size; ++i)
                res[i] = rng.rand();
            return res;
        }

        /// @ingroup tools
        /// random vector in [0, 1]
        ///
        /// - this function is thread safe because we use a random generator for each thread
        /// - we use a C++11 random number generator
        inline Eigen::VectorXd random_vector_bounded(int size)
        {
            static thread_local rgen_double_t rgen(0.0, 1.0);
            return random_vec(size, rgen);
        }

        /// @ingroup tools
        /// random vector generated with a normal distribution centered on 0, with standard deviation of 10
        ///
        /// - this function is thread safe because we use a random generator for each thread
        /// - we use a C++11 random number generator
        inline Eigen::VectorXd random_vector_unbounded(int size)
        {
            static thread_local rgen_gauss_t rgen(0.0, 10.0);
            return random_vec(size, rgen);
        }

        /// @ingroup tools
        /// random vector wrapper for both bounded and unbounded versions
        inline Eigen::VectorXd random_vector(int size, bool bounded = true)
        {
            if (bounded)
                return random_vector_bounded(size);
            return random_vector_unbounded(size);
        }

        /// @ingroup tools
        /// generate n random samples with Latin Hypercube Sampling (LHS) in [0, 1]^dim
        inline Eigen::MatrixXd random_lhs(int dim, int n)
        {
            Eigen::VectorXd cut = Eigen::VectorXd::LinSpaced(n + 1, 0., 1.);
            Eigen::MatrixXd u = Eigen::MatrixXd::Zero(n, dim);

            for (int i = 0; i < n; i++) {
                u.row(i) = tools::random_vector(dim, true);
            }

            Eigen::VectorXd a = cut.head(n);
            Eigen::VectorXd b = cut.tail(n);

            Eigen::MatrixXd rdpoints = Eigen::MatrixXd::Zero(n, dim);
            for (int i = 0; i < dim; i++) {
                rdpoints.col(i) = u.col(i).array() * (b - a).array() + a.array();
            }

            Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, dim);
            Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> perm(n);
            static thread_local std::mt19937 rgen(randutils::auto_seed_128{}.base());
            for (int i = 0; i < dim; i++) {
                perm.setIdentity();
                std::shuffle(perm.indices().data(), perm.indices().data() + perm.indices().size(), rgen);
                Eigen::MatrixXd tmp = perm * rdpoints;
                H.col(i) = tmp.col(i);
            }

            return H;
        }
    } // namespace tools
} // namespace limbo

#endif


/******************************************************************************
 * Description: Gadget using the Berkeley Advanced Reconstruction Toolbox (BART)
 * Authors:
 *   Mahamadou Diakite, PhD. [1]
 *   Nguyen Damien, PhD. [2]
 *   Francesco Santini, PhD. [2]
 * Institutions:
 *   [1] National Institutes of Health (NIH)
 *   [2] University of Basel, Switzerland
 * Lang: C++
 * Date: 08/10/2018
 * Version: 1.5.0
 ******************************************************************************/

#include "bartgadget.h"
#include <boost/tokenizer.hpp>
#include <sstream>
#include <utility>
#include <numeric>
#include <cstdlib>
#include <ctime>
#include <chrono>
#include <memory>
#include <functional>
#include <mutex>

#include <cerrno>
#ifdef _WIN32
    #include <direct.h>
    #define getcwd _getcwd
    #define chdir _chdir
#else
    #include <unistd.h>
#endif

#include "bart_helpers.h"

enum bart_debug_levels { BART_DP_ERROR, BART_DP_WARN, BART_DP_INFO, BART_DP_DEBUG1, BART_DP_DEBUG2, BART_DP_DEBUG3, BART_DP_DEBUG4, BART_DP_TRACE, BART_DP_ALL };

// =============================================================================

std::vector<size_t> Gadgetron::read_BART_hdr(fs::path &filename)
{
     return read_BART_hdr<size_t>(filename);
}

std::pair<std::vector<size_t>, std::vector<std::complex<float>>>
Gadgetron::read_BART_files(fs::path &filename)
{
     return read_BART_files<size_t>(filename);
}

// =============================================================================

namespace Gadgetron {

     BartGadget::BartGadget() :
	  dp{}
     {}

     void BartGadget::replace_default_parameters(std::string & str)
     {
	  std::string::size_type pos = 0u;
	  while ((pos = str.find('$', pos)) != std::string::npos)
	  {
	       auto pos_end = str.find(' ', pos);
	       auto pos_diff = pos_end - pos;
	       std::string tmp = str.substr(pos, pos_diff);
	       tmp.erase(0, 1);
	       if (tmp == std::string("recon_matrix_x"))
		    str.replace(pos, pos_diff, std::to_string(dp.recon_matrix_x));
	       else if (tmp == std::string("recon_matrix_y"))
		    str.replace(pos, pos_diff, std::to_string(dp.recon_matrix_y));
	       else if (tmp == std::string("recon_matrix_z"))
		    str.replace(pos, pos_diff, std::to_string(dp.recon_matrix_z));
	       else if (tmp == std::string("FOV_x"))
		    str.replace(pos, pos_diff, std::to_string(dp.FOV_x));
	       else if (tmp == std::string("FOV_y"))
		    str.replace(pos, pos_diff, std::to_string(dp.FOV_y));
	       else if (tmp == std::string("FOV_z"))
		    str.replace(pos, pos_diff, std::to_string(dp.FOV_z));
	       else if (tmp == std::string("acc_factor_PE1"))
		    str.replace(pos, pos_diff, std::to_string(dp.acc_factor_PE1));
	       else if (tmp == std::string("acc_factor_PE2"))
		    str.replace(pos, pos_diff, std::to_string(dp.acc_factor_PE2));
	       else if (tmp == std::string("reference_lines_PE1"))
		    str.replace(pos, pos_diff, std::to_string(dp.reference_lines_PE1));
	       else if (tmp == std::string("reference_lines_PE2"))
		    str.replace(pos, pos_diff, std::to_string(dp.reference_lines_PE2));
	       else if (tmp == std::string("input_data"))
		    str.replace(pos, pos_diff, dp.input_data);
	       else if (tmp == std::string("reference_data"))
		    str.replace(pos, pos_diff, dp.reference_data);
	       else if (tmp == std::string("traj_data"))
		    str.replace(pos, pos_diff, dp.traj_data);
	       else {
		    GERROR( "Unknown default parameter, please see the complete list of available parameters...");
	       }
	       pos = pos_end;
	  }
     }

     int BartGadget::process_config(ACE_Message_Block * mb)
     {
	  GADGET_CHECK_RETURN(BaseClass::process_config(mb) == GADGET_OK, GADGET_FAIL);

	  /** Let's get some information about the incoming data **/
	  ISMRMRD::IsmrmrdHeader h;
	  try {
	       deserialize(mb->rd_ptr(), h);
	  }
	  catch (...) {
	       GDEBUG("BartGadget::process_config: Failed to parse incoming ISMRMRD Header");
	  }

	  // ===================================================================
	  /* Data provided to the user might or might not be in-memory
	   *
	   * - if -DMEMONLY_CFL then we're always in-memory without extension
	   * - otherwise we add the *.mem extension if
	   *     + if the memory behaviour is BART_ALL_IN_MEM
	   *     + or if the memory behaviour is BART_MIX_DISK_MEM and the user requests it
	   */
	  const auto append_mem_ext_in(!memonly_cfl
				       && (memory_behaviour_ == BART_ALL_IN_MEM
					   || (memory_behaviour_ == BART_MIX_DISK_MEM
					       && BartStoreGadgetronInputInMemory.value())));

	  dp.reference_data  = std::string("reference_data") + (append_mem_ext_in ? ".mem" : "");
	  dp.input_data = std::string("input_data")     + (append_mem_ext_in ? ".mem" : "");
	  dp.traj_data = std::string("traj_data")      + (append_mem_ext_in ? ".mem" : "");

	  // ===================================================================
	  // Adjust BART's debug level

	  GDEBUG_STREAM("BartGadget::process_config: setting BART debug level to "
			<< BartPrintfDebugLevel.value());

	  auto& bart_debug_level(debug_level);
	  if (BartPrintfDebugLevel.value() == "DP_ERROR") {
	       bart_debug_level = BART_DP_ERROR;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_WARN") {
	       bart_debug_level = BART_DP_WARN;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_INFO") {
	       bart_debug_level = BART_DP_INFO;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_DEBUG1") {
	       bart_debug_level = BART_DP_DEBUG1;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_DEBUG2") {
	       bart_debug_level = BART_DP_DEBUG2;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_DEBUG3") {
	       bart_debug_level = BART_DP_DEBUG3;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_DEBUG4") {
	       bart_debug_level = BART_DP_DEBUG4;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_TRACE") {
	       bart_debug_level = BART_DP_TRACE;
	  }
	  else if (BartPrintfDebugLevel.value() == "DP_ALL") {
	       bart_debug_level = BART_DP_ALL;
	  }
	  else {
	       GWARN("Unknown debug level! defaulting to BART_DP_INFO");
	       bart_debug_level = BART_DP_INFO;
	  }

	  // ===================================================================
	  // Check status of bart commands script

	  if (AbsoluteBartCommandScript_path.value().empty())
	      AbsoluteBartCommandScript_path.value(context.paths.gadgetron_home.string() + "/share/gadgetron/bart");

	  command_script_ = AbsoluteBartCommandScript_path.value();
	  command_script_ /= BartCommandScript_name.value();

	  if (!fs::exists(command_script_)) {
	       GERROR("Can't find bart commands script: %s!\n", command_script_.c_str());
	       return GADGET_FAIL;
	  }

	  // ===================================================================

	  memory_behaviour_ = BART_MIX_DISK_MEM;
	  if (BartFileBehaviour.value() == "BART_ALL_IN_MEM") {
	       memory_behaviour_ = BART_ALL_IN_MEM;
	  }
	  else if (BartFileBehaviour.value() == "BART_MIX_DISK_MEM") {
	       memory_behaviour_ = BART_MIX_DISK_MEM;
	  }
	  else {
	       GERROR_STREAM("Invalid value specified for BartFileBehaviour: " << BartFileBehaviour.value());
	       return GADGET_FAIL;
	  }

	  // ===================================================================

	  for (const auto& enc: h.encoding) {
	       auto recon_space = enc.reconSpace;

	       GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Encoding matrix size: " << recon_space.matrixSize.x << " " << recon_space.matrixSize.y << " " << recon_space.matrixSize.z);
	       GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Encoding field_of_view : " << recon_space.fieldOfView_mm.x << " " << recon_space.fieldOfView_mm.y << " " << recon_space.fieldOfView_mm.z);
	       dp.recon_matrix_x = recon_space.matrixSize.x;
	       dp.recon_matrix_y = recon_space.matrixSize.y;
	       dp.recon_matrix_z = recon_space.matrixSize.z;
	       dp.FOV_x = static_cast<uint16_t>(recon_space.fieldOfView_mm.x);
	       dp.FOV_y = static_cast<uint16_t>(recon_space.fieldOfView_mm.y);
	       dp.FOV_z = static_cast<uint16_t>(recon_space.fieldOfView_mm.z);

	       if (!enc.parallelImaging)
	       {
		    GDEBUG_STREAM("BartGadget::process_config: Parallel Imaging not enable...");
	       }
	       else
	       {
		    auto p_imaging = *h.encoding.front().parallelImaging;
		    GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: acceleration Factor along PE1 is " << p_imaging.accelerationFactor.kspace_encoding_step_1);
		    GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: acceleration Factor along PE2 is " << p_imaging.accelerationFactor.kspace_encoding_step_2);
		    dp.acc_factor_PE1 = p_imaging.accelerationFactor.kspace_encoding_step_1;
		    dp.acc_factor_PE2 = p_imaging.accelerationFactor.kspace_encoding_step_2;

		    if (p_imaging.accelerationFactor.kspace_encoding_step_2 > 1) {
			 GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Limits of the size of the calibration region (PE1) " << h.userParameters->userParameterLong[0].name << " is " << h.userParameters->userParameterLong[0].value);
			 GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Limits of the size of the calibration region (PE2) " << h.userParameters->userParameterLong[1].name << " is " << h.userParameters->userParameterLong[1].value);
			 dp.reference_lines_PE1 = h.userParameters->userParameterLong[0].value;
			 dp.reference_lines_PE2 = h.userParameters->userParameterLong[1].value;
		    }
		    else if (p_imaging.accelerationFactor.kspace_encoding_step_1 > 1) {
			 GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Limits of the size of the calibration region (PE1) " << h.userParameters->userParameterLong[0].name << " is " << h.userParameters->userParameterLong[0].value);
			 dp.reference_lines_PE1 = h.userParameters->userParameterLong[0].value;
		    }

		    auto calib = *p_imaging.calibrationMode;
            auto separate = (calib == std::string("separate"));
            auto embedded = (calib == std::string("embedded"));
            auto external = (calib == std::string("external"));
            auto interleaved = (calib == std::string("interleaved"));
            auto other = (calib == std::string("other"));

		    if (p_imaging.accelerationFactor.kspace_encoding_step_1 > 1 || p_imaging.accelerationFactor.kspace_encoding_step_2 > 1)
		    {
			 if (interleaved) {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Calibration mode INTERLEAVE ");
			 }
			 else if (embedded) {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Calibration mode EMBEDDED");
			 }
			 else if (separate) {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Calibration mode SEPARATE");
			 }
			 else if (external) {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Calibration mode EXTERNAL");
			 }
			 else if (other) {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Calibration mode OTHER");
			 }
			 else {
			      GDEBUG_CONDITION_STREAM(isVerboseON.value(), "BartGadget::process_config: Something went terribly wrong, this should never happen!");
			      return GADGET_FAIL;
			 }
		    }

	       }

	  }
	  return GADGET_OK;
     }

     int BartGadget::process(GadgetContainerMessage<IsmrmrdReconData>* m1)
     {
	  static std::mutex mtx;
	  std::lock_guard<std::mutex> guard(mtx);

	  constexpr auto memonly_cfl =
#ifdef MEMONLY_CFL
	       true
#else
	       false
#endif /* MEMONLY_CFL */
	       ;

	  GINFO_STREAM("Process start");

	  auto generated_files_folder(internal::generate_unique_folder(BartWorkingDirectory_path.value()));

	  bool create_folder(!memonly_cfl
			     || memory_behaviour_ != BART_ALL_IN_MEM);
	  if (create_folder) {
	       if (!(fs::exists(generated_files_folder)
		     && fs::is_directory(generated_files_folder))) {
		    if (fs::create_directories(generated_files_folder)){
			 GDEBUG_STREAM("Folder to store *.hdr & *.cfl files is " << generated_files_folder);
		    }
		    else {
			 GERROR("Folder to store *.hdr & *.cfl files doesn't exist...\n");
			 return GADGET_FAIL;
		    }
	       }
	  }

	  internal::ScopeGuard cleanup_guard(generated_files_folder);
	  if (!create_folder) {
	       cleanup_guard.dismiss();
	  }

	  // Gadgetron data is always in-memory
	  const auto ref_filename_src  = std::string("meas_gadgetron_ref")   + (!memonly_cfl ? ".mem" : "");
	  const auto data_filename_src = std::string("meas_gadgetron_input") + (!memonly_cfl ? ".mem" : "");
	  const auto traj_filename_src = std::string("meas_gadgetron_traj")  + (!memonly_cfl ? ".mem" : "");

	  /*** PROCESS EACH DATASET ***/

	  auto it(0UL);
	  for(auto& recon_bit: m1->getObjectPtr()->rbit_) {

	       bool has_traj(recon_bit.data_.trajectory_);

	       // Grab a reference to the buffer containing the reference data
	       auto& input_ref = (*recon_bit.ref_).data_;
	       // Data 7D, fixed order [E0, E1, E2, CHA, N, S, LOC]
	       std::vector<long> DIMS_ref{static_cast<long>(input_ref.get_size(0)),
					  static_cast<long>(input_ref.get_size(1)),
					  static_cast<long>(input_ref.get_size(2)),
					  static_cast<long>(input_ref.get_size(3)),
					  static_cast<long>(input_ref.get_size(4)),
					  static_cast<long>(input_ref.get_size(5)),
					  static_cast<long>(input_ref.get_size(6))};

	       // Grab a reference to the buffer containing the image data
	       auto& input = recon_bit.data_.data_;
	       // Data 7D, fixed order [E0, E1, E2, CHA, N, S, LOC]
	       std::vector<long> DIMS{static_cast<long>(input.get_size(0)),
				      static_cast<long>(input.get_size(1)),
				      static_cast<long>(input.get_size(2)),
				      static_cast<long>(input.get_size(3)),
				      static_cast<long>(input.get_size(4)),
				      static_cast<long>(input.get_size(5)),
				      static_cast<long>(input.get_size(6))};

	       // Grab a reference to the buffer containing the image trajectory data (if present)
	       std::unique_ptr<hoNDArray<std::complex<float>>> traj(nullptr);
	       if (has_traj) {
		    auto& traj_real = *recon_bit.data_.trajectory_;
		    traj = std::make_unique<hoNDArray<std::complex<float>>>(traj_real.get_dimensions());
		    // Data 7D, fixed order [E0, E1, E2, CHA, N, S, LOC]
		    std::vector<long> DIMS_traj{static_cast<long>(traj_real.get_size(0)),
						static_cast<long>(traj_real.get_size(1)),
						static_cast<long>(traj_real.get_size(2)),
						static_cast<long>(traj_real.get_size(3)),
						static_cast<long>(traj_real.get_size(4)),
						static_cast<long>(traj_real.get_size(5)),
						static_cast<long>(traj_real.get_size(6))};
		    std::transform(traj_real.begin(), traj_real.end(),
				   traj->begin(),
				   [] (float r) { return std::complex<float>(r, 0.); });

		    register_mem_cfl_non_managed(traj_filename_src.c_str(), DIMS_traj.size(), &DIMS_traj[0], &(*traj)[0]);
	       }

	       /* The reference data will be pointing to the image data if there is
		  no reference scan. Therefore, we won't write the reference data
		  into files if it's pointing to the raw data.*/
	       if (DIMS_ref != DIMS)
	       {
		    register_mem_cfl_non_managed(ref_filename_src.c_str(), DIMS_ref.size(), &DIMS_ref[0], &input_ref[0]);
	       }

	       register_mem_cfl_non_managed(data_filename_src.c_str(), DIMS.size(), &DIMS[0], &input[0]);

	       if (DIMS_ref != DIMS)
	       {
		    std::ostringstream cmd;
		    cmd << "bart resize -c 0 " << DIMS[0] << " 1 " << DIMS[1] << " 2 " << DIMS[2]
			<< " " << ref_filename_src << " " << dp.reference_data;

		    GDEBUG_STREAM("Gadgetron data is loaded to " << ref_filename_src);
		    GDEBUG_STREAM("BART filename for reference data is " << dp.reference_data);

		    if (!call_BART(cmd.str()))
		    {
			 return GADGET_FAIL;
		    }
	       }

	       std::ostringstream cmd2;
	       if (DIMS[4] != 1)
		    cmd2 << "bart reshape 1023 " << DIMS[0] << " " << DIMS[1] << " " << DIMS[2] << " " << DIMS[3] << " 1 1 1 " << DIMS[5] << " " << DIMS[6] << " " << DIMS[4];
	       else
		    cmd2 << "bart copy";

	       cmd2 << " " << data_filename_src << " " << dp.input_data;

	       GDEBUG_STREAM("Gadgetron data is loaded to " << data_filename_src);
	       GDEBUG_STREAM("BART filename for data is " << dp.input_data);

	       if (!call_BART(cmd2.str()))
	       {
		    return GADGET_FAIL;
	       }

	       if (has_traj) {
		    std::ostringstream cmd3;
		    if (DIMS[4] != 1)
			 cmd3 << "bart reshape 1023 " << DIMS[0] << " " << DIMS[1] << " " << DIMS[2] << " " << DIMS[3] << " 1 1 1 " << DIMS[5] << " " << DIMS[6] << " " << DIMS[4];
		    else
			 cmd3 << "bart copy";

		    cmd3 << " " << traj_filename_src << " " << dp.traj_data;

		    GDEBUG_STREAM("Gadgetron trajectory is loaded to " << traj_filename_src);
		    GDEBUG_STREAM("BART filename for trajectory is " << dp.traj_data);

		    if (!call_BART(cmd3.str()))
		    {
			 return GADGET_FAIL;
		    }
	       }

	       /*** CALL BART COMMAND LINE from the scripting file ***/
	       GDEBUG("Starting processing user script\n");

	       std::string Commands_Line;
           std::ifstream inputFile(command_script_.string());
           if (inputFile.is_open())
	       {
		    std::string Line;
		    while (getline(inputFile, Line))
		    {
			 // crop comment
             Line = Line.substr(0, Line.find_first_of('#'));

			 internal::trim(Line);
			 if (Line.empty() || Line.compare(0, 4, "bart") != 0)
			      continue;

			 replace_default_parameters(Line);
			 if (!call_BART(Line))
			 {
			      return GADGET_FAIL;
			 }

			 Commands_Line = Line;
		    }
	       }
	       else
	       {
		    GERROR("Unable to open %s\n", command_script_.c_str());
		    return GADGET_FAIL;
	       }

	       // ==============================================================

	       fs::path outputFile(internal::get_output_filename(Commands_Line));
	       GDEBUG_STREAM("Detected last output file: " << outputFile);

	       // Reshaped data is always in-memory
	       fs::path outputFileReshape(outputFile);

	       // Remove the extension from the user's output if needed
	       if (outputFileReshape.extension() == ".mem") {
		    outputFileReshape.replace_extension();
	       }
	       outputFileReshape += std::string("_reshape") + (!memonly_cfl ? ".mem" : "");

	       // Reformat the data back to Gadgetron format
	       std::vector<long> header(16);
	       if (memonly_cfl || outputFile.extension() == ".mem") {
		    load_mem_cfl(outputFile.c_str(), header.size(), header.data());
	       }
	       else {
            auto tmp(generated_files_folder / outputFile);
            header = read_BART_hdr<long>(tmp);
	       }

	       std::ostringstream cmd4;
	       cmd4 << "bart reshape 1023 " << header[0] << " " << header[1] << " " << header[2] << " " << header[3] << " " << header[9] * header[4] <<
		    " " << header[5] << " " << header[6] << " " << header[7] << " " << header[8] << " 1 " << outputFile.c_str() << " " << outputFileReshape.c_str();

	       if (!call_BART(cmd4.str()))
	       {
		    return GADGET_FAIL;
	       }

	       /**** READ FROM BART FILES ***/
	       std::vector<long> DIMS_OUT(16);
	       auto data = reinterpret_cast<std::complex<float>*>(load_mem_cfl(outputFileReshape.c_str(), DIMS_OUT.size(), DIMS_OUT.data()));

           if (data == nullptr) {
		    GERROR("Failed to retrieve data from in-memory CFL file!");
		    return GADGET_FAIL;
	       }

	       if (isBartFileBeingStored.value()) {
		    cleanup_guard.dismiss();
	       }

	       IsmrmrdImageArray imarray;

	       // Grab data from BART files
	       std::vector<size_t> BART_DATA_dims{
		    static_cast<size_t>(std::accumulate(DIMS_OUT.begin(), DIMS_OUT.end(), 1, std::multiplies<size_t>()))};
	       hoNDArray<std::complex<float>> DATA(BART_DATA_dims, data);

	       // The image array data will be [E0,E1,E2,1,N,S,LOC]
	       std::vector<size_t> data_dims(DIMS_OUT.begin(), DIMS_OUT.begin()+7);
	       DATA.reshape(data_dims);

	       // Extract the first image from each time frame (depending on the number of maps generated by the user)
	       std::vector<size_t> data_dims_Final{static_cast<size_t>(DIMS_OUT[0]),
						   static_cast<size_t>(DIMS_OUT[1]),
						   static_cast<size_t>(DIMS_OUT[2]),
						   static_cast<size_t>(DIMS_OUT[3]),
						   static_cast<size_t>(DIMS_OUT[4] / header[4]),
						   static_cast<size_t>(DIMS_OUT[5]),
						   static_cast<size_t>(DIMS_OUT[6])};
	       assert(header[4] > 0);
	       imarray.data_.create(data_dims_Final);

	       std::vector<std::complex<float> > DATA_Final;
	       DATA_Final.reserve(std::accumulate(data_dims_Final.begin(), data_dims_Final.end(), 1, std::multiplies<size_t>()));

	       //Each chunk will be [E0,E1,E2,CHA] big
	       std::vector<size_t> chunk_dims{data_dims_Final[0], data_dims_Final[1], data_dims_Final[2], data_dims_Final[3]};
	       const std::vector<size_t> Temp_one_1d(1, chunk_dims[0] * chunk_dims[1] * chunk_dims[2] * chunk_dims[3]);

	       for (uint16_t loc = 0; loc < data_dims[6]; ++loc) {
		    for (uint16_t s = 0; s < data_dims[5]; ++s) {
			 for (uint16_t n = 0; n < data_dims[4]; n += header[4]) {
			      //Grab a wrapper around the relevant chunk of data [E0,E1,E2,CHA] for this loc, n, and s
			      auto chunk = hoNDArray<std::complex<float> >(chunk_dims, &DATA(0, 0, 0, 0, n, s, loc));
			      chunk.reshape(Temp_one_1d);
			      DATA_Final.insert(DATA_Final.end(), chunk.begin(), chunk.end());
			 }
		    }
	       }

	       std::copy(DATA_Final.begin(), DATA_Final.end(), imarray.data_.begin());

	       compute_image_header(recon_bit, imarray, it);
	       send_out_image_array(imarray, it, image_series.value() + (static_cast<int>(it) + 1), GADGETRON_IMAGE_REGULAR);
	       ++it;
	  }

	  m1->release();
	  return GADGET_OK;
     }

     bool call_BART(const std::string &cmdline)
     {
	  GINFO_STREAM("Executing BART command: " << cmdline);
	  enum { MAX_ARGS = 256 };

	  int argc(0);
	  char* argv[MAX_ARGS];

	  auto cmdline_s = std::make_unique<char[]>(cmdline.size()+1);
	  strcpy(cmdline_s.get(), cmdline.c_str());

	  char *p2 = strtok(cmdline_s.get(), " ");
	  while (p2 && argc < MAX_ARGS-1)
	  {
	       argv[argc++] = p2;
           p2 = strtok(nullptr, " ");
	  }
	  argv[argc] = nullptr;

	  // boost::char_separator<char> sep(" ");
	  // boost::tokenizer<boost::char_separator<char>> tokens(cmdline.begin(),
	  // 						       cmdline.end(),
	  // 						       sep);
	  // std::vector<std::unique_ptr<char[]>> tmp;
	  // auto k(0UL);
	  // for (auto tok: tokens) {
	  //      tmp.push_back(std::make_unique<char[]>(tok.size()+1));
	  //      strcpy(tmp.back().get(), tok.c_str());
	  //      argv[k++] = tmp.back().get();
	  // }
	  // argc = tmp.size();

	  char out_str[512] = {'\0'};
	  auto ret(bart_command(512, out_str, argc, argv));
	  if (ret == 0) {
	       if (strlen(out_str) > 0) {
		    GINFO(out_str);
	       }
	       return true;
	  }
     // else {
	       GERROR_STREAM("BART command failed with return code: " << ret);
	       return false;
     // }
     }

     GADGET_FACTORY_DECLARE(BartGadget)
}


// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/detail/config.hpp>

#if BOOST_MP11_WORKAROUND( BOOST_MP11_GCC, < 40800 )
# pragma GCC diagnostic ignored "-Wsign-compare"
#endif

#include <boost/mp11/function.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

int main()
{
    using boost::mp11::mp_less;
    using boost::mp11::mp_true;
    using boost::mp11::mp_false;
    using boost::mp11::mp_int;
    using boost::mp11::mp_size_t;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<0>, mp_int<-1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<0>, mp_int<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<0>, mp_int<1>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<0>, mp_int<2>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<-1>, mp_int<-2>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<-1>, mp_int<-1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<-1>, mp_int<0>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<-1>, mp_int<1>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<1>, mp_int<-1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<1>, mp_int<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<1>, mp_int<1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<1>, mp_int<2>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_size_t<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_size_t<1>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_size_t<2>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_size_t<3>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<1>, mp_size_t<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<1>, mp_size_t<1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<1>, mp_size_t<2>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<1>, mp_size_t<3>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<2>, mp_size_t<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<2>, mp_size_t<1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<2>, mp_size_t<2>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<2>, mp_size_t<3>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_int<-1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_int<0>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_size_t<0>, mp_int<1>>, mp_true>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<-1>, mp_size_t<1>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<0>, mp_size_t<1>>, mp_true>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<1>, mp_size_t<1>>, mp_false>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_less<mp_int<2>, mp_size_t<1>>, mp_false>));

    return boost::report_errors();
}


/*
parse.cpp

Created by Samuel Comeau
*/
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <stack>
#include <boost/regex.hpp>
#include <stdlib.h>
#include "parse.h"
#include "path.h"
#include "hlsm.h"
std::stack<int> in_if;
std::stack<bool> is_else;
using namespace std;
#define TEN_WORD_REGEX "(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?"\
                   "(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?"\
                   "(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?(?:,\\W+(\\w+))?"\
                   "(?:,\\W+(\\w+))?"
#define HUNDRED_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX TEN_WORD_REGEX

/* parse_line
*/
std::string parse_line(string line, Hlsm * h){
    Graph * list = &h->graph;
    int data_width;
    bool data_signed=0;
    comp_t component=ERR;
    //Check for data types
    boost::regex data_regex("(input|output|variable|wire)"
                           "\\W+?(Int|UInt)(\\d+)\\W+(\\w+)"
                           HUNDRED_WORD_REGEX );
    std::string::const_iterator start;
    std::string::const_iterator end;
    start = line.begin();
    end = line.end();
    boost::match_results<std::string::const_iterator> matches;
    boost::match_results<std::string::const_iterator> tmatches;
    boost::match_flag_type flags = boost::match_default;
    if(boost::regex_search(start, end, matches, data_regex, flags)){
        if(matches[1]=="input") component=INPUT;
        if(matches[1]=="output") component=OUTPUT;
        if(matches[1]=="wire") component=VAR;
        if(matches[1]=="variable") component=VAR;

        if(matches[2]=="Int") data_signed=1;
        if (matches[2] == "UInt") data_signed = 0;
        data_width = stoi(matches[3]);
        if (data_width == 1) data_signed = 0;

        for (size_t i = 4; i < matches.size(); ++i){
            if(matches[i]!=""){
                add_data(component, matches[i], data_width, data_signed, list);
                if(!in_if.empty()) {
                    fprintf(stderr,"ERROR:Variable declaration inside if-then-else stucture not allowed\n");
                    exit(EXIT_FAILURE);
                }
            }
        }
   }
   //Check for binary operators
   boost::regex biop_regex("(\\w+)\\s*=\\s*(\\w+)\\s*"
                       "([^a-zA-Z0-9_ \\t\\n\\r\\f][=><]?)\\s*(\\w+)\\s*");
   if(boost::regex_search(start, end, matches, biop_regex, flags)){
        component=ERR;
        #ifdef DEBUG
            std::cout << "sign is " << matches[3] << std::endl;
        #endif
        if(matches[3]=="+"){
            if(matches[4]=="1") component=INC;
            else component=ADD;
        }
        if(matches[3]=="-") {
            if(matches[4]=="1") component=DEC;
            else component=SUB;
        }
        if(matches[3]=="*") component=MUL;
        if(matches[3]=="/") component=DIV;
        if(matches[3]=="%") component=MOD;
        if(matches[3]=="<") component=COMPLT;
        if(matches[3]=="<<") component=SHL;
        if(matches[3]==">") component=COMPGT;
        if(matches[3]==">>") component=SHR;
        if(matches[3]=="==") component=COMPEQ;
        //Check for ternary operator
        boost::regex triop_regex("(\\w+)\\s*=\\s*(\\w+)\\s*"
                               "\\?\\s*(\\w+)\\s*:\\s*(\\w+)\\s*");
        if(boost::regex_search(start, end, tmatches, triop_regex, flags)){
            add_mux(tmatches[2],tmatches[3],tmatches[4],tmatches[1], list);
        } else {
            if(component!=ERR) {
                node_t * new_node = add_op(component, matches[2], matches[4], matches[1], list);
                if(!in_if.empty()){
                    if(!is_else.top()){
                        h->conditions[in_if.top()].then_members.push_back(new_node);
                    } else {
                        h->conditions[in_if.top()].else_members.push_back(new_node);
                    }
                }
            }
            else{
                fprintf(stderr, "ERROR:Unknown operator\n");
                exit(EXIT_FAILURE);
            }
        }
   }
   //Check for assignment
   boost::regex noop_regex("(\\w+)\\s*=\\s*(\\w+)\\s*$");
   if(boost::regex_search(start, end, matches, noop_regex, flags)){
        add_assignment(matches[1], matches[2], list);
   }
   boost::regex if_regex("\\s*if\\s*\\(\\s*(\\w+)\\s*\\)\\s*\\{");
   if(boost::regex_search(start, end, matches, if_regex, flags)){
        //fprintf(stderr,"I didn't implement this, it seemed hard\n");
        //exit(EXIT_FAILURE);
        in_if.push(add_if(matches[1], h));
        is_else.push(false);
   }
   boost::regex end_if_regex("\\}");
   if(boost::regex_search(start, end, matches, end_if_regex, flags)){
        in_if.pop();
        is_else.pop();
   }
   boost::regex else_regex("\\s*else\\s*\\{");
   if(boost::regex_search(start, end, matches, else_regex, flags)){
        in_if.push(h->conditions.size()-1);
        is_else.push(true);
   }
   return line;
}

/* close
    appropriately closes an open file
*/
void close(ifstream * file){
    file->close();
}

/* parse
    implements other functions to return a completed netlist type
*/
void parse(std::string filename, Hlsm * h){
    ifstream netlist(filename, std::ios::in);
    if(!netlist.is_open()){
        perror("Could not open input file");
        exit(EXIT_FAILURE);
    }
    std::string out;
    while(!in_if.empty()) in_if.pop();
    while(!is_else.empty()) is_else.pop();
    while(!netlist.eof()){
        std::string nextline;
        getline(netlist, nextline);
        out = parse_line(nextline, h);
        #ifdef DEBUG
            std::cout << out << std::endl;
        #endif
    };
    if(h->graph.nodes.size()==0){
        fprintf(stderr,"ERROR:Could not parse any nodes.\n");
        exit(EXIT_FAILURE);
    }
    close(&netlist);
    //Close any unterminated edges
    for(edge_t * edge : h->graph.edges){
        if(edge->to == NULL) edge->to = &h->graph.onop;
    }
}


#include <CGAL/boost/graph/copy_face_graph.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <CGAL/boost/graph/named_params_helper.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/use.h>
#include "test_Prefix.h"

#include <boost/numeric/conversion/cast.hpp>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>

#include <fstream>
#include <map>
#include <memory>
#include <utility>

typedef boost::unordered_set<std::size_t> id_map;

namespace PMP = CGAL::Polygon_mesh_processing;

template <typename Graph>
void test_halfedge_around_vertex_iterator(const  Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  boost::unordered_map<g_face_descriptor, std::size_t> map(num_faces(g));
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());

  Adapter fg(g, 0, boost::make_assoc_property_map(map));
  typename boost::graph_traits<Adapter >::vertex_iterator vit, vend;
  for(boost::tie(vit, vend) = vertices(fg); vit != vend; ++vit) {
    halfedge_around_target_iterator havit, havend;
    for(boost::tie(havit, havend) = CGAL::halfedges_around_target(halfedge(*vit, fg), fg);
        havit != havend; ++havit) {
      assert(target(*havit, fg) == *vit);

      // check if we are really moving clockwise
      halfedge_around_target_iterator step = boost::next(havit);
      if(step != havend) {
        halfedge_descriptor stepd = *step;
        assert(stepd == opposite(next(*havit, fg), fg));
      }
    }
  }
}

template <typename Graph>
void test_halfedge_around_face_iterator(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  face_iterator fit, fend;
  for(boost::tie(fit, fend) = faces(fg); fit != fend; ++fit) {
    halfedge_around_face_iterator hafit, hafend;
    boost::tie(hafit, hafend) = CGAL::halfedges_around_face(halfedge(*fit, fg), fg);
    assert(std::distance(hafit, hafend) != 0);
    for(boost::tie(hafit, hafend) = CGAL::halfedges_around_face(halfedge(*fit, fg), fg); hafit != hafend; ++hafit) {
      assert(face(*hafit, fg) == *fit);
    }
  }
}

template<typename Graph>
void test_edge_iterators(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  // do we iterate as many as that?
  edge_iterator eb, ee;
  boost::tie(eb, ee) = edges(fg);
  assert(boost::numeric_cast<edges_size_type>(std::distance(eb, ee)) == num_edges(g));
  id_map ids;
  unsigned int count = 0;
  for(boost::tie(eb, ee) = edges(fg); eb != ee; ++eb) {
    edge_descriptor e = *eb;
    std::pair<id_map::iterator, bool> r = ids.insert(get(boost::edge_index, g, e));
    // unique?
    assert(r.second);
    ++count;
  }
  assert(count == num_edges(fg));
}

template<typename Graph>
void test_vertex_iterators(Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));
  vertex_iterator vb, ve;
  std::size_t count = 0;
  for(boost::tie(vb, ve) = vertices(fg); vb != ve; ++vb){
    ++count;
  }

  assert(count == num_vertices(fg));

  // check that the iterators reach uniques
  id_map ids;

  count = 0;
  for(boost::tie(vb, ve) = vertices(fg); vb != ve; ++vb) {
    std::pair<id_map::iterator, bool> r = ids.insert(get(boost::vertex_index, g, *vb));
    assert(r.second);
    ++count;
  }
  assert(count == num_vertices(fg));
}

template<typename Graph>
void test_out_edges(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  vertex_iterator vb, ve;
  for(boost::tie(vb, ve) = vertices(fg); vb != ve; ++vb) {
    id_map v_ids;

    vertex_descriptor around = *vb;
    out_edge_iterator oeb, oee;
    for(boost::tie(oeb, oee) = out_edges(*vb, fg); oeb != oee; ++oeb) {
      vertex_descriptor t = target(*oeb, fg);
      vertex_descriptor s = source(*oeb, fg);
      assert(s != t);
      assert(s == around);
      assert(t != around);
      std::pair<id_map::iterator, bool> r =
          v_ids.insert(get(boost::vertex_index, g, target(*oeb, fg)));
      assert(r.second);
    }
  }
}

template<typename Graph>
void test_in_edges(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  vertex_iterator vb, ve;
  for(boost::tie(vb, ve) = vertices(fg); vb != ve; ++vb) {
    id_map v_ids;
    vertex_descriptor around = *vb;
    in_edge_iterator ieb, iee;
    for(boost::tie(ieb, iee) = in_edges(*vb, fg); ieb != iee; ++ieb) {
      vertex_descriptor t = target(*ieb, fg);
      vertex_descriptor s = source(*ieb, fg);
      assert(t == around);
      assert(s != around);
      std::pair<id_map::iterator, bool> r =
          v_ids.insert(get(boost::vertex_index, g, source(*ieb, fg)));
      assert(r.second);
    }
  }
}

template<typename Graph>
void test_in_out_edges(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  // check that the sets of in out edges are the same
  vertex_iterator vb, ve;
  for(boost::tie(vb, ve) = vertices(fg); vb != ve; ++vb) {
    id_map v_ids;
    std::vector<vertex_descriptor> in, out;
    in_edge_iterator ieb, iee;
    for(boost::tie(ieb, iee) = in_edges(*vb, fg); ieb != iee; ++ieb) {
      std::pair<id_map::iterator, bool> r =
          v_ids.insert(get(boost::vertex_index, g, source(*ieb, fg)));
      assert(r.second);
      in.push_back(source(*ieb, fg));
    }
    out_edge_iterator oeb, oee;
    for(boost::tie(oeb, oee) = out_edges(*vb, fg); oeb != oee; ++oeb) {
      std::pair<id_map::iterator, bool> r =
          v_ids.insert(get(boost::vertex_index, g, target(*oeb, fg)));
      // insertion must fail
      assert(!r.second);
      out.push_back(target(*oeb, fg));
    }
    // did we walk the vertices in the same order?
    assert(in.size() == out.size());
    assert(std::equal(in.begin(), in.end(), out.begin()));
    assert(in.size() == in_degree(*vb, fg));
    assert(out.size() == out_degree(*vb, fg));
    assert(in.size() == degree(*vb, fg));
    assert(degree(*vb, fg) == in_degree(*vb, fg));
    assert(degree(*vb, fg) == out_degree(*vb, fg));
  }
}

// check that every edge can be found through edge(u, v, g)
template<typename Graph>
void test_edge_find(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));
  typedef std::pair<edge_descriptor, bool>   ret;

  edge_iterator eb, ee;
  for(boost::tie(eb, ee) = edges(fg); eb != ee; ++eb) {
    vertex_descriptor s = source(*eb, fg);
    vertex_descriptor t = target(*eb, fg);
    ret found = edge(s, t, fg);
    ret found2 = edge(t, s, fg);
    assert(found.second);
    assert(found2.second);
    assert(found.first == *eb);
    assert(found2.first == *eb);
  }
}

template<typename Graph>
void test_faces(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));

  unsigned int count = 0;
  face_iterator fb, fe;
  for(boost::tie(fb, fe) = faces(fg); fb != fe; ++fb) {
    ++count;
    // reverse look-up
    halfedge_descriptor assoc = halfedge(*fb, fg);
    assert(face(assoc, fg) == *fb);
    // check the enclosure
    halfedge_around_face_iterator encb, ence;
    for(boost::tie(encb, ence) = CGAL::halfedges_around_face(halfedge(*fb, fg), fg); encb != ence; ++encb) {
      assert(face(*encb, fg) == *fb);
    }
  }
  assert(count == num_faces(fg));
}

template<typename Graph>
void test_index_property_maps(const Graph& g)
{
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);

  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, -1, boost::make_assoc_property_map(map));
  assert(is_empty(fg));

  // Non const
  typedef typename CGAL::GetInitializedVertexIndexMap<Adapter>::type VIMap;
  VIMap vim = CGAL::get_initialized_vertex_index_map(fg);
  assert(CGAL::BGL::internal::is_index_map_valid(vim, num_vertices(fg), vertices(fg)));

  typedef typename CGAL::GetInitializedHalfedgeIndexMap<Adapter>::type HIMap;
  HIMap him = CGAL::get_initialized_halfedge_index_map(fg);
  assert(CGAL::BGL::internal::is_index_map_valid(him, num_halfedges(fg), halfedges(fg)));

  typedef typename CGAL::GetInitializedFaceIndexMap<Adapter>::type FIMap;
  FIMap fim = CGAL::get_initialized_face_index_map(fg);
  assert(CGAL::BGL::internal::is_index_map_valid(fim, num_faces(fg), faces(fg)));

  fg.set_selected_faces(0, boost::make_assoc_property_map(map));
  assert(!is_empty(fg));

  assert(CGAL::BGL::internal::is_index_map_valid(vim, num_vertices(fg), vertices(fg)));
  assert(CGAL::BGL::internal::is_index_map_valid(him, num_halfedges(fg), halfedges(fg)));
  assert(CGAL::BGL::internal::is_index_map_valid(fim, num_faces(fg), faces(fg)));

  // Const
  const Adapter cfg(g, 0, boost::make_assoc_property_map(map));
  typedef typename CGAL::GetInitializedVertexIndexMap<Adapter>::const_type CVIMap;
  CVIMap cvim = CGAL::get_initialized_vertex_index_map(cfg);
  assert(CGAL::BGL::internal::is_index_map_valid(cvim, num_vertices(cfg), vertices(cfg)));

  typedef typename CGAL::GetInitializedHalfedgeIndexMap<Adapter>::const_type CHIMap;
  CHIMap chim = CGAL::get_initialized_halfedge_index_map(cfg);
  assert(CGAL::BGL::internal::is_index_map_valid(chim, num_halfedges(cfg), halfedges(cfg)));

  typedef typename CGAL::GetInitializedFaceIndexMap<Adapter>::const_type CFIMap;
  CFIMap cfim = CGAL::get_initialized_face_index_map(cfg);
  assert(CGAL::BGL::internal::is_index_map_valid(cfim, num_faces(cfg), faces(cfg)));
}

template<typename Graph>
void test_read(const Graph& g)
{
  typedef typename boost::graph_traits<Graph>::face_descriptor g_face_descriptor;
  typedef CGAL::Face_filtered_graph<Graph> Adapter;
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);

  std::map<g_face_descriptor, std::size_t> map;
  PMP::connected_components(g, boost::make_assoc_property_map(map), CGAL::parameters::all_default());
  Adapter fg(g, 0, boost::make_assoc_property_map(map));
  assert(fg.is_selection_valid());
  assert(CGAL::is_valid_polygon_mesh(fg));
}

template <typename Graph>
void
test_graph_range(const std::vector<Graph>& graphs)
{
  for(Graph p : graphs)
  {
    test_read(p);
    test_vertex_iterators(p);
    test_out_edges(p);
    test_in_edges(p);
    test_in_out_edges(p);
    test_edge_find(p);
    test_faces(p);
    test_edge_iterators(p);
    test_halfedge_around_face_iterator(p);
    test_halfedge_around_vertex_iterator(p);
    test_index_property_maps(p);
  }
}

typedef SM::Point Point_3;

template<class Mesh, typename VertexPointPMap>
struct Constraint : public boost::put_get_helper<bool,Constraint<Mesh, VertexPointPMap> >
{
  typedef typename boost::graph_traits<Mesh>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag      category;
  typedef bool                                  value_type;
  typedef bool                                  reference;
  typedef edge_descriptor                       key_type;

  Constraint()
    :g_(NULL)
  {}

  Constraint(Mesh& g, VertexPointPMap vpp)
    : g_(&g), vppmap(vpp)
  {}

  bool operator[](edge_descriptor e) const
  {
    const Mesh& g = *g_;
    if(
       ((boost::get(vppmap, target(e, g)) == Point_3(1,1,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(1,1,1)) &&
        (boost::get(vppmap, target(e, g)) == Point_3(0,0,0) ||
         boost::get(vppmap, source(e, g)) == Point_3(0,0,0)))
       ||
       ((boost::get(vppmap, target(e, g)) == Point_3(1,1,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(1,1,1)) &&
        (boost::get(vppmap, target(e, g)) == Point_3(0,0,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(0,0,1)))
       ||
       ((boost::get(vppmap, target(e, g)) == Point_3(0,0,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(0,0,1)) &&
        (boost::get(vppmap, target(e, g)) == Point_3(0,0,0) ||
         boost::get(vppmap, source(e, g)) == Point_3(0,0,0)))
       ||
       ((boost::get(vppmap, target(e, g)) == Point_3(1,0,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(1,0,1)) &&
        (boost::get(vppmap, target(e, g)) == Point_3(0,0,0) ||
         boost::get(vppmap, source(e, g)) == Point_3(0,0,0)))
       ||
       ((boost::get(vppmap, target(e, g)) == Point_3(1,1,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(1,1,1)) &&
        (boost::get(vppmap, target(e, g)) == Point_3(1,0,1) ||
         boost::get(vppmap, source(e, g)) == Point_3(1,0,1)))
       )
      return true;
    else
      return false;
  }

  const Mesh* g_;
  VertexPointPMap vppmap;
};
template<class Mesh, class FCCMAP, class Adapter>
void test_mesh(Adapter fga)
{
  CGAL_GRAPH_TRAITS_MEMBERS(Adapter);
  //check that there is the right number of simplices in fga
  CGAL_assertion(CGAL::is_valid_polygon_mesh(fga));
  CGAL_assertion(num_faces(fga) == 2);
  CGAL_assertion(num_edges(fga) == 5);
  CGAL_assertion(num_halfedges(fga) == 10);
  CGAL_assertion(num_vertices(fga) == 4);
  halfedge_descriptor h = halfedge(*faces(fga).first, fga);
  CGAL_assertion_code( vertex_descriptor v = source(h, fga));
  //check that next() works inside the patch
  CGAL_assertion(next(next(next(h, fga), fga), fga) == h);
  //check that next() works on bordure of the patch
  h = opposite(h, fga);
  CGAL_assertion(next(next(next(next(h, fga), fga), fga), fga) == h);
  //check that prev() works inside the patch
  h = halfedge(*faces(fga).first, fga);
  CGAL_assertion(prev(prev(prev(h, fga), fga), fga) == h);
  //check that prev() works on bordure of the patch
  h = opposite(h, fga);
  CGAL_assertion(prev(prev(prev(prev(h, fga), fga), fga), fga) == h);
  //check degree
  CGAL_assertion(degree(v, fga) == 3);
  //check in_edges and out_edges
  CGAL_assertion(std::distance(in_edges(v, fga).first ,in_edges(v, fga).second) == 3);
  CGAL_assertion(std::distance(out_edges(v, fga).first ,out_edges(v, fga).second) == 3);

  Mesh copy;
  CGAL::copy_face_graph(fga, copy);
}

template <typename PolygonMesh>
void merge_vertices(typename boost::graph_traits<PolygonMesh>::vertex_descriptor v_keep,
                    typename boost::graph_traits<PolygonMesh>::vertex_descriptor v_rm,
                    std::vector<typename boost::graph_traits<PolygonMesh>::face_descriptor>& incident_faces,
                    PolygonMesh& mesh)
{
  typedef typename boost::graph_traits<PolygonMesh>::halfedge_descriptor halfedge_descriptor;

  halfedge_descriptor oh = halfedge(v_keep, mesh), done = oh;
  do
  {
    incident_faces.push_back(face(oh, mesh));
    oh = opposite(next(oh, mesh), mesh);
  }
  while(oh != done);

  halfedge_descriptor h = halfedge(v_rm, mesh);
  halfedge_descriptor start = h;
  do
  {
    set_target(h, v_keep, mesh);
    incident_faces.push_back(face(h, mesh));
    h = opposite(next(h, mesh), mesh);
  }
  while(h != start);

  remove_vertex(v_rm, mesh);
}

void test_invalid_selections()
{
  // this creates a non-manifold (pinched) vertex
  SM mesh;
  read_a_mesh(mesh, "data/7_faces_triangle.off");

  std::vector<SM::Face_index> face_range;
  face_range.push_back(SM::Face_index(1));
  face_range.push_back(SM::Face_index(2));
  face_range.push_back(SM::Face_index(3));

  CGAL::Face_filtered_graph<SM> bad_fg(mesh, face_range);
  assert(!bad_fg.is_selection_valid());

  // this creates a non-manifold vertex (multiple umbrellas)
  clear(mesh);
  read_a_mesh(mesh, "data/genus3.off");
  assert(is_valid_polygon_mesh(mesh));

  face_range.clear();
  merge_vertices(SM::Vertex_index(1337), SM::Vertex_index(87), face_range, mesh);

  CGAL::Face_filtered_graph<SM> bad_fg_2(mesh, face_range);
  assert(!bad_fg_2.is_selection_valid());
}

int main()
{
  test_graph_range(poly_data());

#ifdef CGAL_USE_SURFACE_MESH
  test_graph_range(sm_data());
#endif

#ifdef CGAL_USE_OPENMESH
  test_graph_range(omesh_data());
#endif

  test_invalid_selections();

  // Make a tetrahedron and test the adapter for a patch that only contains 2 faces
  typedef CGAL::Face_filtered_graph<SM> SM_Adapter;
  typedef SM::Property_map<boost::graph_traits<SM>::face_descriptor , std::size_t> SM_FCCMap;
  auto sm = std::make_unique<SM>();
  CGAL::make_tetrahedron(Point_3(1,1,1), Point_3(0,0,0), Point_3(0,0,1), Point_3(1,0,1), *sm);

  SM_FCCMap fccmap = sm->add_property_map<boost::graph_traits<SM>::face_descriptor, std::size_t>("f:CC").first;
  SM::Property_map<boost::graph_traits<SM>::vertex_descriptor, SM::Point> positions = sm->points();
  CGAL::Polygon_mesh_processing::connected_components(
    *sm, fccmap, CGAL::parameters::edge_is_constrained_map(Constraint<SM, SM::Property_map<boost::graph_traits<SM>::vertex_descriptor,
                                                           SM::Point> >(*sm, positions)));

  boost::unordered_set<long unsigned int> pids;
  pids.insert(0);
  pids.insert(2);
  SM_Adapter sm_adapter(*sm, pids, fccmap);
  test_mesh<SM,SM_FCCMap, SM_Adapter>(sm_adapter);

  typedef boost::graph_traits<Polyhedron> PolyTraits;
  typedef boost::property_map<Polyhedron, boost::vertex_point_t>::const_type VPMap;
  typedef PolyTraits::face_descriptor poly_face_descriptor;
  typedef boost::associative_property_map<std::map<poly_face_descriptor, PolyTraits::faces_size_type> > FCMap;
  typedef boost::property_map<Polyhedron, CGAL::face_external_index_t>::const_type FIMap;
  typedef boost::property_map<Polyhedron, CGAL::vertex_external_index_t>::const_type VIMap;
  typedef boost::property_map<Polyhedron, CGAL::halfedge_external_index_t>::const_type HIMap;
  typedef CGAL::Face_filtered_graph<Polyhedron, FIMap, VIMap, HIMap> Poly_Adapter;

  auto poly = std::make_unique<Polyhedron>();
  CGAL::make_tetrahedron(Point_3(1,1,1), Point_3(0,0,0), Point_3(0,0,1), Point_3(1,0,1), *poly);

  FIMap poly_fimap = get(CGAL::face_external_index, *poly);
  VIMap poly_vimap = get(CGAL::vertex_external_index, *poly);
  HIMap poly_himap = get(CGAL::halfedge_external_index, *poly);
  std::map<poly_face_descriptor, PolyTraits::faces_size_type> fc_map;
  FCMap poly_fccmap(fc_map);

  VPMap vpmap = get(boost::vertex_point, *poly);
  PMP::connected_components(*poly, poly_fccmap,
                            CGAL::parameters::edge_is_constrained_map(Constraint<Polyhedron, VPMap >(*poly, vpmap))
                                             .face_index_map(poly_fimap));
  Poly_Adapter poly_adapter(*poly, pids, poly_fccmap,
                            CGAL::parameters::face_index_map(poly_fimap)
                                             .vertex_index_map(poly_vimap)
                                             .halfedge_index_map(poly_himap));
  test_mesh<Polyhedron, FCMap, Poly_Adapter>(poly_adapter);
}


#ifndef STAN_MATH_PRIM_PROB_INV_CHI_SQUARE_RNG_HPP
#define STAN_MATH_PRIM_PROB_INV_CHI_SQUARE_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/scalar_seq_view.hpp>
#include <stan/math/prim/fun/size.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a pseudorandom inverse chi squared variate with the nu
 * degrees of freedom using the specified random number generator.
 *
 * nu can be a scalar or a one-dimensional container.
 *
 * @tparam T_deg Type of degrees of freedom parameter
 * @tparam RNG class of random number generator
 * @param nu (Sequence of) positive degrees of freedom parameter(s)
 * @param rng random number generator
 * @return (Sequence of) inverse chi squared random variate(s)
 * @throw std::domain_error if nu is nonpositive
 */
template <typename T_deg, class RNG>
inline typename VectorBuilder<true, double, T_deg>::type inv_chi_square_rng(
    const T_deg& nu, RNG& rng) {
  using boost::variate_generator;
  using boost::random::chi_squared_distribution;
  using T_nu_ref = ref_type_t<T_deg>;
  static const char* function = "inv_chi_square_rng";
  T_nu_ref nu_ref = nu;
  check_positive_finite(function, "Degrees of freedom parameter", nu_ref);

  scalar_seq_view<T_nu_ref> nu_vec(nu_ref);
  size_t N = stan::math::size(nu);
  VectorBuilder<true, double, T_deg> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, chi_squared_distribution<> > chi_square_rng(
        rng, chi_squared_distribution<>(nu_vec[n]));
    output[n] = 1 / chi_square_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/**
 * \ file FixedDelayLineFilter.cpp
 */

#include <iostream>

#include <ATK/Delay/FixedDelayLineFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#include <ATK/Mock/SimpleSinusGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/SumFilter.h>

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

#include <boost/math/constants/constants.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_1_test )
{
  ATK::FixedDelayLineFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(0), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_line100_delay_100_test )
{
  ATK::FixedDelayLineFilter<double> filter(100);
  BOOST_CHECK_THROW(filter.set_delay(100), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( FixedDelayLineFilter_sinus_line100_delay50_test )
{
  std::vector<double> data(PROCESSSIZE);
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<double>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<double> outdata(PROCESSSIZE);

  ATK::FixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(1);
  output.process(49);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(gsl::index i = 0; i < 50; ++i)
  {
    BOOST_REQUIRE_EQUAL(0, outdata[i]);
  }

  for(gsl::index i = 50; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i - 50], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_line100_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FixedDelayLineFilter<double> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 1; i < 100; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_line1000_delay50_progressive_test)
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_frequency(480);

  ATK::FixedDelayLineFilter<double> filter(1000);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);
  filter.process(50);

  ATK::SumFilter<double> sum;
  sum.set_input_sampling_rate(48000);
  sum.set_input_port(0, &filter, 0);
  sum.set_input_port(1, &generator, 0);

  ATK::TriangleCheckerFilter<double> output;
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &sum, 0);
  output.set_amplitude(0);

  for (std::size_t i = 50; i < 1000; ++i)
  {
    output.process(i);
  }
}

BOOST_AUTO_TEST_CASE(FixedDelayLineFilter_sinus_delay_test )
{
  ATK::FixedDelayLineFilter<double> filter(128);
  filter.set_delay(10);
  BOOST_CHECK_EQUAL(filter.get_delay(), 10);
}


/*!
 * @file     LogElementTypes.hpp
 * @author   Gabriel Hottiger
 * @date     Sep 28, 2016
 * @brief    List of supported non-STL types.
 */

#pragma once

// kindr
#ifdef SILO_USE_KINDR
  #include <kindr/Core>
#endif

// Eigen
#include <Eigen/Core>

namespace signal_logger {

//! Time stamp pair <s,ns>
typedef std::pair<int64_t, int64_t> TimestampPair;

//! Eigen types
typedef Eigen::Vector3d  Vector3d;
typedef Eigen::MatrixXf  MatrixXf;
typedef Eigen::MatrixXd  MatrixXd;
typedef Eigen::MatrixXi  MatrixXi;
typedef Eigen::Matrix< long ,Eigen::Dynamic, Eigen::Dynamic >           MatrixXl;
typedef Eigen::Matrix< short ,Eigen::Dynamic, Eigen::Dynamic >          MatrixXs;
typedef Eigen::Matrix< char ,Eigen::Dynamic, Eigen::Dynamic >           MatrixXc;
typedef Eigen::Matrix< unsigned char ,Eigen::Dynamic, Eigen::Dynamic >  MatrixXUc;
typedef Eigen::Matrix< bool ,Eigen::Dynamic, Eigen::Dynamic >           MatrixXb;
typedef Eigen::Matrix< std::string ,Eigen::Dynamic, Eigen::Dynamic >    MatrixXstring;

#ifdef SILO_USE_KINDR
  //! Kindr types
  typedef kindr::Position3D             KindrPositionD;
  typedef kindr::RotationQuaternionPD   KindrRotationQuaternionD;
  typedef kindr::EulerAnglesZyxPD       KindrEulerAnglesZyxD;
  typedef kindr::LocalAngularVelocityPD KindrAngularVelocityD;
  typedef kindr::AngleAxisPD            KindrAngleAxisD;
  typedef kindr::RotationMatrixPD       KindrRotationMatrixD;
  typedef kindr::RotationVectorPD       KindrRotationVectorD;
  typedef kindr::Velocity3D             KindrLinearVelocityD;
  typedef kindr::Acceleration3D         KindrLinearAccelerationD;
  typedef kindr::AngularAcceleration3D  KindrAngularAccelerationD;
  typedef kindr::Force3D                KindrForceD;
  typedef kindr::Torque3D               KindrTorqueD;
  typedef kindr::VectorTypeless3D       KindrVectorD;
  typedef kindr::HomTransformQuatD      KindrHomTransformQuatD;

  //! Kindr vector at position type
  template<typename VectorType_>
  struct KindrVectorAtPosition {
    typedef VectorType_ VectorType;
    VectorType_ vector;
    std::string vectorFrame;
    KindrPositionD position;
    std::string positionFrame;
  };
#endif

} // end namespace


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_DETAIL_FULL_MULTI_AXIS_INDEX_ITER_HPP_INCLUDED
#define NDHIST_DETAIL_FULL_MULTI_AXIS_INDEX_ITER_HPP_INCLUDED 1

#include <vector>

#include <boost/assert.hpp>

#include <ndhist/detail/axis.hpp>

namespace ndhist {
namespace detail {

/**
 * @brief The full_multi_axis_index_iter struct provides a n-dimensional index
 *        iterator over the full range of each axis. Each axis can be fixed to
 *        certain value if needed.
 */
struct full_multi_axis_index_iter
{
    full_multi_axis_index_iter(
        std::vector<intptr_t> const & arr_shape
    )
      : is_end_point_(false)
      , arr_shape_(arr_shape)
    {
        uintptr_t const nd = arr_shape_.size();
        fixed_axes_mask_.resize(nd);
        indices_.resize(nd);
    }

    /**
     * @brief Initializes the iteration over the array (including OOR bins)
     *        for the given fixed axes indices (*fixed_axes_indices*).
     *        All vectors must be of
     *        length *nd* -- the dimensionality of the array.
     */
    void
    init_iteration(std::vector<intptr_t> const & fixed_axes_indices)
    {
        uintptr_t const nd = arr_shape_.size();
        indices_.assign(nd, axis::OOR_UNDERFLOW);
        n_iterations_ = 1;
        last_iter_axis_ = -1;
        for(uintptr_t i=0; i<nd; ++i)
        {
            fixed_axes_mask_[i] = (fixed_axes_indices[i] != axis::FLAGS_FLOATING_INDEX);
            if(! fixed_axes_mask_[i])
            {
                last_iter_axis_ = i;
                n_iterations_ *= arr_shape_[i]+2;
            }
            else
            {
                indices_[i] = fixed_axes_indices[i];
            }
        }

        if(last_iter_axis_ == -1)
        {
            throw AssertionError("No iteration axes specified for oor multi-axis iteration!");
        }

        p_ = last_iter_axis_;
        iter_index_ = 0;
        is_end_point_ = false;
    }

    std::vector<intptr_t> const &
    get_indices() const
    {
        return indices_;
    }

    void increment()
    {
        // Increment the indices values for the next value.
        if(iter_index_ == n_iterations_-1)
        {
            is_end_point_ = true;
            return;
        }

        // Move the index pointer to the next non-fixed outer-axis if the index
        // of the current axis has reached its maximum. Then increase
        // the index and reset all indices to the right of this
        // increased index to zero. After this operation, the index
        // pointer points to the inner-most axis (excluding the
        // fixed axes).
        while((indices_[p_] == axis::OOR_OVERFLOW) || fixed_axes_mask_[p_])
        {
            --p_;
        }
        BOOST_ASSERT(p_ >= 0);
        if(indices_[p_] == arr_shape_[p_]-1) {
            indices_[p_] = axis::OOR_OVERFLOW;
        }
        else {
            ++indices_[p_];
        }
        while(p_ < last_iter_axis_)
        {
            ++p_;
            if(! fixed_axes_mask_[p_]) {
                indices_[p_] = axis::OOR_UNDERFLOW;
            }
        }

        ++iter_index_;
    }

    bool is_end() const
    {
        return is_end_point_;
    }

    bool is_oor_bin() const
    {
        uintptr_t const nd = arr_shape_.size();
        for(uintptr_t i=0; i<nd; ++i)
        {
            if((indices_[i] == axis::OOR_UNDERFLOW) ||
               (indices_[i] == axis::OOR_OVERFLOW))
            {
                return true;
            }
        }
        return false;
    }

    uintptr_t
    get_oor_array_index() const
    {
        uintptr_t idx = 0;
        uintptr_t const nd = arr_shape_.size();
        for(uintptr_t i=0; i<nd; ++i)
        {
            if(! ((indices_[i] == axis::OOR_UNDERFLOW) || (indices_[i] == axis::OOR_OVERFLOW)))
            {
                // Mark the axis i as available.
                idx |= (1<<i);
            }
        }
        return idx;
    }

    bool is_end_point_;
    std::vector<intptr_t> const & arr_shape_;
    std::vector<bool> fixed_axes_mask_;
    std::vector<intptr_t> indices_;
    intptr_t last_iter_axis_;
    intptr_t n_iterations_;
    intptr_t p_;
    intptr_t iter_index_;
};

}// namespace detail
}// namespace ndhist

#endif  // !NDHIST_DETAIL_FULL_MULTI_AXIS_INDEX_ITER_HPP_INCLUDED


/*
 * grid_map_pcl_from_pointcloud2.cpp
 *
 *  Created on: Nov 7, 2021
 *      Author: Yibin Li
 */

#include <iostream>

#include <ros/ros.h>

#include <grid_map_core/GridMap.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_cv/grid_map_cv.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>

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

#include "grid_map_pcl/GridMapPclLoader.hpp"
#include "grid_map_pcl/helpers.hpp"

#include "opencv2/core.hpp"
//#include <opencv2/core/utility.hpp>
#include "opencv2/highgui.hpp"

#include "sensor_msgs/PointCloud2.h"

namespace gm = ::grid_map::grid_map_pcl;

ros::Publisher gridMapPub;
ros::Subscriber sub;

grid_map::GridMapPclLoader gridMapPclLoader;
//ros::NodeHandle nh("~");

void pointcloud_callback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
    // maybe move all process into this subscriber callback function
    ROS_INFO("inside callback");
    ros::NodeHandle nh("~");

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*cloud_msg,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud_input(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*pcl_cloud_input);
    ROS_INFO("pointcloud2 to pcl pointcloud conversion succeed!");

    gridMapPclLoader.setInputCloud(pcl_cloud_input);
    gm::processPointcloud(&gridMapPclLoader, nh);

//    grid_map::GridMap gridMap = gridMapPclLoader.getGridMap();
    grid_map::GridMap originalMap = gridMapPclLoader.getGridMap();
    grid_map::GridMap gridMap;
    grid_map::GridMapCvProcessing::changeResolution(originalMap, gridMap, 0.001);
    gridMap.setFrameId(gm::getMapFrame(nh));

    std::vector<std::string> layer_name = gridMap.getLayers();
    for (auto &name : layer_name) {
        ROS_INFO("layer name %s", name.c_str());
    }

    std::string grid_map_col = std::to_string(gridMap.getSize()(1));
    std::string grid_map_row = std::to_string(gridMap.getSize()(0));

    ROS_INFO("grid map column size: %s", grid_map_col.c_str());
    ROS_INFO("grid map row size: %s", grid_map_row.c_str());

    grid_map::Matrix& m = gridMap.get("elevation");

//    namespace Eigen{
//        template<class Matrix>
//        void write_binary(const char* filename, const Matrix& matrix){
//            std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
//            typename Matrix::Index rows=matrix.rows(), cols=matrix.cols();
//            out.write((char*) (&rows), sizeof(typename Matrix::Index));
//            out.write((char*) (&cols), sizeof(typename Matrix::Index));
//            out.write((char*) matrix.data(), rows*cols*sizeof(typename Matrix::Scalar) );
//            out.close();
//        }
//        template<class Matrix>
//        void read_binary(const char* filename, Matrix& matrix){
//            std::ifstream in(filename, std::ios::in | std::ios::binary);
//            typename Matrix::Index rows=0, cols=0;
//            in.read((char*) (&rows),sizeof(typename Matrix::Index));
//            in.read((char*) (&cols),sizeof(typename Matrix::Index));
//            matrix.resize(rows, cols);
//            in.read( (char *) matrix.data() , rows*cols*sizeof(typename Matrix::Scalar) );
//            in.close();
//        }
//    } // Eigen::
//    int m_columns = m.cols();
//    int m_rows = m.rows();
//    Eigen::ArrayXf mask = Eigen::ArrayXf::Zero(m_rows, m_columns);
//    Eigen::ArrayXf filtered_m_array = (m.array() > -0.8).select(mask, m);
//    filtered_m_array = (filtered_m_array < -0.6).select(mask, filtered_m_array);
//    grid_map::Matrix filtered_m = filtered_m_array.matrix();
//
//    float filtered_m_min_value = filtered_m.minCoeff();
//    float filtered_m_max_value = filtered_m.maxCoeff();
//
//    ROS_INFO("filtered_m min value: %s", std::to_string(filtered_m_min_value).c_str());
//    ROS_INFO("filtered_m max value: %s", std::to_string(filtered_m_max_value).c_str());

    ROS_INFO("matrix column: %s", std::to_string(m.cols()).c_str());
    cv::Mat map;
    grid_map::GridMapCvConverter::toImage<unsigned char,3>(gridMap, "elevation", CV_16UC3, map);
    cv::imwrite("/home/viplab/grid_map_output/grid_map_img_from_cv_converter.png", map);

    // eigen matrix to opencv image
    cv::Mat map_from_eigen;
    float min_value = gridMap.get("elevation").minCoeffOfFinites();
    float max_value = gridMap.get("elevation").maxCoeffOfFinites();
    ROS_INFO("matrix min value: %s", std::to_string(min_value).c_str());
    ROS_INFO("matrix max value: %s", std::to_string(max_value).c_str());
//    eigen2cv(m, map_from_eigen);
//    cv::imwrite("/home/viplab/map_from_eigen.png", map_from_eigen);

    // eigen matrix to local txt file
    std::ofstream file("/home/viplab/grid_map_output/grid_map_eigen.txt");
    if (file.is_open())
    {
        file << m << '\n';
    }

    // publish grid map

    grid_map_msgs::GridMap msg;
    grid_map::GridMapRosConverter::toMessage(gridMap, msg);
    gridMapPub.publish(msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "grid_map_pcl_loader_node");
    ros::NodeHandle nh("~");
    gm::setVerbosityLevelToDebugIfFlagSet(nh);

//    grid_map::GridMapPclLoader gridMapPclLoader;

    ros::Rate loop_rate(10);
    sub = nh.subscribe(gm::getPointcloudTopic(nh), 1, pointcloud_callback);

    gridMapPub = nh.advertise<grid_map_msgs::GridMap>("grid_map_from_raw_pointcloud", 1, true);

/*
//    const std::string pathToCloud = gm::getPcdFilePath(nh);
    gridMapPclLoader.loadParameters(gm::getParameterPath());
//    gridMapPclLoader.loadCloudFromPcdFile(pathToCloud);

    gm::processPointcloud(&gridMapPclLoader, nh);

    grid_map::GridMap gridMap = gridMapPclLoader.getGridMap();
    gridMap.setFrameId(gm::getMapFrame(nh));

//    gm::saveGridMap(gridMap, nh, gm::getMapRosbagTopic(nh));

    // publish grid map

    grid_map_msgs::GridMap msg;
    grid_map::GridMapRosConverter::toMessage(gridMap, msg);
    gridMapPub.publish(msg);
    */

    // run
    ros::spin();
    return EXIT_SUCCESS;
}


//    Copyright 2019 Jij Inc.

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

#ifndef OPENJIJ_SYSTEM_CLASSICAL_ISING_HPP__
#define OPENJIJ_SYSTEM_CLASSICAL_ISING_HPP__

#include <cassert>
#include <utility>
#include <system/system.hpp>
#include <graph/all.hpp>
#include <utility/eigen.hpp>
#include <type_traits>
#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace openjij {
    namespace system {

        /**
         * @brief naive ClassicalIsing structure (system for classical Ising model)
         *
         * @tparam GraphType type of graph
         * @tparam eigen_impl specify that Eigen implementation is enabled.
         */
        template<typename GraphType, bool eigen_impl=false>
            struct ClassicalIsing {
                static_assert(!eigen_impl, "Eigen implementation is not supported.");

                using system_type = classical_system;

                /**
                 * @brief Constructor to initialize spin and interaction
                 *
                 * @param spin
                 * @param interaction
                 */
                ClassicalIsing(const graph::Spins& init_spin, const GraphType& init_interaction)
                    : spin{init_spin}, interaction{init_interaction}, num_spins{init_spin.size()} {
                        assert(init_spin.size() == init_interaction.get_num_spins());
                    }

                /**
                 * @brief reset spins
                 *
                 * @param init_spin
                 */
                void reset_spins(const graph::Spins& init_spin){
                    this->spin = init_spin;
                }

                graph::Spins spin;
                const GraphType interaction;
                /**
                 * @brief number of real spins (dummy spin excluded)
                 */
                const std::size_t num_spins; //spin.size()
            };

        //TODO: unify Dense and Sparse Eigen-implemented ClassicalIsing struct

        /**
         * @brief ClassicalIsing structure for Dense graph (Eigen-based)
         *
         * @tparam FloatType type of floating-point
         */
        template<typename FloatType>
            struct ClassicalIsing<graph::Dense<FloatType>, true>{
                using system_type = classical_system;

                //matrix (row major)
                using MatrixXx = Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
                //vector (col major)
                using VectorXx = Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>;

                /**
                 * @brief Constructor to initialize spin and interaction
                 *
                 * @param spin
                 * @param interaction
                 */
                ClassicalIsing(const graph::Spins& init_spin, const graph::Dense<FloatType>& init_interaction)
                    : spin(utility::gen_vector_from_std_vector<FloatType, Eigen::ColMajor>(init_spin)),
                    interaction(utility::gen_matrix_from_graph<Eigen::RowMajor>(init_interaction)),
                    num_spins(init_interaction.get_num_spins()){
                        assert(init_spin.size() == init_interaction.get_num_spins());
                    }

                /**
                 * @brief reset spins
                 *
                 * @param init_spin
                 */
                void reset_spins(const graph::Spins& init_spin){
                    this->spin = utility::gen_vector_from_std_vector<FloatType, Eigen::ColMajor>(init_spin);
                }

                VectorXx spin;
                const MatrixXx interaction;

                /**
                 * @brief number of real spins (dummy spin excluded)
                 */
                const std::size_t num_spins; //spin.size()-1
            };

        /**
         * @brief ClassicalIsing structure for Sparse graph (Eigen-based)
         *
         * @tparam FloatType type of floating-point
         */
        template<typename FloatType>
            struct ClassicalIsing<graph::Sparse<FloatType>, true>{
                using system_type = classical_system;

                //matrix (row major)
                using SparseMatrixXx = Eigen::SparseMatrix<FloatType, Eigen::RowMajor>;
                //vector (col major)
                using VectorXx = Eigen::Matrix<FloatType, Eigen::Dynamic, 1, Eigen::ColMajor>;

                /**
                 * @brief Constructor to initialize spin and interaction
                 *
                 * @param spin
                 * @param interaction
                 */
                ClassicalIsing(const graph::Spins& init_spin, const graph::Sparse<FloatType>& init_interaction)
                    : spin(utility::gen_vector_from_std_vector<FloatType, Eigen::ColMajor>(init_spin)),
                    interaction(utility::gen_matrix_from_graph<Eigen::RowMajor>(init_interaction)),
                    num_spins(init_interaction.get_num_spins()){
                        assert(init_spin.size() == init_interaction.get_num_spins());
                    }

                /**
                 * @brief reset spins
                 *
                 * @param init_spin
                 */
                void reset_spins(const graph::Spins& init_spin){
                    this->spin = utility::gen_vector_from_std_vector<FloatType, Eigen::ColMajor>(init_spin);
                }

                VectorXx spin;
                const SparseMatrixXx interaction;

                /**
                 * @brief number of real spins (dummy spin excluded)
                 */
                const std::size_t num_spins; //spin.size()-1
            };

        /**
         * @brief helper function for ClassicalIsing constructor
         *
         * @tparam eigen_impl
         * @tparam GraphType
         * @param init_spin initial spin
         -        * @param init_interaction initial interaction
         *
         * @return generated object
         */
        template<bool eigen_impl=false,typename GraphType>
            ClassicalIsing<GraphType, eigen_impl> make_classical_ising(const graph::Spins& init_spin, const GraphType& init_interaction){
                return ClassicalIsing<GraphType, eigen_impl>(init_spin, init_interaction);
            }

    } // namespace system
} // namespace openjij

#endif