// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <iostream>
#include <sstream>

#include <boost/format.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/constants.hpp"
#include "heom/dipole_matrix.hpp"
#include "heom/dipole_pseudo_pathway.hpp"
#include "heom/e_field.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_graph.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_solver.hpp"
#include "heom/sites_to_states.hpp"
#include "heom/transient_absorption_config.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using heom::int_t;
using heom::real_t;
using heom::real_format;
using heom::complex_t;
using heom::default_delimiter;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	// command line parsing
	// usage: app_* <ocl_config> <heom_config> [partition_file]
	std::string ocl_config_file = "ocl_config.cfg";
	std::string heom_config_file = "transient_absorption.cfg";

	if (argc >= 2)
		ocl_config_file = argv[1];
	if (argc >= 3)
		heom_config_file = argv[2];

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::transient_absorption_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// prepare output file (so we run into possible file errors before starting the computation)
		auto& observation_filename = heom_config.observations().get().front().get().second; // filename of first specified observation, guaranteed to exist by config checks
		std::ofstream observation_file(observation_filename);
		if (observation_file.fail())
			throw std::runtime_error("Error: could not open output file: " + observation_filename);

		// transient absorption computation over all combinations of dipole matrices and phases
		const size_t num_dipole_matrices = heom_config.dipole_tensor_prefactors().size();
		const size_t num_pulse_phases = heom_config.probe_pulse_phases().size();
		const size_t num_experiments = num_dipole_matrices * num_pulse_phases;

		// solver iterations and steps
		const int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		const int_t steps_per_iteration = heom_config.program_observe_steps();
		const int_t total_steps = iterations * heom_config.program_observe_steps();

		// output data structure for averaged traces
		const int_t num_observations = iterations + 1;
		std::vector<heom::matrix_trace_observation_view> observations(num_observations);
		// e_field is needed once, and is the same across runs
		std::vector<complex_t> e_field_observations(num_observations);

		// transient absorption pseudo pathway
		const auto& pathway_spec = heom_config.spectra_include_esa() ? heom::dipole_pseudo_pathway_esa_spec : heom::dipole_pseudo_pathway_wgs_spec;
		const auto sts_mode = pathway_spec.sites_to_states_mode();

		// for progress estimate
		const size_t solver_runs = num_dipole_matrices * num_pulse_phases;
		size_t solver_run = 0;

		// iterate over all dipoles and compute population dynamics
		for (size_t tensor_index = 0; tensor_index < num_dipole_matrices; ++tensor_index) {
			// loop over all pulse phases
			for (size_t phase_index = 0; phase_index < num_pulse_phases; ++phase_index)
			{
				const bool first_solver_run = tensor_index == 0 && phase_index == 0; // only observe first solver run

				heom::instance heom_instance(heom_config, sts_mode, complete_graph);
				heom_instance.set_hierarchy_top(); // NOTE: hierarchy top is initialised to zero with first element being complex_t(1.0,0.0) here

				// OpenCL range
				ocl::nd_range range {
					{}, // offset
					{1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
					{1, 1} // local size
				};

				// create a solver from configuration and instance
				solver_t solver(ocl_config, range, heom_config, heom_instance);
				std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
				solver.write_ocl_runtime_config(std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;
				++solver_run; // for progress estimate

				// compute dipole matrices
				auto get_dipole_matrix = [&](size_t spec_index) {
					return heom::get_dipole_matrix_for_pathway(pathway_spec, spec_index, heom_config.system_sites(), heom_config, tensor_index);
				};
				auto dipole_matrix_minus = get_dipole_matrix(0);
				auto dipole_matrix_plus = get_dipole_matrix(1);
				// TODO: check scaling
				dipole_matrix_minus.scale(heom::get_dipole_tensor_prefactor(pathway_spec, heom_config, tensor_index));

				// TODO: remove this scaling, i.e. fix unit of dipole matrices across all applications, maybe use factor during initialisation of config object or change input file format
				//       scaling dipole strengths in dipole_config_entries does work for this app, but messes with the result of others (tested with linear_absorption)
				dipole_matrix_minus.scale(heom::constants::debye_to_coulomb_meter);
				dipole_matrix_plus.scale(heom::constants::debye_to_coulomb_meter);
//std::cout << dipole_matrix_minus << std::endl << std::endl;
//std::cout << dipole_matrix_plus << std::endl << std::endl;

				// TODO: check if this is really plus and not prescaled... prefactor might be one in config (?) vs. other apps
				// setup observer with pre-scaled dipole matrix
				heom::matrix_trace_observer<solver_t> trace_observer(complete_graph, solver, dipole_matrix_plus.data()); // D+ from left via observer

				// extract hamiltonian as complex_matrix_t for the e_field
				heom::complex_matrix_t hamiltonian(dipole_matrix_plus.rows(), dipole_matrix_plus.cols());
				for(size_t i = 0; i < hamiltonian.rows(); ++i) {
					for(size_t j = 0; j < hamiltonian.cols(); ++j) {
						const size_t real_index = 2 * (i * hamiltonian.rows() + j);
						const size_t imag_index = real_index + 1;
						hamiltonian.at(i, j) = heom::complex_t(heom_instance.hamiltonian()[real_index], heom_instance.hamiltonian()[imag_index]);
					}
				}

				// add e-field pre-evaluation action to the ODE for transient spectra computation
				auto e_field = heom::e_field<decltype(heom_config), heom::ode>(heom_config, phase_index, dipole_matrix_plus, dipole_matrix_minus, hamiltonian);
				solver.ode().clear_pre_evaluate_actions();
				solver.ode().add_pre_evaluate_action(e_field);

				// with create_hierarchy_mask we create mask corresponding to configured filtering strategy, then update solver
				// NOTE: .data() returns pointer to mask_t array
				heom::hierarchy_mask hierarchy_mask = heom::create_hierarchy_mask(heom_config.filtering_strategy(), complete_graph, heom_config.baths_matsubaras(), heom_config.filtering_first_layer());
				solver.update_hierarchy_mask(hierarchy_mask.data());

				std::cout << "-------------------- Hierarchy Mask Counter ----------------" << std::endl;
				heom::write_hierarchy_mask_stats(hierarchy_mask, std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;

				// generate first observation with initial value
				observations[0] += trace_observer.observe_trace(0.0);
				if (first_solver_run)
					e_field_observations[0] = e_field.compute_field(0.0);

				// setup output per propagation output
				heom::config::obervation_type_list per_propagation_observations;

				// build single-propagation observation list from config list
				for (auto& pair : heom_config.observations().get()) {
					auto& obs_type = pair.get().first;
					auto& filename = pair.get().second;

					if (heom::is_single_propagation_observation(obs_type)) {
						std::remove_const<std::remove_reference<decltype(pair)>::type>::type new_pair;
						new_pair.get().first = pair.get().first; // copy observation type

						std::stringstream new_filename;
						size_t dot_pos = filename.find_first_of('.');
						if (dot_pos != std::string::npos) {
							new_filename << filename.substr(0, dot_pos); // copy everything prior to first dot
						} else {
							new_filename << filename; // copy whole filename, in case not dot was found
						}
						new_filename << "_d" << tensor_index << "_p" << phase_index; // add create suffix
						if (dot_pos != std::string::npos) {
							new_filename << filename.substr(dot_pos); // copy file ending
						}

						new_pair.get().second = new_filename.str(); // set new filename
						per_propagation_observations.get().push_back(new_pair);
					}
				}
				heom::observer_list observer_list = heom::make_file_observer_list(per_propagation_observations, complete_graph, solver);
				observer_list.observe(0.0);

				// write file header once
				if (first_solver_run) {
					trace_observer.write_header(observation_file, false);
					observation_file << default_delimiter;
					observation_file << "e-field_real";
					observation_file << default_delimiter;
					observation_file << "e-field_imag";
					observation_file << '\n';
				}

				// propagate system
				for (int_t k = 0; k < iterations; ++k)
				{
					// propagate
					solver.step_forward(steps_per_iteration);

					// after propagation:
					const auto current_step = (k + 1) * steps_per_iteration;
					const real_t current_time = current_step * heom_config.solver_step_size();

					// observe trace of dipole_matrix_minus - rho
					observations[k + 1] += trace_observer.observe_trace(current_time);
					if (first_solver_run)
						e_field_observations[k + 1] = e_field.compute_field(current_time);

					// observe single-propagation observation types
					observer_list.observe(current_time);

					// update status
					std::stringstream status_prefix;
					status_prefix << "Calculation transient absorption ("
					              << "dipole: " << tensor_index + 1 << '/' << num_dipole_matrices
					              << ", "
					              << "phase: " << phase_index + 1 << '/' << num_pulse_phases
					              << "): ";
					heom::write_progress(current_step - 1, total_steps, solver_run, solver_runs, status_prefix.str(), std::cout);
				}

				// write_complex_matrix(result_buffer_top, heom_instance.states(), std::cout);
				std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
				solver.write_runtime_summary(std::cout);
				std::cout << "------------------------------------------------------------" << std::endl;
			}
		}

		// write averaged traces, with added columns for e-field
		auto format = real_format;
		//for (auto& obs : observations) {
		for (int_t i = 0; i < num_observations; ++i) {
			observations[i].avg(num_experiments);
			observation_file << observations[i];
			observation_file << default_delimiter;
			observation_file << format % e_field_observations[i].real();
			observation_file << default_delimiter;
			observation_file << format % e_field_observations[i].imag();
			observation_file << '\n';
		}
		observation_file << std::flush; // tell the OS to write to disk

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "main(): application runtime: " << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


// Copyright 2016. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include "explicit_system.h"
#include <boost/format.hpp>
#include <fstream>

namespace explicit_fea {

    ExplicitSystem::ExplicitSystem(Mesh _mesh,
                                   ForceList _external_forces,
                                   const ColumnVector& _initial_displacements,
                                   const ColumnVector& _initial_velocities,
                                   double _t_0,
                                   const Options& _options) :
        mesh(std::move(_mesh)),
        external_forces(std::move(_external_forces)),
        displacements_0(_initial_displacements),
        velocities_0(_initial_velocities),
        velocities_1(ColumnVector::Zero(_initial_displacements.size())),
        accelerations_0(ColumnVector::Zero(_initial_displacements.size())),
        accelerations_1(ColumnVector::Zero(_initial_displacements.size())),
        forces(ColumnVector::Zero(_initial_displacements.size())),
        RHS(ColumnVector::Zero(_initial_displacements.size())),
        t_0(_t_0),
        dt_0(0.0),
        options(_options) {
        if (displacements_0.size() != velocities_0.size())
            throw std::runtime_error("Size of initial velocities and initial displacements are not equal.");
        if (displacements_0.size() != mesh.get_global_stiffness_matrix().cols())
            throw std::runtime_error("Size of displacements and velocities does not match the number of columns in the global matrices.");

        LHS.resize(mesh.get_global_stiffness_matrix().rows(), mesh.get_global_stiffness_matrix().cols());
        assemble_damping_matrix();
        apply_bcs(t_0);
    }

    void ExplicitSystem::Options::load(const rapidjson::Document &config_doc) {
        if (config_doc.HasMember("options")) {
            if (config_doc["options"].HasMember("beta")) {
                if (!config_doc["options"]["beta"].IsNumber()) {
                    throw std::runtime_error("beta provided in options configuration is not a number.");
                }
                beta = config_doc["options"]["beta"].GetDouble();
            }
            if (config_doc["options"].HasMember("gamma")) {
                if (!config_doc["options"]["gamma"].IsNumber()) {
                    throw std::runtime_error("gamma provided in options configuration is not a number.");
                }
                gamma = config_doc["options"]["gamma"].GetDouble();
            }
            if (config_doc["options"].HasMember("damping_beta")) {
                if (!config_doc["options"]["damping_beta"].IsNumber()) {
                    throw std::runtime_error("damping_beta provided in options configuration is not a number.");
                }
                damping_beta = config_doc["options"]["damping_beta"].GetDouble();
            }
            if (config_doc["options"].HasMember("damping_alpha")) {
                if (!config_doc["options"]["damping_alpha"].IsNumber()) {
                    throw std::runtime_error("damping_alpha provided in options configuration is not a number.");
                }
                damping_alpha = config_doc["options"]["damping_alpha"].GetDouble();
            }
        }
    }

    void ExplicitSystem::update(double dt_1) {
        double t_1 = t_0 + dt_1;
        if (!compare.equal(dt_1, dt_0)) {
            assemble_LHS(dt_1);
        }

        apply_external_forces(t_1);
        RHS = forces;
        RHS.noalias() -= damping_matrix * (velocities_0 + (1.0 - options.gamma) * dt_1 * accelerations_0);
        RHS.noalias() -= mesh.get_global_stiffness_matrix() * (displacements_0 + dt_1 * velocities_0 + (0.5 - options.beta) * dt_1 * dt_1 * accelerations_0);
        apply_bcs(t_1);

        accelerations_1 = solver.solve(RHS);
        velocities_1.noalias() += (1.0 - options.gamma) * dt_1 * accelerations_0 + options.gamma * dt_1 * accelerations_1;

        // advance all current time values to next iteration
        displacements_0.noalias() += dt_1 * velocities_0 + dt_1 * dt_1 * (0.5 - options.beta) * accelerations_0 + dt_1 * dt_1 * options.beta * accelerations_1;
        velocities_0 = velocities_1;
        accelerations_0 = accelerations_1;
        t_0 = t_1;
        dt_0 = dt_1;
    }

	Mesh const& ExplicitSystem::getMesh() const {
		return mesh;
	}

    ColumnVector const& ExplicitSystem::getDisplacements() const {
        return displacements_0;
    }

    ColumnVector ExplicitSystem::getForces() const {
        ColumnVector out(displacements_0.size());
        out = mesh.get_global_stiffness_matrix() * displacements_0;
        out += mesh.get_mass_matrix() * accelerations_0;
        return out;
    }

    ColumnVector const& ExplicitSystem::getVelocities() const {
        return velocities_0;
    }

    double ExplicitSystem::getTime() const {
        return t_0;
    }

    void ExplicitSystem::assemble_damping_matrix() {
        damping_matrix.resize(mesh.get_global_stiffness_matrix().rows(), mesh.get_global_stiffness_matrix().cols());
        damping_matrix = options.damping_alpha * mesh.get_mass_matrix() + options.damping_beta * mesh.get_global_stiffness_matrix();
        damping_matrix.prune(1.e-14);
        damping_matrix.makeCompressed();
    }

    void ExplicitSystem::assemble_LHS(double dt){
        LHS = mesh.get_mass_matrix() + options.gamma * dt * damping_matrix + options.beta * dt * dt * mesh.get_global_stiffness_matrix();
        LHS.prune(1.e-14);
        LHS.makeCompressed();
        solver.compute(LHS);
    }

    void ExplicitSystem::apply_bcs(double time) {
        for (BCList::const_iterator it = mesh.get_bcs().begin(); it != mesh.get_bcs().end(); ++it) {
            RHS[(*it)->global_index] = 0.0;
            if ((*it)->type == BC::Type::DISPLACEMENT) {
                displacements_0[(*it)->global_index] = (*it)->get_value(time);
                velocities_0[(*it)->global_index] = 0.0;
            } else if ((*it)->type == BC::Type::VELOCITY) {
                velocities_0[(*it)->global_index] = (*it)->get_value(time);
            }
        }
    }

    void ExplicitSystem::apply_external_forces(double time) {
        for (ForceList::const_iterator it = external_forces.begin(); it != external_forces.end(); ++it) {
            forces[(*it)->global_index] = (*it)->get_value(time);
        }
    }

} // namespace explicit_fea


#ifndef STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP
#define STAN_MCMC_HMC_INTEGRATORS_IMPL_LEAPFROG_HPP

#include <Eigen/Dense>
#include <stan/mcmc/hmc/integrators/base_leapfrog.hpp>

namespace stan {
  namespace mcmc {

    template <typename Hamiltonian>
    class impl_leapfrog: public base_leapfrog<Hamiltonian> {
    public:
      impl_leapfrog(): base_leapfrog<Hamiltonian>(),
                       max_num_fixed_point_(10),
                       fixed_point_threshold_(1e-8) {}

      void begin_update_p(typename Hamiltonian::PointType& z,
                          Hamiltonian& hamiltonian,
                          double epsilon,
                          callbacks::logger& logger) {
        hat_phi(z, hamiltonian, epsilon, logger);
        hat_tau(z, hamiltonian, epsilon, this->max_num_fixed_point_,
                logger);
      }

      void update_q(typename Hamiltonian::PointType& z,
                    Hamiltonian& hamiltonian,
                    double epsilon,
                    callbacks::logger& logger) {
        // hat{T} = dT/dp * d/dq
        Eigen::VectorXd q_init = z.q + 0.5 * epsilon * hamiltonian.dtau_dp(z);
        Eigen::VectorXd delta_q(z.q.size());

        for (int n = 0; n < this->max_num_fixed_point_; ++n) {
          delta_q = z.q;
          z.q.noalias() = q_init + 0.5 * epsilon * hamiltonian.dtau_dp(z);
          hamiltonian.update_metric(z, logger);

          delta_q -= z.q;
          if (delta_q.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
        hamiltonian.update_gradients(z, logger);
      }

      void end_update_p(typename Hamiltonian::PointType& z,
                        Hamiltonian& hamiltonian,
                        double epsilon,
                        callbacks::logger& logger) {
        hat_tau(z, hamiltonian, epsilon, 1, logger);
        hat_phi(z, hamiltonian, epsilon, logger);
      }

      // hat{phi} = dphi/dq * d/dp
      void hat_phi(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   callbacks::logger& logger) {
        z.p -= epsilon * hamiltonian.dphi_dq(z, logger);
      }

      // hat{tau} = dtau/dq * d/dp
      void hat_tau(typename Hamiltonian::PointType& z,
                   Hamiltonian& hamiltonian,
                   double epsilon,
                   int num_fixed_point,
                   callbacks::logger& logger) {
        Eigen::VectorXd p_init = z.p;
        Eigen::VectorXd delta_p(z.p.size());

        for (int n = 0; n < num_fixed_point; ++n) {
          delta_p = z.p;
          z.p.noalias() = p_init - epsilon * hamiltonian.dtau_dq(z, logger);
          delta_p -= z.p;
          if (delta_p.cwiseAbs().maxCoeff() < this->fixed_point_threshold_)
            break;
        }
      }

      int max_num_fixed_point() {
        return this->max_num_fixed_point_;
      }

      void set_max_num_fixed_point(int n) {
        if (n > 0) this->max_num_fixed_point_ = n;
      }

      double fixed_point_threshold() {
        return this->fixed_point_threshold_;
      }

      void set_fixed_point_threshold(double t) {
        if (t > 0) this->fixed_point_threshold_ = t;
      }

    private:
      int max_num_fixed_point_;
      double fixed_point_threshold_;
    };

  }  // mcmc
}  // stan

#endif


/*=========================================================================
 *
 * Copyright Universitat Pompeu Fabra, Department of Information and
 * Comunication Technologies.
 *
 *  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.txt
 *
 *  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 itkCudaRWSegmentationFilter_hxx
#define itkCudaRWSegmentationFilter_hxx

#include <itkImageRegionIterator.h>
#include <itkImageRegionConstIterator.h>

#include <vector>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#define CLEANUP(s)     \
  do                   \
  {                    \
    printf("%s\n", s); \
    fflush(stdout);    \
  } while (0)

namespace itk
{
template <typename TInputImage, typename TOutputImage>
void CudaRWSegmentationFilter<TInputImage, TOutputImage>::GenerateData()
{
  if (m_LabelImage == nullptr) // Exit if SetLabelImage has not been called
  {
    std::cout << "Label image has not been set" << std::endl;
    return;
  }
  if (OutputImageType::ImageDimension != 2 && OutputImageType::ImageDimension != 3)
  {
    std::cout << "Exit segmentation. Image dimension must be 2 or 3 but is " << OutputImageType::ImageDimension << std::endl;
    return;
  }

  // Get regions to iterate for original and label images
  typename OutputImageType::RegionType regionLabel = m_LabelImage->GetLargestPossibleRegion();

  // Set label image iterator to get the bounding box size and the quantity of marked nodes
  typedef itk::ImageRegionIterator<OutputImageType> IteratorLabelType;
  IteratorLabelType itLabel(m_LabelImage, regionLabel);

  // Crop image to bounds containin labels
  typename OutputImageType::RegionType regionLabelCrop;
  typename InputImageType::RegionType regionCrop;

  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionLabelCrop.SetSize(i, regionLabel.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabel.GetSize()[i]);
  }

  int markedLength = 0;

  itLabel.GoToBegin();
  while (!itLabel.IsAtEnd())
  {
    // Get image boundaries to crop image
    if (itLabel.Get() != 0)
    {
      ++markedLength;
      // Get bounding region
      for (int i = 0; i != OutputImageType::ImageDimension; ++i)
      {
        if (itLabel.GetIndex()[i] < regionLabelCrop.GetIndex()[i])
        {
          regionCrop.SetIndex(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetIndex(i, itLabel.GetIndex()[i]);
        }
        else if (itLabel.GetIndex()[i] > regionLabelCrop.GetSize()[i])
        {
          regionCrop.SetSize(i, itLabel.GetIndex()[i]);
          regionLabelCrop.SetSize(i, itLabel.GetIndex()[i]);
        }
      }
    }
    ++itLabel;
  }

  int totalNodes = 1;
  for (int i = 0; i != OutputImageType::ImageDimension; ++i)
  {
    regionCrop.SetIndex(i, regionCrop.GetIndex()[i]);
    regionLabelCrop.SetIndex(i, regionLabelCrop.GetIndex()[i]);
    regionCrop.SetSize(i, regionCrop.GetSize()[i] - regionCrop.GetIndex()[i] + 1);
    regionLabelCrop.SetSize(i, regionLabelCrop.GetSize()[i] - regionLabelCrop.GetIndex()[i] + 1);

    totalNodes *= regionCrop.GetSize()[i];
  }

  /////////////////////// Build Graph /////////////////////////////
  /*
    // Create graph. Each node will correspond to a pixel connected to its neighbors by an edge.
    // Neighbors are those nodes which distance is 1 ( d = sqrt((x-xi)² + (y-yi)² + (z-zi)²) = 1 ),
    // where x, y, and z are the indices of a pixel and xi, yi, and zi are the indeces of a neighboring pixel
    // Iterate through all pixels of input and label images
    */

  int unmarkedLength = totalNodes - markedLength; /* Quantity of unmarked nodes */

  // Define vectors to store graph data
  std::vector<float> *nodes = new std::vector<float>(totalNodes);  /* Pixel intensity for all nodes/pixels */
  std::vector<float> *labels = new std::vector<float>(totalNodes); /* Label of each node/pixel */
  /*  For node 'i'. If 'i' is a marked node, previousFound->at(i) is how many unmarked nodes there are before node 'i'.
        If 'i' is an unmarked node, previousFound->at(i) is how many marked nodes there are before node 'i'.
        This values are needed to build ordered Lu and BT matrices.
    */
  std::vector<int> *previousFound = new std::vector<int>(totalNodes);
  std::vector<int> *unmarked = new std::vector<int>(unmarkedLength); /* Indices of unmarked nodes ordered */
  std::vector<int> *marked = new std::vector<int>(markedLength);     /* Store labels of marked nodes */
  std::vector<int> *markedIdx = new std::vector<int>(markedLength);  /* Indices of marked nodes ordered */
  std::vector<int> *nameLabels = new std::vector<int>();             /* Values of the different labels of the prior */

  // Set bounding box image iterators
  typedef itk::ImageRegionConstIterator<InputImageType> ConstIteratorImageType;
  ConstIteratorImageType itImageCrop(this->GetInput(), regionCrop);
  IteratorLabelType itLabelCrop(m_LabelImage, regionLabelCrop);

  int foundMarked = 0;
  int foundUnmarked = 0;
  int NodeIdx = 0;

  itImageCrop.GoToBegin();
  itLabelCrop.GoToBegin();
  while (!itImageCrop.IsAtEnd())
  {
    // Store intensity in a std::vector
    nodes->at(NodeIdx) = itImageCrop.Get();
    // Store labels in a std::vector. Each index of 'nodes' and 'labels' correspond to the same pixel
    labels->at(NodeIdx) = itLabelCrop.Get();

    if (itLabelCrop.Get() == 0)
    {
      // Get the index of each node that is unmarked
      unmarked->at(foundUnmarked) = NodeIdx;
      // Store how many marked points have been found before this unmarked node
      previousFound->at(NodeIdx) = foundMarked;
      ++foundUnmarked;
    }
    else
    {
      // Set to label value if label != 0
      marked->at(foundMarked) = itLabelCrop.Get();
      // Get the index of each node that is marked
      markedIdx->at(foundMarked) = NodeIdx;
      // Store how many unmarked points have been found before this marked node
      previousFound->at(NodeIdx) = foundUnmarked;
      ++foundMarked;
      bool found;

      // Store the different labels in a std::vector
      found = std::find(nameLabels->begin(), nameLabels->end(), itLabelCrop.Get()) != nameLabels->end();
      if (!found)
      {
        nameLabels->push_back(itLabelCrop.Get());
      }
    }
    ++itImageCrop;
    ++itLabelCrop;
    ++NodeIdx;
  }

  // Sort labels
  std::sort(nameLabels->begin(), nameLabels->end());

  int totalLabels = nameLabels->size();
  if (m_SolveForAllLabels)
  {
    totalLabels += 1;
  }
  // Linear system: Lu * X = -BT * M
  // Convert marked (M) into a Eigen::Sparse matrix markedRHS. Needed for the computation of -BT * M (Eigen::SparseMatrix * Eigen::SparseMatrix)
  Eigen::SparseMatrix<float, Eigen::ColMajor> *markedRHS = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);
  for (int i = 0; i != totalLabels - 1; ++i)
  {
    int pos = 0;
    for (auto itMarked = marked->begin(); itMarked != marked->end(); ++itMarked)
    {
      if (*itMarked == nameLabels->at(i))
        markedRHS->insert(pos, i) = 1;
      ++pos;
    }
  }

  marked->clear();
  delete marked;

  /////////////////////// Build Laplacian matrix /////////////////////////////
  // Normalize intensity gradient over image spacing
  std::vector<float> spacing;
  typename InputImageType::SpacingType space = this->GetInput()->GetSpacing();
  if (OutputImageType::ImageDimension == 2)
    spacing = {space[1], space[0], space[0], space[1]};
  else if (OutputImageType::ImageDimension == 3)
    spacing = {space[2], space[1], space[0], space[0], space[1], space[2]};

  std::vector<int> neighbors;
  int x = regionCrop.GetSize()[0];
  int y = regionCrop.GetSize()[1];
  //  Right hand of the equation: -BT * M
  Eigen::SparseMatrix<float, Eigen::ColMajor> *BTxM = new Eigen::SparseMatrix<float, Eigen::ColMajor>(markedLength, totalLabels - 1);

  // Build BT
  // Compare whether NumCols < NumRows for less iterations during BT building
  if (markedIdx->size() < unmarked->size())
  {
    Eigen::SparseMatrix<float, Eigen::ColMajor> *BT = new Eigen::SparseMatrix<float, Eigen::ColMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through marked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itMarked = markedIdx->begin(); itMarked != markedIdx->end(); ++itMarked)
    {
      valNode = nodes->at(*itMarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itMarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) == 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(neighbors.at(i) - previousFound->at(neighbors.at(i)), node - previousFound->at(node)) = -w;
        }
      }
    }
    markedIdx->clear();
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedIdx, markedRHS, BT;
  }
  else
  {
    markedIdx->clear();
    delete markedIdx;

    Eigen::SparseMatrix<float, Eigen::RowMajor> *BT = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, markedLength);
    BT->reserve(Eigen::VectorXi::Constant(markedLength, 6));

    int node;
    float valNode, valNeighbor, w;

    // Iterate through unmarked nodes to build BT. Rows correspond to unmarked nodes, columns to marked nodes
    for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
    {
      valNode = nodes->at(*itUnmarked); // Intensity of node
      // Obtain neighbors indexes. right and left, top and bottom, front and back.
      node = *itUnmarked;
      if (OutputImageType::ImageDimension == 2)
        neighbors = {node - x, node - 1, node + 1, node + x};
      else if (OutputImageType::ImageDimension == 3)
        neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
      for (int i = 0; i != neighbors.size(); ++i)
      {
        // Make sure all the neighbors computed fall within the bounding box dimension
        if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes && labels->at(neighbors.at(i)) != 0)
        {
          valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
          w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
          //  Columns of BT correspond to marked nodes, rows to unmarked
          BT->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
        }
      }
    }
    //  Right hand of the equation: -BT * M
    *BTxM = -*BT * *markedRHS;
    BT->resize(0, 0);
    BT->data().squeeze();
    markedRHS->resize(0, 0);
    markedRHS->data().squeeze();
    delete markedRHS, BT;
  }

  int nnz = 0;

  // Build Lu. LHS of the equation
  Eigen::SparseMatrix<float, Eigen::RowMajor> *Lu = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, unmarkedLength);
  Lu->reserve(Eigen::VectorXi::Constant(unmarkedLength, 7));

  // Iterate through unmarked nodes to build Lu. Rows correspond to unmarked nodes, columns to unmarked nodes
  for (auto itUnmarked = unmarked->begin(); itUnmarked != unmarked->end(); ++itUnmarked)
  {
    int node;
    float valNode, valNeighbor, w, degree;

    valNode = nodes->at(*itUnmarked); // Intensity of node
    // Obtain neighbors indexes. right and left, top and bottom, front and back.
    node = *itUnmarked;
    if (OutputImageType::ImageDimension == 2)
      neighbors = {node - x, node - 1, node + 1, node + x};
    else if (OutputImageType::ImageDimension == 3)
      neighbors = {node - x * y, node - x, node - 1, node + 1, node + x, node + x * y};
    degree = 0.0;
    for (int i = 0; i != neighbors.size(); ++i)
    {
      // Make sure all the neighbors computed fall within the bounding box dimension
      if (neighbors.at(i) >= 0 && neighbors.at(i) < totalNodes)
      {
        valNeighbor = nodes->at(neighbors.at(i));                                    // Intensity of neighbor pixel
        w = (exp(-m_Beta * pow((valNode - valNeighbor) / spacing.at(i), 2)) + 1e-6); // Intensity gradient following a Gaussian function
        degree += w;                                                                 // Sum of the weights
        //  Columns of Lu correspond to unmarked nodes
        if (labels->at(neighbors.at(i)) == 0) // If neighbor is an unmarked node, build Lu
        {
          Lu->insert(node - previousFound->at(node), neighbors.at(i) - previousFound->at(neighbors.at(i))) = -w;
          ++nnz;
        }
      }
    }
    // Add node degree to diagonal of Lu
    Lu->insert(node - previousFound->at(node), node - previousFound->at(node)) = degree;
    ++nnz;
  }
  Lu->makeCompressed();

  Eigen::SparseMatrix<float, Eigen::RowMajor> *Lu_PreconditionerGpu = new Eigen::SparseMatrix<float, Eigen::RowMajor>(unmarkedLength, unmarkedLength);
  Lu_PreconditionerGpu->reserve(Eigen::VectorXi::Constant(unmarkedLength, 1));
  for (int i = 0; i != unmarkedLength; ++i)
    Lu_PreconditionerGpu->insert(i, i) = 1 / Lu->coeffRef(i, i);
  Lu_PreconditionerGpu->makeCompressed();

  nodes->clear();
  unmarked->clear();
  previousFound->clear();
  neighbors.clear();
  delete nodes, unmarked, previousFound;

  // /////////////////////// Solve linear system /////////////////////////////
  // Convert BTxM into a float array for bicgstab cuda solver
  float *bhost_all_labels = new float[unmarkedLength * (totalLabels - 1)];

  for (int i = 0; i != unmarkedLength * (totalLabels - 1); ++i)
    bhost_all_labels[i] = 0;

  for (int k = 0; k < BTxM->outerSize(); ++k)
  {
    for (Eigen::SparseMatrix<float, Eigen::ColMajor>::InnerIterator itMat(*BTxM, k); itMat; ++itMat)
    {
      bhost_all_labels[itMat.row() + k * unmarkedLength] = itMat.value();
    }
  }

  // Get pointers for CSR matrices Lu and Lu_PreconditionerGpu
  cooValAhost = Lu->valuePtr();
  csrColPtrAhost = Lu->outerIndexPtr();
  cooRowPtrAhost = Lu->innerIndexPtr();

  cooValAhostM = Lu_PreconditionerGpu->valuePtr();
  csrColPtrAhostM = Lu_PreconditionerGpu->outerIndexPtr();
  cooRowPtrAhostM = Lu_PreconditionerGpu->innerIndexPtr();

  int bicgstab_exit_status, cuda_mem_cpy_exit_status, cuda_mem_free_exit_status;

  // Result vector
  probabilities = new float[unmarkedLength * (totalLabels - 1)];
  probabilitiesInner = new float[unmarkedLength];

  // Allocate GPU memory
  cuda_mem_cpy_exit_status = this->AllocGPUMemory(unmarkedLength, nnz);
  if (cuda_mem_cpy_exit_status != 0)
  {
    std::cout << "GPU memory allocation failed with error " << cuda_mem_cpy_exit_status << std::endl;
    return;
  }

  // Solve S-1 linear systems
  for (int linearSystem = 0; linearSystem != totalLabels - 1; ++linearSystem)
  {
    // Get pinter for each linear system RHS
    bhost = &bhost_all_labels[linearSystem * unmarkedLength];
    // Call CUDA BiCGStab solver
    bicgstab_exit_status = this->BiCGStab(unmarkedLength, nnz);
    if (bicgstab_exit_status != 0)
    {
      std::cout << "BiCStab failed with error" << bicgstab_exit_status << std::endl;
      return;
    }

    for (int i = 0; i != unmarkedLength; ++i)
      probabilities[i + linearSystem * unmarkedLength] = probabilitiesInner[i];
  }

  // Free GPU memeory when all linear systems have been solved
  cuda_mem_free_exit_status = this->FreeGPUMemory();
  if (cuda_mem_free_exit_status != 0)
  {
    std::cout << "GPU memory free failed with error" << cuda_mem_free_exit_status << std::endl;
  }
  Lu->resize(0, 0);
  Lu->data().squeeze();
  Lu_PreconditionerGpu->resize(0, 0);
  Lu_PreconditionerGpu->data().squeeze();
  BTxM->resize(0, 0);
  BTxM->data().squeeze();
  delete Lu, Lu_PreconditionerGpu, BTxM;
  delete[] probabilitiesInner, bhost, bhost_all_labels, cooRowPtrAhost, csrColPtrAhost, cooValAhost, cooRowPtrAhostM, csrColPtrAhostM, cooValAhostM;

  std::vector<int> *RWLabels = new std::vector<int>(unmarkedLength);

  /*  Assign a label to each unmarked node according to the result of the solver.
      The label that is assigned is that one corresponding to the highest probability.
      Since we solver for S-1 systems, last label probability is computed by subtraction */
  for (int i = 0; i != unmarkedLength; ++i)
  {
    float maxProbability = probabilities[i];
    int maxLabelPos = 0;
    float accumulatedProbability = maxProbability;
    for (int j = 1; j != totalLabels - 1; ++j)
    {
      accumulatedProbability += probabilities[i + j * unmarkedLength];
      if (probabilities[i + j * unmarkedLength] > maxProbability)
      {
        maxProbability = probabilities[i + j * unmarkedLength];
        maxLabelPos = j;
      }
    }
    RWLabels->at(i) = 0.95 - accumulatedProbability > maxProbability ? nameLabels->back() : nameLabels->at(maxLabelPos);
  }

  delete[] probabilities;

  int valBackground;
  if (!m_WriteBackground)
    valBackground = 0;
  else
    valBackground = nameLabels->back();

  typename OutputImageType::Pointer outputLabels = this->GetOutput();

  outputLabels->Graft(m_LabelImage);
  outputLabels->FillBuffer(valBackground);

  // Iterate through original label image to create segmentation image
  // Assign labels known from label image to their original value. Assign unmarked labels
  // according to the result from the solver

  IteratorLabelType itOut1(outputLabels, regionLabelCrop);

  // Set computed labels to segmentation image
  int unmarkedIdx = 0;
  int idxOutput = 0;

  itOut1.GoToBegin();
  while (!itOut1.IsAtEnd())
  {
    if (labels->at(idxOutput) == 0)
    {
      if (RWLabels->at(unmarkedIdx) != nameLabels->back())
        itOut1.Set(RWLabels->at(unmarkedIdx));
      else
        itOut1.Set(valBackground);
      ++unmarkedIdx;
    }
    else if (labels->at(idxOutput) != nameLabels->back())
      itOut1.Set(labels->at(idxOutput));

    ++idxOutput;
    ++itOut1;
  }

  labels->clear();
  RWLabels->clear();
  nameLabels->clear();
  delete labels, RWLabels, nameLabels;

  return;
}

template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::BiCGStab(int M, int nnz)
{

  int iter, flag;

  cusparseStatus_t status;
  cusparseHandle_t handle = 0;
  cusparseMatDescr_t descra = 0;

  int nnzM = M;
  int N = M;
  int N2 = N;

  float timesOne[1] = {1.0};
  float timesZero[1] = {0.0};
  float timesMinusOne[1] = {-1.0};

  cudaStat4 = cudaMemcpy(r, bhost,
                         (size_t)(N * sizeof(r[0])),
                         cudaMemcpyHostToDevice);

  cudaStat5 = cudaMemcpy(xdev, xhost,
                         (size_t)(N * sizeof(xdev[0])),
                         cudaMemcpyHostToDevice);

  // Copy right hand side to GPU memory
  if ((cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess))
  {
    CLEANUP("Memcpy from Host to Device failed");
    return EXIT_FAILURE;
  }

  /* initialize cusparse library */
  status = cusparseCreate(&handle);
  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("CUSPARSE Library initialization failed");
    return EXIT_FAILURE;
  }
  /* create and setup matrix descriptor */
  status = cusparseCreateMatDescr(&descra);
  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("Matrix descriptor initialization failed");
    return EXIT_FAILURE;
  }
  cusparseSetMatType(descra, CUSPARSE_MATRIX_TYPE_GENERAL);
  cusparseSetMatIndexBase(descra, CUSPARSE_INDEX_BASE_ZERO);

  bnrm2 = cublasSnrm2(N, r, 1);

  float tol = m_Tolerance * bnrm2;

  status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesMinusOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, xdev, timesOne, r); /* r = r - A*x; r is now residual */

  if (status != CUSPARSE_STATUS_SUCCESS)
  {
    CLEANUP("Matrix‐vector multiplication failed");
    return EXIT_FAILURE;
  }

  error = cublasSnrm2(N, r, 1) / bnrm2; /* norm_r = norm(b) */
  if (error < tol)
  {
    CLEANUP("Error smaller than tolerance failed");
    return EXIT_FAILURE; /* x is close enough already */
  }

  omega = 1.0;

  cudaStat1 = cudaMemcpy(r_tld, r, (size_t)(N * sizeof(r[0])),
                         cudaMemcpyDeviceToDevice); /* r_tld = r */

  if ((cudaStat1 != cudaSuccess))
  {
    CLEANUP("Memcpy from r to r_tld failed");
    return EXIT_FAILURE;
  }

  // Loop of the BiCGStab solver
  for (iter = 0; iter < m_MaximumNumberOfIterations; ++iter)
  {
    rho = cublasSdot(N, r_tld, 1, r, 1); /* rho = r_tld'*r */

    if (rho == 0.0)
      break;

    if (iter > 0)
    {
      beta = (rho / rho_1) * (alpha / omega);
      cublasSaxpy(N, -omega, v, 1, p, 1);
      cublasSaxpy(N, 1.0 / beta, r, 1, p, 1);
      cublasSscal(N, beta, p, 1); /* p = r + beta*( p - omega*v ) */
    }
    else
    {
      cudaStat1 = cudaMemcpy(p, r,
                             (size_t)(N * sizeof(r[0])),
                             cudaMemcpyDeviceToDevice); /* p = r */
      if ((cudaStat1 != cudaSuccess))
      {
        CLEANUP("Memcpy from r to p failed");
        return EXIT_FAILURE;
      }
    }

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N2, N2, nnzM, timesOne, descra, cooValAdevM, csrColPtrAdevM, cooRowPtrAdevM, p, timesZero, p_hat);

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, p_hat, timesZero, v); /* v = A*p_hat */

    alpha = rho / cublasSdot(N, r_tld, 1, v, 1); /* alph = rho / ( r_tld'*v ) */

    cudaStat1 = cudaMemcpy(s, r, (size_t)(N * sizeof(r[0])),
                           cudaMemcpyDeviceToDevice); /* s = r */
    if ((cudaStat1 != cudaSuccess))
    {
      CLEANUP("Memcpy from r to s failed");
      return EXIT_FAILURE;
    }
    cublasSaxpy(N, -alpha, v, 1, s, 1);
    snrm2 = cublasSnrm2(N, s, 1);

    cublasSaxpy(N, alpha, p_hat, 1, xdev, 1); /*  h = x + alph*p_hat */

    if (snrm2 < tol)
    {
      // cublasSaxpy(N, alpha, p_hat, 1, s, 1);
      resid = snrm2 / bnrm2;

      cudaStat5 = cudaMemcpy(probabilitiesInner, xdev,
                             (size_t)(N * sizeof(probabilitiesInner[0])),
                             cudaMemcpyDeviceToHost); /* x = h */
      if ((cudaStat5 != cudaSuccess))
      {
        CLEANUP("Memcpy from x to xhost failed");
        return EXIT_FAILURE;
      }

      break;
    }

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N2, N2, nnzM, timesOne, descra, cooValAdevM, csrColPtrAdevM, cooRowPtrAdevM, s, timesZero, s_hat);

    status = cusparseScsrmv(handle, CUSPARSE_OPERATION_NON_TRANSPOSE, N, N, nnz, timesOne, descra, cooValAdev, csrColPtrAdev, cooRowPtrAdev, s_hat, timesZero, t); /* t = A*s_hat */

    omega = cublasSdot(N, t, 1, s, 1) / cublasSdot(N, t, 1, t, 1); /* omega = ( t'*s) / ( t'*t ) */

    cublasSaxpy(N, omega, s_hat, 1, xdev, 1); /*  x = h + omega*s_hat */

    cudaStat1 = cudaMemcpy(r, s, (size_t)(N * sizeof(r[0])),
                           cudaMemcpyDeviceToDevice); /* r = s */
    if ((cudaStat1 != cudaSuccess))
    {
      CLEANUP("Memcpy from s to r failed");
      return EXIT_FAILURE;
    }

    cublasSaxpy(N, -omega, t, 1, r, 1); /* r = s - omega*t */
    snrm2 = cublasSnrm2(N, r, 1);

    if (snrm2 <= tol)
    {
      resid = snrm2 / bnrm2;

      cudaStat5 = cudaMemcpy(probabilitiesInner, xdev,
                             (size_t)(N * sizeof(probabilitiesInner[0])),
                             cudaMemcpyDeviceToHost); /* x = x */
      if ((cudaStat5 != cudaSuccess))
      {
        CLEANUP("Memcpy from x to xhost failed");
        return EXIT_FAILURE;
      }
      break;
    }
    if (omega == 0.0)
      break;
    rho_1 = rho;
  }
  ++iter;

  error = snrm2 / bnrm2;

  if (error <= tol)
  {
    flag = 0;
  }
  else if (omega == 0.0)
  {
    flag = -2;
  }
  else if (rho == 0.0)
  {
    flag = -1;
  }
  else
    flag = 1;

  m_SolverIterations += iter;
  m_SolverError = std::min(m_SolverError, error);

  return flag;
}

// Allocate GPU memory and copy data to device
template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::AllocGPUMemory(int M, int nnz)
{

  int nnzM = M; // Lu has size MxM, Lu_PreconditionerGpu has size MxM
  int N = M;

  // Define initial guess
  xhost = new float[N];
  for (int i = 0; i < N; i++)
  {
    xhost[i] = 0;
  }

  cudaStat1 = cudaMalloc((void **)&cooRowPtrAdev, nnz * sizeof(cooRowPtrAdev[0]));
  cudaStat3 = cudaMalloc((void **)&cooValAdev, nnz * sizeof(cooValAdev[0]));
  cudaStat4 = cudaMalloc((void **)&r, N * sizeof(r[0]));
  cudaStat5 = cudaMalloc((void **)&s, N * sizeof(s[0]));
  cudaStat6 = cudaMalloc((void **)&t, N * sizeof(t[0]));
  cudaStat7 = cudaMalloc((void **)&r_tld, N * sizeof(r_tld[0]));
  cudaStat8 = cudaMalloc((void **)&p, N * sizeof(p[0]));
  cudaStat9 = cudaMalloc((void **)&p_hat, N * sizeof(p_hat[0]));
  cudaStat10 = cudaMalloc((void **)&s_hat, N * sizeof(s_hat[0]));
  cudaStat11 = cudaMalloc((void **)&v, N * sizeof(v[0]));
  cudaStat12 = cudaMalloc((void **)&xdev, N * sizeof(xdev[0]));

  cudaStat13 = cudaMalloc((void **)&csrColPtrAdev, (N + 1) * sizeof(csrColPtrAdev[0]));

  cudaStat14 = cudaMalloc((void **)&cooRowPtrAdevM, nnzM * sizeof(cooRowPtrAdevM[0]));
  cudaStat16 = cudaMalloc((void **)&cooValAdevM, nnzM * sizeof(cooValAdevM[0]));
  cudaStat17 = cudaMalloc((void **)&csrColPtrAdevM, (N + 1) * sizeof(csrColPtrAdevM[0]));

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess) ||
      (cudaStat6 != cudaSuccess) ||
      (cudaStat7 != cudaSuccess) ||
      (cudaStat8 != cudaSuccess) ||
      (cudaStat9 != cudaSuccess) ||
      (cudaStat10 != cudaSuccess) ||
      (cudaStat11 != cudaSuccess) ||
      (cudaStat12 != cudaSuccess) ||

      (cudaStat14 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess))
  {
    CLEANUP("Device malloc failed");
    return EXIT_FAILURE;
  }

  cudaStat1 = cudaMemcpy(cooRowPtrAdev, cooRowPtrAhost,
                         (size_t)(nnz * sizeof(cooRowPtrAdev[0])),
                         cudaMemcpyHostToDevice);
  cudaStat13 = cudaMemcpy(csrColPtrAdev, csrColPtrAhost,
                          (size_t)((N + 1) * sizeof(csrColPtrAdev[0])),
                          cudaMemcpyHostToDevice);
  cudaStat3 = cudaMemcpy(cooValAdev, cooValAhost,
                         (size_t)(nnz * sizeof(cooValAdev[0])),
                         cudaMemcpyHostToDevice);

  cudaStat14 = cudaMemcpy(cooRowPtrAdevM, cooRowPtrAhostM,
                          (size_t)(nnzM * sizeof(cooRowPtrAdevM[0])),
                          cudaMemcpyHostToDevice);
  cudaStat17 = cudaMemcpy(csrColPtrAdevM, csrColPtrAhostM,
                          (size_t)((N + 1) * sizeof(cooColPtrAdevM[0])),
                          cudaMemcpyHostToDevice);
  cudaStat16 = cudaMemcpy(cooValAdevM, cooValAhostM,
                          (size_t)(nnzM * sizeof(cooValAdevM[0])),
                          cudaMemcpyHostToDevice);

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat13 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat14 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess))
  {
    CLEANUP("Memcpy from Host to Device failed");
    return EXIT_FAILURE;
  }

  return 0;
}

// Free GPU memeory when all linear systems have been solved
template <typename TInputImage, typename TOutputImage>
int CudaRWSegmentationFilter<TInputImage, TOutputImage>::FreeGPUMemory()
{
  /* shutdown CUBLAS */
  cublas_status = cublasShutdown();
  if (cublas_status != CUBLAS_STATUS_SUCCESS)
  {
    fprintf(stderr, "!!!! shutdown error (A)\n");
    return EXIT_FAILURE;
  }

  cudaStat1 = cudaFree(cooRowPtrAdev);
  cudaStat3 = cudaFree(cooValAdev);
  cudaStat4 = cudaFree(r);
  cudaStat5 = cudaFree(s);
  cudaStat6 = cudaFree(t);
  cudaStat7 = cudaFree(r_tld);
  cudaStat8 = cudaFree(p);
  cudaStat9 = cudaFree(p_hat);
  cudaStat10 = cudaFree(s_hat);
  cudaStat11 = cudaFree(v);
  cudaStat12 = cudaFree(xdev);

  cudaStat13 = cudaFree(csrColPtrAdev);

  cudaStat14 = cudaFree(cooRowPtrAdevM);
  cudaStat16 = cudaFree(cooValAdevM);
  cudaStat17 = cudaFree(csrColPtrAdevM);

  if ((cudaStat1 != cudaSuccess) ||
      (cudaStat3 != cudaSuccess) ||
      (cudaStat4 != cudaSuccess) ||
      (cudaStat5 != cudaSuccess) ||
      (cudaStat6 != cudaSuccess) ||
      (cudaStat7 != cudaSuccess) ||
      (cudaStat8 != cudaSuccess) ||
      (cudaStat9 != cudaSuccess) ||
      (cudaStat10 != cudaSuccess) ||
      (cudaStat11 != cudaSuccess) ||
      (cudaStat12 != cudaSuccess) ||

      (cudaStat14 != cudaSuccess) ||
      (cudaStat16 != cudaSuccess) ||
      (cudaStat17 != cudaSuccess))
  {
    CLEANUP("Device memory free failed");
    return EXIT_FAILURE;
  }

  return 0;
}

template <typename TInputImage, typename TOutputImage>
void CudaRWSegmentationFilter<TInputImage, TOutputImage>::PrintSelf(std::ostream &os, Indent indent) const
{
  Superclass::PrintSelf(os, indent);
  os << indent << "Beta: " << m_Beta << std::endl;
  os << indent << "Tolerance: " << m_Tolerance << std::endl;
  os << indent << "MaximumNumberOfIterations: " << m_MaximumNumberOfIterations << std::endl;
  os << indent << "WriteBackground: " << m_WriteBackground << std::endl;
}

} // namespace itk
#endif


// Prepare for G2O-------------------------------------------------

#include <Eigen/StdVector>
#include <iostream>

#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>

#include "targetTypes6D.hpp"
// #include "continuous_to_discrete.h"
// #include "targetTypes3D.hpp"
using namespace Eigen;
using namespace std;
using namespace g2o;

// Prepare for ROS-------------------------------------------------
#include <ros/ros.h>
#include <ros/time.h>
#include "sensor_msgs/Imu.h"
// #include "sensor_msgs/MagneticField.h"
// #include "sensor_msgs/Temperature.h"
// #include "sensor_msgs/FluidPressure.h"
// #include "sensor_msgs/Joy.h"
// #include "sensor_msgs/NavSatFix.h"
// #include "sensor_msgs/Range.h"

// #include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
// #include "geometry_msgs/PoseStamped.h"
// #include "geometry_msgs/TwistWithCovarianceStamped.h"
// #include <geometry_msgs/Vector3Stamped.h>
// #include <sensor_fusion_comm/ExtEkf.h>
#include <sensor_fusion_comm/DoubleArrayStamped.h>

// #include "nav_msgs/Odometry.h"
// #include "crossover_nav/Ack.h"
// #include "crossover_nav/Navdata.h"
// #include "crossover_nav/odom_data.h"
// #include "crossover_nav/Status.h"

#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Quaternion.h>

// #include <dynamic_reconfigure/BoolParameter.h>
// #include <dynamic_reconfigure/Reconfigure.h>
// #include <dynamic_reconfigure/Config.h>

// #include <termios.h>

//function convert arduino to ros
//------------ROS version-------
// #include <iostream>
inline long millis() {
  return (1e3 * ros::Time::now().sec + 1e-6 * ros::Time::now().nsec);
}
inline long micros() {
  return (1e6 * ros::Time::now().sec + 1e-3 * ros::Time::now().nsec);
}
//not use millis() it give me online floating point behind sec
//go use ros::Time::now();
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

#define delay(x) ros::Duration(x/1000.0).sleep()
#define say(x) (std::cout << x)
#define sayend(x) (std::cout << x << std::endl)
#define saytab(x) (std::cout << x << "\t")
#define saycomma(x) (std::cout << x << " ,")
#define Min(a,b) ((a)<(b)?(a):(b))
#define Max(a,b) ((a)>(b)?(a):(b))
#define isfinite(X) std::isfinite(X)
#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))
#define wrap_pi(x) (x < -3.14 ? x+6.28 : (x > 3.14 ? x - 6.28: x))
#define HALF_M_PI 1.570796

sensor_msgs::Imu imu;
geometry_msgs::PoseWithCovarianceStamped gps_pose , poseop_msgs;
sensor_fusion_comm::DoubleArrayStamped state_out_msf;
#define NUM_ITERATE 1

Vector3d gps[5];
Vector6d msf_state[17];
Vector3d acc[17];
double dtime[200];
int Iterate = 0;
Vector6d state;
int Numimu = 0;
int Numstate = 0;

int num_state = 0; //round max = 17
float GPS_lOOCKUP[5][2] = {{0,0},
                         {3,0},
                         {3,-3},
                         {0,-3},
                         {0,0}
                          };

float STATE_LOOCKUP[17][2];

// void imucallback(const sensor_msgs::Imu::ConstPtr &data)
// {
//   // imu = *data;
//   sensor_msgs::Imu imu_bf = *data;

//   static ros::Time st = ros::Time::now();
//   static ros::Duration ct = ros::Time::now()-st;
//   static ros::Duration pt= ct;
//   ct = ros::Time::now()-st;
//   dtime[Numimu] = (ct - pt).toSec();

//   //transform bf to ef
//   tf::Quaternion qimu(imu_bf.linear_acceleration.x,imu_bf.linear_acceleration.y,imu_bf.linear_acceleration.z,0);
//   tf::Quaternion q(imu_bf.orientation.x,imu_bf.orientation.y,imu_bf.orientation.z,imu_bf.orientation.w);

//   qimu = q*qimu*q.inverse() - tf::Quaternion(0,0,9.81,0);

//   imu.linear_acceleration.x = qimu.x();
//   imu.linear_acceleration.y = qimu.y();
//   imu.linear_acceleration.z = qimu.z();
//   imu.orientation = imu_bf.orientation;

//   acc[Numimu][0] = imu.linear_acceleration.x;
//   acc[Numimu][1] = imu.linear_acceleration.y;
//   acc[Numimu][2] = imu.linear_acceleration.z;

//   Numimu++;
//   // tf::Quaternion q(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w);
//   // tf::Matrix3x3 m(q);
//   // m.getRPY(roll, pitch, yaw);
//   //ROS_INFO("ax:[%f]", yaw);
// }
// void gpscallback(/*const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& data*/) {
//   // gps_pose = *data;

//   if(Iterate>=NUM_ITERATE) return;

//   gps[num_state][0] = GPS_lOOCKUP[0][0];
//   gps[num_state][1] = gps_pose.pose.pose.position.y;
//   gps[num_state][2] = 0;

//   Iterate++;

// }
// void state_out_callback(/*const sensor_fusion_comm::DoubleArrayStamped::ConstPtr& data*/) {

//   state_out_msf = *data;

//   for(int i =0;i<6;i++)
//     msf_state[Numstate][i] = state_out_msf.data[i];
//   Numstate++;
// }

void Optimize_test(double dt);

int main( int argc, char** argv )
{
  ros::init(argc, argv, "gps_tester");
  ros::NodeHandle n;
  ros::Rate r(200);

  // ros::Subscriber imu_sub           = n.subscribe<sensor_msgs::Imu>("/imu_max", 2, imucallback);
  // ros::Subscriber gps_sub           = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/imu_max/pose", 2, gpscallback);
  // ros::Subscriber state_out_msf_sub = n.subscribe<sensor_fusion_comm::DoubleArrayStamped>("/msf_core/state_out", 2, state_out_callback);
  ros::Publisher  poseop_pub        = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/optimized", 10);

  static ros::Time start_time = ros::Time::now();
  static ros::Duration cur_time = ros::Time::now()-start_time;
  static ros::Duration prev_time= cur_time;
  static double dt = 0.25;

  poseop_msgs.header.frame_id = "odom";

  state_out_msf.data.resize(36);
  // state.setZero();
  // Iterate over the simulation steps

  state.setZero();
  for (int k = 0; k < 3; k++)
    {
      state[k] = 0;
    }

  STATE_LOOCKUP[0][0] = 0;
  STATE_LOOCKUP[0][1] = 0;

  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i][0] = (3.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i][1] = 0;
    if(i==1 || i==4) {
      acc[i] = {16*3/3,0,0};
      if(i==4) acc[i]*=-1;
    }else
      acc[i] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+4][0] = 3.0+(1.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i+4][1] = -i;
    if(i==1 || i==4) {
      acc[i+4] = {16*1/3,-16*4/3,0};
      if(i==4) acc[i+4]*=-1;
    }else
      acc[i+4] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+8][0] = 4.0+ (-3.0/4.0)*i;
    /*y*/STATE_LOOCKUP[i+8][1] = -4.0 + (1.0/4.0)*i;
    if(i==1 || i==4) {
      acc[i+8] = {-16*3/3,16*1/3,0};
      if(i==4) acc[i+8]*=-1;
    }else
      acc[i+8] = {0,0,0};
  }
  for(int i=1;i<=4;i++) {
    /*x*/STATE_LOOCKUP[i+12][0] = 1.0;
    /*y*/STATE_LOOCKUP[i+12][1] = -3.0+i;
    if(i==1 || i==4) {
      acc[i+12] = {16*0/3,16*4/3,0};
      if(i==4) acc[i+12]*=-1;
    }else
      acc[i+12] = {0,0,0};
  }

  for(int i=0;i<17;i++) {
   saytab(acc[i][0]); sayend(acc[i][1]);
 }

  while(ros::ok() && Iterate <= 3)
    {

        gps[Iterate+1][0] = GPS_lOOCKUP[Iterate+1][0];
        gps[Iterate+1][1] = GPS_lOOCKUP[Iterate+1][1];
        gps[Iterate+1][2] = 0;

        for(int i = 1;i<=4;i++) {
          msf_state[Iterate*4+i][0] = STATE_LOOCKUP[Iterate*4+i][0];
          msf_state[Iterate*4+i][1] = STATE_LOOCKUP[Iterate*4+i][1];
          msf_state[Iterate*4+i][2] = 0;
          msf_state[Iterate*4+i][3] = 0;
          msf_state[Iterate*4+i][4] = 0;
          msf_state[Iterate*4+i][5] = 0;
          // Numstate++;
        }

        Iterate ++;
      }
        saytab("START Iterate = ");sayend(Iterate);
        Optimize_test(dt);
        sayend("END");

}

void Optimize_test(double dt) {
 // Set up the parameters of the simulation
  int numberOfTimeSteps = NUM_ITERATE;
  const double processNoiseSigma = 0.05;
  const double accelerometerNoiseSigma = 0.01;
  const double gpsNoiseSigma = 0.001;
  // const double dt = 1;

  // Set up the optimiser and block solver
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  typedef BlockSolver< BlockSolverTraits<6, 6> > BlockSolver;
  BlockSolver::LinearSolverType * linearSolver
      = new LinearSolverCholmod<BlockSolver::PoseMatrixType>();
  BlockSolver* blockSolver = new BlockSolver(linearSolver);
  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
  optimizer.setAlgorithm(optimizationAlgorithm);

  // Sample the start location of the target

    state = msf_state[0];

  // Construct the first vertex; this corresponds to the initial
  // condition and register it with the optimiser
  saytab("add vertex state ");sayend(0);
  VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();
  stateNode->setEstimate(state);
  stateNode->setId(0);
  optimizer.addVertex(stateNode);

  // Construct the GPS observation
  Vector3d gpsMeasurement = gps[0];
  saytab("add edge gps ");sayend(0);
  // Add the GPS observation
  GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
  goe->setMeasurement(gpsMeasurement);
  goe->setVertex(0, stateNode);
  optimizer.addEdge(goe);

  // Construct the msf observation
  Vector3d msfMeasurement(0,0,0);

  saytab("add edge statemsf ");sayend(0);
  // Add the msf observation
  GPSObservationEdgePositionVelocity3D* goe2 = new GPSObservationEdgePositionVelocity3D(msfMeasurement, 0.1);
  goe2->setMeasurement(msfMeasurement);
  goe2->setVertex(0, stateNode);
  optimizer.addEdge(goe2);

  // Set up last estimate
  VertexPositionVelocity3D* lastStateNode = stateNode;

  // Iterate over the simulation steps
  for (int k = 0; k < Iterate; ++k)
    {
      for(int j = 1; j <= 4/*Numstate per round*/; j++)
      {
        // saytab(j);saytab(Numstate);sayend(Iterate);

          // state = msf_state[k*4+j];

          // Construct vertex which corresponds to the current state of the target
          VertexPositionVelocity3D* stateNode = new VertexPositionVelocity3D();

          stateNode->setId(k*4+j);
          stateNode->setEstimate(state);
          stateNode->setMarginalized(false);
          optimizer.addVertex(stateNode);

          saytab("add vertex state ");sayend(k*4+j);

          // // Construct the accelerometer measurement
          Vector3d accelerometerMeasurement/*(msf_state[k*4+j][0],msf_state[k*4+j][1],0);//*/ = acc[k*4+j];

          TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(dt, accelerometerNoiseSigma);
          toe->setVertex(0, lastStateNode);
          toe->setVertex(1, stateNode);
          VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
          VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
          toe->setMeasurement(accelerometerMeasurement);
          optimizer.addEdge(toe);
          // compute the initial guess via the odometry
          g2o::OptimizableGraph::VertexSet vPrevSet;
          vPrevSet.insert(vPrev);
          toe->initialEstimate(vPrevSet,vCurr);

          lastStateNode = stateNode;
      }
                // Construct the msf observation
          Vector3d gpsMeasurement2 = gps[k+1];

          // Add the msf observation
          GPSObservationEdgePositionVelocity3D* goe2 = new GPSObservationEdgePositionVelocity3D(gpsMeasurement2, 0.1);
          goe2->setMeasurement(gpsMeasurement2);
          goe2->setVertex(0, stateNode);
          optimizer.addEdge(goe2);

        // Construct the GPS observation
        Vector3d gpsMeasurement = gps[k+1];

      saytab("add edge gps ");sayend(k+1);
      // Add the GPS observation
      GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
      goe->setMeasurement(gpsMeasurement);
      goe->setVertex(0, stateNode);
      optimizer.addEdge(goe);
    }

  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
  cerr << "number of edges:" << optimizer.edges().size() << endl;

  // Print the results

  // cout << "state=\n" << state << endl;

#if 0
  for (int k = 0; k < numberOfTimeSteps; k++)
    {
      cout << "computed estimate " << k << "\n"
           << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
       }
#endif

  // Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
  // Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
  // cout << "v1=\n" << v1 << endl;
  // cout << "v2=\n" << v2 << endl;
  // cout << "delta state=\n" << v2-v1 << endl;
       // Vector6d v[4];
       Vector6d v[17];
       for(int i=0;i<17;i++)
          v[i] = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(i)->second)->estimate();
      for(int i=0;i<17;i++)
       cout << v[i][0] << "\t" << v[i][1] <<endl;

      cout << endl;
      cout << endl;
      cout << endl;
      for(int i=0;i<17;i++)
       cout << msf_state[i][0] << "\t" << msf_state[i][1] <<endl;

      cout << endl;
      cout << endl;
      cout << endl;
      for(int i=0;i<5;i++)
        cout << gps[i][0] << "\t" << gps[i][1] <<endl;
       // poseop_msgs.header.stamp = ros::Time::now();
       // poseop_msgs.pose.pose.position.x = v[0];
       // poseop_msgs.pose.pose.position.y = v[1];
       // poseop_msgs.pose.pose.position.z = v[2];

       // poseop_msgs.pose.pose.orientation = imu.orientation;

      SparseBlockMatrix<MatrixXd> spinv;

      optimizer.computeMarginals(spinv, stateNode);

      //optimizer.solver()->computeMarginals();

      // covariance
      //
      cout << "covariance\n" << spinv << endl;

      // for(int u =0;u<6;u++)
      //   for(int v=0;v<6;v++)
      //     poseop_msgs.pose.covariance[6*u+v] = spinv(u, v);
      poseop_msgs.pose.covariance[0]=0.1;
      poseop_msgs.pose.covariance[7]=0.1;
      poseop_msgs.pose.covariance[14]=0.1;
      poseop_msgs.pose.covariance[21]=0.1;
      poseop_msgs.pose.covariance[28]=0.1;
      poseop_msgs.pose.covariance[35]=0.1;

}

// #ifndef G2O_TARGET_TYPES_6D_HPP_
// #define G2O_TARGET_TYPES_6D_HPP_

// #include <g2o/core/base_vertex.h>
// #include <g2o/core/base_binary_edge.h>
// #include <g2o/core/base_unary_edge.h>
// #include <Eigen/Core>

// using namespace g2o;

// typedef Eigen::Matrix<double,6,1> Vector6d;
// typedef Eigen::Matrix<double,6,6> Matrix6d;

// // This header file specifies a set of types for the different
// // tracking examples; note that

// class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d>
// {
// public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//   VertexPosition3D()
//   {
//   }

//   virtual void setToOriginImpl() {
//     _estimate.setZero();
//   }

//   virtual void oplusImpl(const double* update)
//   {
//     _estimate[0] += update[0];
//     _estimate[1] += update[1];
//     _estimate[2] += update[2];
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// };

// class PositionVelocity3DEdge
// {
// };

// class VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d>
// {
// public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//   VertexPositionVelocity3D()
//   {
//   }

//   virtual void setToOriginImpl() {
//     _estimate.setZero();
//   }

//   virtual void oplusImpl(const double* update)
//   {
//     for (int k = 0; k < 6; k++)
//       _estimate[k] += update[k];
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// };

// // The odometry which links pairs of nodes together
// class TargetOdometry3DEdge : public g2o::BaseBinaryEdge<6, Eigen::Vector3d, VertexPositionVelocity3D, VertexPositionVelocity3D>
// {
// public:
//   TargetOdometry3DEdge(double dt, double noiseSigma)
//   {
//     _dt = dt;

//     double q = noiseSigma * noiseSigma;
//     double dt2 = dt * dt;

//     // Process noise covariance matrix; this assumes an "impulse"
//     // noise model; we add a small stabilising term on the diagonal to make it invertible
//     Matrix6d Q=Matrix6d::Zero();
//     Q(0, 0) = Q(1,1) = Q(2,2) = dt2*dt2*q/4 + 1e-4;
//     Q(0, 3) = Q(1, 4) = Q(2, 5) = dt*dt2*q/2;
//     Q(3, 3) = Q(4,4) = Q(5,5) = dt2 * q + 1e-4;
//     Q(3, 0) = Q(4, 1) = Q(5, 2) = dt*dt2*q/2;

//     setInformation(Q.inverse());
//   }

//   /** set the estimate of the to vertex, based on the estimate of the from vertex in the edge. */
//   virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to){
//     assert(from.size() == 1);
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
//     VertexPositionVelocity3D* vj = static_cast<VertexPositionVelocity3D*>(to);
//     Vector6d viEst=vi->estimate();
//     Vector6d vjEst=viEst;

//     for (int m = 0; m < 3; m++)
//     {
//       vjEst[m] += _dt * (vjEst[m+3] + 0.5 * _dt * _dt* _measurement[m]);
//     }

//     for (int m = 0; m < 3; m++)
//     {
//       vjEst[m+3] =0;//+= _dt * _measurement[m];
//     }

//     vjEst[0] = _measurement[0];
//     vjEst[1] = _measurement[1];
//     vjEst[2] = 0;
//     vj->setEstimate(vjEst);
//   }

//   /** override in your class if it's not possible to initialize the vertices in certain combinations */
//   virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
//     //only works on sequential vertices
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
//     return (to->id() - vi->id() == 1) ? 1.0 : -1.0;
//   }

//   void computeError()
//   {
//     const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
//     const VertexPositionVelocity3D* vj = static_cast<const VertexPositionVelocity3D*>(_vertices[1]);

//     for (int k = 0; k < 3; k++)
//       {
//         _error[k] = vi->estimate()[k] /*+ _dt * (vi->estimate()[k+3] + 0.5 * _dt * _measurement[k]) - vj->estimate()[k]*/-_measurement[k];
//       }
//     for (int k = 3; k < 6; k++)
//       {
//         _error[k] = 0;//vi->estimate()[k] + _dt * _measurement[k-3]- vj->estimate()[k];
//       }
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }

// private:
//   double _dt;
// };

// // The GPS
// class GPSObservationEdgePositionVelocity3D : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPositionVelocity3D>
// {
// public:
//   GPSObservationEdgePositionVelocity3D(const Eigen::Vector3d& measurement, double noiseSigma)
//   {
//     setMeasurement(measurement);
//     setInformation(Eigen::Matrix3d::Identity() / (noiseSigma*noiseSigma));
//   }

//   void computeError()
//   {
//     const VertexPositionVelocity3D* v = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
//     for (int k = 0; k < 3; k++)
//       {
//         _error[k] = v->estimate()[k] - _measurement[k];
//       }
//   }

//   virtual bool read(std::istream& /*is*/)
//   {
//     return false;
//   }

//   virtual bool write(std::ostream& /*os*/) const
//   {
//     return false;
//   }
// };

// #endif //  __TARGET_TYPES_6D_HPP__


// Volumetric3D_EMC.cpp
// created by Kuangdai on 16-May-2017
// genetral Volumetric3D model with IRIS-EMC format

#include "Volumetric3D_EMC.h"
#include <sstream>
#include "XMPI.h"
#include "XMath.h"
#include "Geodesy.h"
#include "Parameters.h"
#include "NetCDF_Reader.h"
#include "NetCDF_ReaderAscii.h"

#include <dirent.h>
#include <sys/types.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <algorithm>

bool compareFunc(const std::pair<std::string, float> &a,
    const std::pair<std::string, float> &b) {
    return a.second < b.second;
}

void Volumetric3D_EMC::initialize() {
    // meta data
    Eigen::Matrix<float, Eigen::Dynamic, 1> fdata, fdep, flat, flon;
    if (XMPI::root()) {
        std::vector<size_t> dims;
        std::string fname = Parameters::sInputDirectory + "/" + mFileName;
        if (mOneFilePerDepth) {
            // get all files
            DIR *dir = opendir(fname.c_str());
            if (!dir) {
                throw std::runtime_error("Volumetric3D_EMC::initialize || "
                    "Error opening directory of data files || Directory = " + fname);
            }
            struct dirent *entry;
            std::vector<std::pair<std::string, float>> fileDepth;
            while ((entry = readdir(dir)) != NULL) {
                // file name
                std::string fn(entry->d_name);
                if (fn.find(".grd") == std::string::npos) {
                    continue;
                }
                // depth
                std::string depthStr(fn);
                boost::replace_first(depthStr, ".grd", "");
                std::size_t found = depthStr.find_last_of("_");
                if (found == std::string::npos) {
                    throw std::runtime_error("Volumetric3D_EMC::initialize || "
                        "Error processing file name || File = " + fn);
                }
                depthStr = depthStr.substr(found + 1);
                float depth = 0.;
                try {
                    depth = boost::lexical_cast<float>(depthStr);
                } catch (std::exception) {
                    throw std::runtime_error("Volumetric3D_EMC::initialize || "
                        "Error processing file name || File = " + fn);
                }
                // add pair
                fileDepth.push_back(std::make_pair(fname + "/" + fn, depth));
            }
            closedir(dir);

            // sort depth
            std::sort(fileDepth.begin(), fileDepth.end(), compareFunc);

            // read lat and lon
            Eigen::Matrix<double, Eigen::Dynamic, 1> dlat, dlon;
            if (NetCDF_Reader::checkNetCDF_isAscii(fileDepth[0].first)) {
                NetCDF_ReaderAscii reader;
                reader.open(fileDepth[0].first);
                reader.read1D("lat", dlat);
                reader.read1D("lon", dlon);
                reader.close();
            } else {
                NetCDF_Reader reader;
                reader.open(fileDepth[0].first);
                reader.read1D("lat", dlat);
                reader.read1D("lon", dlon);
                reader.close();
            }
            flat = dlat.cast<float>();
            flon = dlon.cast<float>();

            // depths and data
            size_t depthLen = flat.size() * flon.size();
            size_t totalLen = depthLen * fileDepth.size();
            fdep.resize(fileDepth.size());
            fdata.resize(totalLen);
            for (int i = 0; i < fileDepth.size(); i++) {
                fdep(i) = fileDepth[i].second;
                Eigen::Matrix<float, Eigen::Dynamic, 1> temp;
                if (NetCDF_Reader::checkNetCDF_isAscii(fileDepth[i].first)) {
                    NetCDF_ReaderAscii reader;
                    reader.open(fileDepth[i].first);
                    reader.readMetaData(mVarName, temp, dims);
                    reader.close();
                } else {
                    NetCDF_Reader reader;
                    reader.open(fileDepth[i].first);
                    reader.readMetaData(mVarName, temp, dims);
                    reader.close();
                }
                fdata.block(i * depthLen, 0, depthLen, 1) = temp;
            }
            dims.insert(dims.begin(), fdep.size());

            // perturbations are given in percentage
            fdata *= .01f;
        } else {
            if (NetCDF_Reader::checkNetCDF_isAscii(fname)) {
                NetCDF_ReaderAscii reader;
                reader.open(fname);
                reader.read1D("depth", fdep);
                reader.read1D("latitude", flat);
                reader.read1D("longitude", flon);
                reader.readMetaData(mVarName, fdata, dims);
                reader.close();
            } else {
                NetCDF_Reader reader;
                reader.open(fname);
                reader.read1D("depth", fdep);
                reader.read1D("latitude", flat);
                reader.read1D("longitude", flon);
                reader.readMetaData(mVarName, fdata, dims);
                reader.close();
            }
        }

        // check dimensions
        if (dims.size() != 3) {
            throw std::runtime_error("Volumetric3D_EMC::initialize || Inconsistent data dimensions || "
                "File/Directory = " + fname);
        }
        if (dims[0] != fdep.size() || dims[1] != flat.size() || dims[2] != flon.size()) {
            throw std::runtime_error("Volumetric3D_EMC::initialize || Inconsistent data dimensions || "
                "File/Directory = " + fname);
        }
        if (!XMath::sortedAscending(fdep) || !XMath::sortedAscending(flat) || !XMath::sortedAscending(flon)) {
            throw std::runtime_error("Volumetric3D_EMC::initialize || Grid coordinates are not sorted ascendingly || "
                "File/Directory = " + fname);
        }
    }
    XMPI::bcastEigen(fdep);
    XMPI::bcastEigen(flat);
    XMPI::bcastEigen(flon);
    XMPI::bcastEigen(fdata);

    mGridDep = fdep.cast<double>();
    mGridLat = flat.cast<double>();
    mGridLon = flon.cast<double>();
    RDColX data = fdata.cast<double>();

    // SI
    mGridDep *= 1e3;
    if (mReferenceType == Volumetric3D::MaterialRefType::Absolute) {
        // convert to SI
        data *= MaterialPropertyAbsSI[mMaterialProp];
    }

    // apply factor
    data *= mFactor;

    // special flag
    if (!boost::iequals(mModelFlag, "none")) {
        if (boost::iequals(mModelFlag, "abs")) {
            data.array() = data.array().abs();
            data *= mModelFlagFactor;
        } else if (boost::iequals(mModelFlag, "pow")) {
            double absmax = data.array().abs().maxCoeff();
            double scalefact = std::abs(absmax / std::pow(absmax, mModelFlagFactor));
            data.array() = data.array().sign() * (data.array().abs().pow(mModelFlagFactor) * scalefact);
        } else {
            throw std::runtime_error("Volumetric3D_EMC::initialize || "
                "Unknown special model flag, flag = " + mModelFlag);
        }
    }

    // reshape data
    int pos = 0;
    for (int i = 0; i < mGridDep.size(); i++) {
        RDMatXX mat(mGridLat.size(), mGridLon.size());
        for (int j = 0; j < mGridLat.size(); j++) {
            for (int k = 0; k < mGridLon.size(); k++) {
                mat(j, k) = data(pos++);
            }
        }
        mGridData.push_back(mat);
    }
}

void Volumetric3D_EMC::initialize(const std::vector<std::string> &params) {
    if (params.size() < 4) throw std::runtime_error("Volumetric3D_EMC::initialize || "
        "Not enough parameters to initialize a Volumetric3D_EMC object, at least 4 needed.");

    const std::string source = "Volumetric3D_EMC::initialize";

    // initialize data
    Parameters::castValue(mFileName, params[0], source);
    Parameters::castValue(mVarName, params[1], source);

    // property name
    bool found = false;
    for (int i = 0; i < Volumetric3D::MaterialPropertyString.size(); i++) {
        if (boost::iequals(params[2], Volumetric3D::MaterialPropertyString[i])) {
            mMaterialProp = Volumetric3D::MaterialProperty(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_EMC::initialize || "
            "Unknown material property, name = " + params[2]);
    }

    // reference type
    found = false;
    for (int i = 0; i < Volumetric3D::MaterialRefTypeString.size(); i++) {
        if (boost::iequals(params[3], Volumetric3D::MaterialRefTypeString[i]) ||
            boost::iequals(params[3], Volumetric3D::MaterialRefTypeStringShort[i])) {
            mReferenceType = Volumetric3D::MaterialRefType(i);
            found = true;
            break;
        }
    }
    if (!found) {
        throw std::runtime_error("Volumetric3D_EMC::initialize || "
            "Unknown material reference type, type = " + params[3]);
    }

    try {
        int ipar = 4;
        Parameters::castValue(mFactor, params.at(ipar++), source);
        Parameters::castValue(mGeographic, params.at(ipar++), source);
        Parameters::castValue(mOneFilePerDepth, params.at(ipar++), source);
        Parameters::castValue(mVerticalDiscontinuities, params.at(ipar++), source);
        Parameters::castValue(mModelFlag, params.at(ipar++), source);
        Parameters::castValue(mModelFlagFactor, params.at(ipar++), source);
    } catch (std::out_of_range) {
        // nothing
    }

    if (!boost::iequals(mModelFlag, "none")) {
        if (mReferenceType == MaterialRefType::Absolute) {
            throw std::runtime_error("Volumetric3D_EMC::initialize || "
                "Imposing special model flag on an absolute model.");
        }
    }
    initialize();
}

bool Volumetric3D_EMC::get3dProperties(double r, double theta, double phi, double rElemCenter,
    std::vector<MaterialProperty> &properties,
    std::vector<MaterialRefType> &refTypes,
    std::vector<double> &values) const {

    // header
    properties = std::vector<MaterialProperty>(1, mMaterialProp);
    refTypes = std::vector<MaterialRefType>(1, mReferenceType);
    values = std::vector<double>(1, 0.);

    // to geocentric
    if (mGeographic) {
        // which radius to use?
        theta = pi / 2. - Geodesy::theta2Lat_d(theta, 0.) * degree;
    }

    // regularise
    double dep = Geodesy::getROuter() - r;
    double lat = 90. - theta / degree;
    double lon = phi / degree;
    XMath::checkLimits(dep, 0., Geodesy::getROuter());
    XMath::checkLimits(lat, -90., 90.);
    if (mGridLon[0] < 0.) {
        // lon starts from -180.
        if (lon > 180.) {
            lon -= 360.;
        }
        XMath::checkLimits(lon, -180., 180.);
    } else {
        // lon starts from 0.
        XMath::checkLimits(lon, 0., 360.);
    }

    // check center
    double dmin = mGridDep[0];
    double dmax = mGridDep[mGridDep.size() - 1];
    double dcenter = Geodesy::getROuter() - rElemCenter;
    if (dcenter < dmin || dcenter > dmax) {
        return false;
    }
    if (dep < dmin && dep > dmin * 0.999999) {
        dep = dmin;
    }
    if (dep > dmax && dep < dmax * 1.000001) {
        dep = dmax;
    }

    // interpolation
    int ldep0, llat0, llon0, ldep1, llat1, llon1;
    double wdep0, wlat0, wlon0, wdep1, wlat1, wlon1;
    if (mVerticalDiscontinuities) {
        // use element center depth to locate layer
        XMath::interpLinear(dcenter, mGridDep, ldep0, wdep0);
        if (ldep0 < 0) {
            return false;
        }
        // use point depth to determine value
        wdep0 = 1. - 1. / (mGridDep(ldep0 + 1) - mGridDep(ldep0)) * (dep - mGridDep(ldep0));
    } else {
        XMath::interpLinear(dep, mGridDep, ldep0, wdep0);
    }
    XMath::interpLinear(lat, mGridLat, llat0, wlat0);
    XMath::interpLinear(lon, mGridLon, llon0, wlon0);
    if (ldep0 < 0 || llat0 < 0 || llon0 < 0) {
        return false;
    }

    ldep1 = ldep0 + 1;
    llat1 = llat0 + 1;
    llon1 = llon0 + 1;
    wdep1 = 1. - wdep0;
    wlat1 = 1. - wlat0;
    wlon1 = 1. - wlon0;

    values[0] += mGridData[ldep0](llat0, llon0) * wdep0 * wlat0 * wlon0;
    values[0] += mGridData[ldep0](llat1, llon0) * wdep0 * wlat1 * wlon0;
    values[0] += mGridData[ldep0](llat0, llon1) * wdep0 * wlat0 * wlon1;
    values[0] += mGridData[ldep0](llat1, llon1) * wdep0 * wlat1 * wlon1;
    values[0] += mGridData[ldep1](llat0, llon0) * wdep1 * wlat0 * wlon0;
    values[0] += mGridData[ldep1](llat1, llon0) * wdep1 * wlat1 * wlon0;
    values[0] += mGridData[ldep1](llat0, llon1) * wdep1 * wlat0 * wlon1;
    values[0] += mGridData[ldep1](llat1, llon1) * wdep1 * wlat1 * wlon1;
    return true;
}

std::string Volumetric3D_EMC::verbose() const {
    std::stringstream ss;
    ss << "\n======================= 3D Volumetric =======================" << std::endl;
    ss << "  Model Name           =   EMC" << std::endl;
    ss << "  Data File            =   " << mFileName << std::endl;
    ss << "  Variable Name        =   " << mVarName << std::endl;
    ss << "  Material Property    =   " << MaterialPropertyString[mMaterialProp] << std::endl;
    ss << "  Reference Type       =   " << MaterialRefTypeString[mReferenceType] << std::endl;
    ss << "  Num. Depths          =   " << mGridDep.size() << std::endl;
    ss << "  Num. Latitudes       =   " << mGridLat.size() << std::endl;
    ss << "  Num. Longitudes      =   " << mGridLon.size() << std::endl;
    ss << "  Depth Range          =   [" << mGridDep.minCoeff() << ", " << mGridDep.maxCoeff() << "]" << std::endl;
    ss << "  Latitude Range       =   [" << mGridLat.minCoeff() << ", " << mGridLat.maxCoeff() << "]" << std::endl;
    ss << "  Longitude Range      =   [" << mGridLon.minCoeff() << ", " << mGridLon.maxCoeff() << "]" << std::endl;
    ss << "  Factor               =   " << mFactor << std::endl;
    ss << "  Use Geographic       =   " << (mGeographic ? "YES" : "NO") << std::endl;
    ss << "  One File per Depth   =   " << (mOneFilePerDepth ? "YES" : "NO") << std::endl;
    if (!boost::iequals(mModelFlag, "none")) {
        ss << "  Special Model Flag   =   " << mModelFlag << std::endl;
        ss << "  Model Flag Factor    =   " << mModelFlagFactor << std::endl;
    }
    ss << "======================= 3D Volumetric =======================\n" << std::endl;
    return ss.str();
}


// #ifdef HAVE_CONFIG_H
// #include "config.h"
// #endif

// #include <assert.h>
// #include <stdlib.h>
// #include <algorithm>
// #include <vector>
// #include <boost/tuple/tuple.hpp>
// #include <boost/tuple/tuple_comparison.hpp>

// #ifdef _OPENMP
// #include <omp.h>
// #endif

// #include <rysq.hpp>
// #include "cxx/sugar.hpp"
// #include "quadrature.h"
// #include "memory.h"

// using namespace rysq;

// parallel::Fock::Fock(const Quartet<Shell> &shells) : shells(shells) {}

// parallel::Fock::~Fock() {}

// void parallel::Fock::operator()(const std::vector<Center> &centers,
// 				const std::vector<Int4> &quartets,
// 				const matrix::Adapter &D,
// 				matrix::Adapter &F,
// 				const Parameters &parameters) {

//     if (quartets.empty()) return;

// #ifdef _OPENMP
// #pragma omp parallel
// #endif
//     {

// #ifdef _OPENMP
// 	int num_threads = omp_get_num_threads();
// 	int thread_num = omp_get_thread_num();
// #else
// 	int num_threads = 1;
// 	int thread_num = 0;
// #endif

// 	rysq::Fock fock(this->shells);

// 	int nij = shells.size(0,1);
// 	int nkl = shells.size(2,3);
// 	int nik = shells.size(0,2);
// 	int nil = shells.size(0,3);
// 	int njk = shells.size(1,2);
// 	int njl = shells.size(1,3);

// 	memory::Map<double> myMemory(32*1024);

// #ifdef _OPENMP
// #pragma omp for schedule(dynamic,1) nowait
// #endif
// 	for (uint q = 0; q < quartets.size(); ++q) {

// 	    int i, j, k, l;
// 	    util::unpack(quartets[q], i, j, k, l);

// 	    double *Fij = myMemory.get(i, j, nij);
// 	    double *Fkl = myMemory.get(k, l, nkl);
// 	    double *Fik = myMemory.get(i, k, nik);
// 	    double *Fil = myMemory.get(i, l, nil);
// 	    double *Fjk = myMemory.get(j, k, njk);
// 	    double *Fjl = myMemory.get(j, l, njl);

// 	    Quartet<Center> C(centers[i], centers[j], centers[k], centers[l]);

// 	    matrix::Density<6> D6 = matrix::index(D, i, j, k, l);
// 	    double *f[] = { Fij, Fkl, Fik, Fil, Fjk, Fjl };
// 	    matrix::Fock<6> F6(f);

// 	    Parameters p = parameters;
// 	    p.scale /= Quartet<Shell>::symmetry(i, j, k, l);

// 	    // fock(C, D6, F6, p);

// 	}

// 	typedef boost::tuple<int,int,double*,size_t> Block;
// 	std::vector<Block> blocks;
// 	typedef std::vector<Block>::iterator block_it;

// 	while (!myMemory.empty()) {
// 	    memory::Map<double>::Entry kv = myMemory.pop();
// 	    blocks.push_back(Block(kv.j, kv.i, kv.ptr, kv.n));
// 	}
// 	std::sort(blocks.begin(), blocks.end());

// 	std::vector<block_it> columns;
// 	typedef std::vector<block_it>::iterator column_it;
// 	block_it it = blocks.begin();

// 	while (it < blocks.end()) {
// 	    int j = it->get<0>();
// 	    columns.push_back(it);
// 	    while (it < blocks.end() && it->get<0>() == j) ++it;
// 	}
// 	columns.push_back(it);

// 	for (int t = 0; t < num_threads; ++t) {
// 	    int k = (thread_num + t)%num_threads;

// 	    for (column_it col = columns.begin(); col < columns.end() - 1; ++col) {
// 		block_it block = *col;
// 		int j = block->get<0>();
// 		if (j%num_threads != k) continue;

// 		while (block < *(col+1)) {
// 		    double *dest = F(block->get<1>(), block->get<0>());
// 		    double *source = block->get<2>();
// 		    size_t size = block->get<3>();
// 		    util::add(size, dest, source);
// 		    ++block;
// 		}
// 	    }
// #ifdef _OPENMP
// #pragma omp barrier
// #endif
// 	}

//     }

// }