#pragma once

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

#include <boost/graph/graph_traits.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include "graphviz_interface.hpp"

#include "DiGraph.hpp"
#include "tran_mat_cell.hpp"
#include <fmt/format.h>

const size_t DEFAULT_N_SAMPLES = 200;

enum InitialBeta { ZERO, ONE, HALF, MEAN, RANDOM };

typedef std::unordered_map<std::string, std::vector<double>>
    AdjectiveResponseMap;

typedef std::vector<std::vector<std::vector<std::vector<double>>>>
    ObservedStateSequence;

typedef std::vector<std::vector<std::vector<double>>>
    PredictedObservedStateSequence;

typedef std::pair<std::tuple<std::string, int, std::string>,
                  std::tuple<std::string, int, std::string>>
    CausalFragment;

typedef std::vector<std::vector<
    std::unordered_map<std::string, std::unordered_map<std::string, double>>>>
    FormattedPredictionResult;

typedef std::tuple<std::pair<std::pair<int, int>, std::pair<int, int>>,
                   std::vector<std::string>,
                   FormattedPredictionResult>
    Prediction;

AdjectiveResponseMap construct_adjective_response_map(size_t n_kernels);

/**
 * The AnalysisGraph class is the main model/interface for Delphi.
 */
class AnalysisGraph {

  DiGraph graph;

  public:
  AnalysisGraph() {}
  Node& operator[](std::string);
  Node& operator[](int);
  Edge& edge(int, int);
  size_t num_vertices();
  size_t num_edges();
  // Manujinda: I had to move this up since I am usign this within the private:
  // block This is ugly. We need to re-factor the code to make it pretty again
  auto node_indices() {
    return boost::make_iterator_range(boost::vertices(this->graph));
  };

  auto nodes(){
    using boost::adaptors::transformed;
    return this->node_indices() |
          transformed([&](int v) -> Node& { return (*this)[v]; });
  };

  boost::range_detail::integer_iterator<unsigned long> begin() {return boost::vertices(this->graph).first;};
  boost::range_detail::integer_iterator<unsigned long> end() {return boost::vertices(this->graph).second;};

  auto successors(int i);

  // Allocate a num_verts x num_verts 2D array (std::vector of std::vectors)
  void allocate_A_beta_factors();

  void print_A_beta_factors();

  private:
  void clear_state();

  // Maps each concept name to the vertex id of the
  // vertex that concept is represented in the CAG
  // concept name --> CAG vertex id
  std::unordered_map<std::string, int> name_to_vertex = {};

  // Keeps track of indicators in CAG to ensure there are no duplicates.
  // std::vector<std::string> indicators_in_CAG;
  std::unordered_set<std::string> indicators_in_CAG;

  // A_beta_factors is a 2D array (std::vector of std::vectors) that keeps track
  // of the β factors involved with each cell of the transition matrix A.
  //
  // Accordign to our current model, which uses variables and their partial
  // derivatives with respect to each other ( x --> y, βxy = ∂y/∂x ),
  // atmost half of the transition matrix cells can be affected by βs.
  // According to the way we organize the transition matrix, the cells
  // A[row][col] where row is an even index and col is an odd index
  // are such cells.
  //
  // Each cell of matrix A_beta_factors represent all the directed paths
  // starting at the vertex equal to the column index of the matrix and
  // ending at the vertex equal to the row index of the matrix.
  //
  // Each cell of matrix A_beta_factors is an object of Tran_Mat_Cell class.
  std::vector<std::vector<std::shared_ptr<Tran_Mat_Cell>>> A_beta_factors;

  // A set of (row, column) numbers of the 2D matrix A_beta_factors
  // where the cell (row, column) depends on β factors.
  std::set<std::pair<int, int>> beta_dependent_cells;

  // Maps each β to all the transition matrix cells that are dependent on it.
  std::multimap<std::pair<int, int>, std::pair<int, int>> beta2cell;

  double t = 0.0;
  double delta_t = 1.0;

  // Latent state that is evolved by sampling.
  // Since s0 is used to represent a sequence of latent states,
  // I named this s0_original. Once things are refactored, we might be able to
  // convert this to s0
  Eigen::VectorXd s0;

  // Transition matrix that is evolved by sampling.
  // Since variable A has been already used locally in other methods,
  // I chose to name this A_orginal. After refactoring the code, we could
  // rename this to A.
  Eigen::MatrixXd A_original;

  int n_timesteps;
  int pred_timesteps;
  std::pair<std::pair<int, int>, std::pair<int, int>> training_range;

  // This is a column of the
  // this->training_latent_state_sequences
  // prediction_initial_latent_states.size() = this->res
  // TODO: If we make the code using this variable to directly fetch the values
  // from this->training_latent_state_sequences, we can get rid of this
  std::vector<Eigen::VectorXd> prediction_initial_latent_states;
  std::vector<std::string> pred_range;

  // Access this as
  // prediction_latent_state_sequences[ sample ][ time step ]
  std::vector<std::vector<Eigen::VectorXd>> predicted_latent_state_sequences;

  // Access this as
  // prediction_observed_state_sequences
  //                            [ sample ][ time step ][ vertex ][ indicator ]
  std::vector<PredictedObservedStateSequence>
      predicted_observed_state_sequences;

  PredictedObservedStateSequence test_observed_state_sequence;

  // Sampling resolution. Default is 200
  int res = DEFAULT_N_SAMPLES;

  // Keep track whether the model is trained.
  // Used to check whether there is a trained model before calling
  // generate_prediction()
  bool trained = false;

  // Access this as
  // current_latent_state
  Eigen::VectorXd current_latent_state;

  // Access this as
  // observed_state_sequence[ time step ][ vertex ][ indicator ]
  ObservedStateSequence observed_state_sequence;

  std::vector<Eigen::MatrixXd> transition_matrix_collection;

  // Remember the old β and the edge where we perturbed the β.
  // We need this to revert the system to the previous state if the proposal
  // gets rejected.
  std::pair<boost::graph_traits<DiGraph>::edge_descriptor, double>
      previous_beta;

  double log_likelihood = 0.0;
  double previous_log_likelihood = 0.0;
  bool data_heuristic = false;

  void get_subgraph(int vert,
                    std::unordered_set<int>& vertices_to_keep,
                    int cutoff,
                    bool inward);

  void get_subgraph_between(int start,
                            int end,
                            std::vector<int>& path,
                            std::unordered_set<int>& vertices_to_keep,
                            int cutoff);

  /**
   * Finds all the simple paths starting at the start vertex and
   * ending at the end vertex.
   * Uses find_all_paths_between_util() as a helper to recursively find the
   * paths
   */
  void find_all_paths_between(int start, int end, int cutoff);

  /**
   * Recursively finds all the simple paths starting at the start vertex and
   * ending at the end vertex. Used by find_all_paths_between()
   * Paths found are added to the Tran_Mat_Cell object that is tracking the
   * transition matrix cell (2*end, 2*start)
   *
   * @param start: Start vertex of the path
   * @param end  : End vertex of the path
   * @param path : A path starting at vettex start that is being explored
   *
   * @return void
   */
  void find_all_paths_between_util(int start,
                                   int end,
                                   std::vector<int>& path,
                                   int cutoff);

  /*
   ==========================================================================
   Utilities
   ==========================================================================
  */
  void set_default_initial_state();

  std::mt19937 rand_num_generator;

  // Uniform distribution used by the MCMC sampler
  std::uniform_real_distribution<double> uni_dist;

  // Normal distrubution used to perturb β
  std::normal_distribution<double> norm_dist;

  int get_vertex_id_for_concept(std::string concept, std::string caller);

  int get_degree(int vertex_id);

  void remove_node(int node_id);

  public:
  ~AnalysisGraph() {}

  /**
   * A method to construct an AnalysisGraph object given a JSON-serialized list
   * of INDRA statements.
   *
   * @param filename: The path to the file containing the JSON-serialized INDRA
   * statements.
   */
  static AnalysisGraph from_json_file(std::string filename,
                                      double belief_score_cutoff = 0.9,
                                      double grounding_score_cutoff = 0.0,
                                      std::string ontology = "WM");

  /**
   * A method to construct an AnalysisGraph object given from a std::vector of
   * ( subject, object ) pairs (Statements)
   *
   * @param statements: A std::vector of CausalFragment objects
   */
  static AnalysisGraph
  from_causal_fragments(std::vector<CausalFragment> causal_fragments);

  // TODO Change the name of this function to something better, like
  // restrict_to_subgraph_for_concept, update docstring

  /**
   * Returns a new AnaysisGraph related to the concept provided,
   * which is a subgraph of this graph.
   *
   * @param concept: The concept where the subgraph is about.
   * @param depth  : The maximum number of hops from the concept provided
   *                 to be included in the subgraph.
   * #param inward : Sets the direction of the causal influence flow to
   *                 examine.
   *                 False - (default) A subgraph rooted at the concept
   * provided.
   *                 True  - A subgraph with all the paths ending at the concept
   * provided.
   */
  AnalysisGraph get_subgraph_for_concept(std::string concept,
                                         bool inward = false,
                                         int depth = -1);

  /**
   * Returns a new AnaysisGraph related to the source concept and the target
   * concetp provided, which is a subgraph of this graph.
   * This subgraph contains all the simple directed paths of length less than
   * or equal to the provided cutoff.
   *
   * @param source_concept: The concept where the influence starts.
   * @param target_concept: The concept where the influence ends.
   * @param cutoff        : Maximum length of a directed simple path from
   *                        the source to target to be included in the
   *                        subgraph.
   */
  AnalysisGraph get_subgraph_for_concept_pair(std::string source_concept,
                                              std::string target_concept,
                                              int cutoff = -1);

  void prune(int cutoff = 2);

  void add_node(std::string concept);

  void add_edge(CausalFragment causal_fragment);
  void change_polarity_of_edge(std::string source_concept,
                               int source_polarity,
                               std::string target_concept,
                               int target_polarity);
  void remove_node(std::string concept);

  // Note:
  //      Although just calling this->remove_node(concept) within the loop
  //          for( std::string concept : concept_s )
  //      is suffifient to implement this method, it is not very efficient.
  //      It re-calculates directed simple paths for each vertex removed
  //
  //      Therefore, the code in this->remove_node() has been duplicated with
  //      slightly different flow to achive a more efficient execution.
  void remove_nodes(std::unordered_set<std::string> concepts);

  void remove_edge(std::string src, std::string tgt);

  void remove_edges(std::vector<std::pair<std::string, std::string>> edges);

  auto edges() { return boost::make_iterator_range(boost::edges(graph)); }

  /** Number of nodes in the graph */
  int num_nodes() { return boost::num_vertices(graph); }

  auto predecessors(int i) {
    return boost::make_iterator_range(boost::inv_adjacent_vertices(i, graph));
  }

  // Merge node n1 into node n2, with the option to specify relative polarity.
  // void
  // merge_nodes_old(std::string n1, std::string n2, bool same_polarity = true);

  /**
   * Merges the CAG nodes for the two concepts concept_1 and concept_2
   * with the option to specify relative polarity.
   */
  void merge_nodes(std::string concept_1,
                   std::string concept_2,
                   bool same_polarity = true);

  auto out_edges(int i) {
    return boost::make_iterator_range(boost::out_edges(i, graph));
  }

  double get_beta(std::string source_vertex_name,
                  std::string target_vertex_name) {
    // This is ∂target / ∂source
    // return this->A_original(2 * this->name_to_vertex[target_vertex_name],
    //                        2 * this->name_to_vertex[source_vertex_name] + 1);
    return this->A_original(
        2 * get_vertex_id_for_concept(target_vertex_name, "get_beta()"),
        2 * get_vertex_id_for_concept(source_vertex_name, "get_beta()") + 1);
  }

  void construct_beta_pdfs();

  AnalysisGraph
  find_all_paths_for_concept(std::string concept, int depth, bool reverse);

  /*
   * Find all the simple paths between all the paris of nodes of the graph
   */
  void find_all_paths();

  /*
   * Prints the simple paths found between all pairs of nodes of the graph
   * Groupd according to the starting and ending vertex.
   * find_all_paths() should be called before this to populate the paths
   */
  void print_all_paths();

  // Given an edge (source, target vertex ids - i.e. a β ≡ ∂target/∂source),
  // print all the transition matrix cells that are dependent on it.
  void print_cells_affected_by_beta(int source, int target);

  /*
   ==========================================================================
   Sampling and inference
   ----------------------

   This section contains code for sampling and Bayesian inference.
   ==========================================================================
  */

  // Sample elements of the stochastic transition matrix from the
  // prior distribution, based on gradable adjectives.
  void sample_initial_transition_matrix_from_prior();

  /**
   * Utility function that converts a time range given a start date and end date
   * into an integer value.
   * At the moment returns the number of months withing the time range.
   * This should be the number of traing data time points we have
   *
   * @param start_year  : Start year of the training data sequence
   * @param start_month : Start month of the training data sequence
   * @param end_year    : End year of the training data sequence
   * @param end_month   : End month of the training data sequence
   *
   * @return            : Number of months in the training data sequence
   *                      Including both start and end months
   */
  int calculate_num_timesteps(int start_year,
                              int start_month,
                              int end_year,
                              int end_month);

  /**
   * Get the observed state (values for all the indicators)
   * for a given time point from data.
   * See data.hpp::get_data_value() for missing data rules.
   * Note: units are automatically set according
   * to the parameterization of the given CAG.
   *
   * @param year    : Year of the time point data is extracted
   * @param month   : Month of the time point data is extracted
   * @param country : Country where the data is about
   * @param state   : State where the data is about
   * @param county  : County where the data is about
   *
   * @return        : Observed state std::vector for the specified location
   *                  on the specified time point.
   *                  Access it as: [ vertex id ][ indicator id ]
   */
  std::vector<std::vector<std::vector<double>>>
  get_observed_state_from_data(int year,
                               int month,
                               std::string country = "South Sudan",
                               std::string state = "",
                               std::string county = "");

  /**
   * Set the observed state sequence for a given time range from data.
   * The sequence includes both ends of the range.
   * See data.hpp::get_data_value() for missing data rules.
   * Note: units are automatically set according
   * to the parameterization of the given CAG.
   *
   * @param start_year  : Start year of the sequence of data
   * @param start_month : Start month of the sequence of data
   * @param end_year    : End year of the sequence of data
   * @param end_month   : End month of the sequenec of data
   * @param country     : Country where the data is about
   * @param state       : State where the data is about
   * @param county      : County where the data is about
   *
   */
  void
  set_observed_state_sequence_from_data(int start_year,
                                        int start_month,
                                        int end_year,
                                        int end_month,
                                        std::string country = "South Sudan",
                                        std::string state = "",
                                        std::string county = "");

  /**
   * Utility function that sets an initial latent state from observed data.
   * This is used for the inference of the transition matrix as well as the
   * training latent state sequences.
   *
   * @param timestep: Optional setting for setting the initial state to be other
   *                  than the first time step. Not currently used.
   *                  0 <= timestep < this->n_timesteps
   */
  void set_initial_latent_state_from_observed_state_sequence();

  void initialize_random_number_generator();

  void set_random_initial_latent_state();

  /**
   * To help experiment with initializing βs to differet values
   *
   * @param ib: Criteria to initialize β
   */
  void init_betas_to(InitialBeta ib = InitialBeta::MEAN);

  /**
   * Train a prediction model given a CAG with indicators
   *
   * @param start_year  : Start year of the sequence of data
   * @param start_month : Start month of the sequence of data
   * @param end_year    : End year of the sequence of data
   * @param end_month   : End month of the sequenec of data
   * @param res         : Sampling resolution. The number of samples to retain.
   * @param burn        : Number of samples to throw away. Start retaining
   *                      samples after throwing away this many samples.
   * @param country     : Country where the data is about
   * @param state       : State where the data is about
   * @param county      : county where the data is about
   * @param units       : Units for each indicator. Maps
   *                      indicator name --> unit
   * @param initial_beta: Criteria to initialize β
   *
   */
  void train_model(int start_year = 2012,
                   int start_month = 1,
                   int end_year = 2017,
                   int end_month = 12,
                   int res = 200,
                   int burn = 10000,
                   std::string country = "South Sudan",
                   std::string state = "",
                   std::string county = "",
                   std::map<std::string, std::string> units = {},
                   InitialBeta initial_beta = InitialBeta::ZERO,
                   bool use_heuristic = false);

  /**
   * Sample a collection of observed state sequences from the likelihood
   * model given a collection of transition matrices.
   *
   * @param prediction_timesteps: The number of timesteps for the prediction
   * sequences.
   * @param initial_prediction_step: The initial prediction timestep relative
   *                                 to training timesteps.
   * @param total_timesteps: Total number of timesteps from the initial
   *                         training date to the end prediction date.
   */
  void sample_predicted_latent_state_sequences(int prediction_timesteps,
                                               int initial_prediction_step,
                                               int total_timesteps);

  /** Generate predicted observed state sequenes given predicted latent state
   * sequences using the emission model
   */
  void
  generate_predicted_observed_state_sequences_from_predicted_latent_state_sequences();

  /**
   * Given a trained model, generate this->res number of
   * predicted observed state sequences.
   *
   * @param start_year  : Start year of the prediction
   *                      Should be >= the start year of training
   * @param start_month : Start month of the prediction
   *                      If training and prediction start years are equal
   *                      should be >= the start month of training
   * @param end_year    : End year of the prediction
   * @param end_month   : End month of the prediction
   *
   * @return Predicted observed state (indicator value) sequence for the
   *         prediction period including start and end time points.
   *         This is a tuple.
   *         The first element is a std::vector of std::strings with lables for
   * each time point predicted (year-month). The second element contains
   * predicted values. Access it as: [ sample number ][ time point ][ vertex
   * name ][ indicator name ]
   */
  Prediction generate_prediction(int start_year,
                                 int start_month,
                                 int end_year,
                                 int end_month);

  /**
   * Format the prediction result into a format python callers favor.
   *
   * @param pred_timestes: Number of timesteps in the predicted sequence.
   *
   * @return Re-formatted prediction result.
   *         Access it as:
   *         [ sample number ][ time point ][ vertex name ][ indicator name ]
   */

  FormattedPredictionResult format_prediction_result();

  /**
   * this->generate_prediction() must be called before callign this method.
   * Outputs raw predictions for a given indicator that were generated by
   * generate_prediction(). Each column is a time step and the rows are the
   * samples for that time step.
   *
   * @param indicator: A std::string representing the indicator variable for
   which we
   *                   want predictions for.

   * @return A this->res x this->pred_timesteps dimension 2D array
   *         (std::vector of std::vectors)
   *
  */
  std::vector<std::vector<double>> prediction_to_array(std::string indicator);

  std::vector<Eigen::VectorXd> synthetic_latent_state_sequence;
  // ObservedStateSequence synthetic_observed_state_sequence;
  bool synthetic_data_experiment = false;

  void generate_synthetic_latent_state_sequence();

  void
  generate_synthetic_observed_state_sequence_from_synthetic_latent_state_sequence();

  std::pair<PredictedObservedStateSequence, Prediction>
  test_inference_with_synthetic_data(
      int start_year = 2015,
      int start_month = 1,
      int end_year = 2015,
      int end_month = 12,
      int res = 100,
      int burn = 900,
      std::string country = "South Sudan",
      std::string state = "",
      std::string county = "",
      std::map<std::string, std::string> units = {},
      InitialBeta initial_beta = InitialBeta::HALF);

  // TODO: Need testing
  /**
   * Sample observed state std::vector.
   * This is the implementation of the emission function.
   *
   * @param latent_state: Latent state std::vector.
   *                      This has 2 * number of vertices in the CAG.
   *                      Even indices track the state of each vertex.
   *                      Odd indices track the state of the derivative.
   *
   * @return Observed state std::vector. Observed state for each indicator for
   * each vertex. Indexed by: [ vertex id ][ indicator id ]
   */
  std::vector<std::vector<double>>
  sample_observed_state(Eigen::VectorXd latent_state);

  /**
   * Find all the transition matrix (A) cells that are dependent on the β
   * attached to the provided edge and update them.
   * Acts upon this->A_original
   *
   * @param e: The directed edge ≡ β that has been perturbed
   */
  void update_transition_matrix_cells(
      boost::graph_traits<DiGraph>::edge_descriptor e);

  /**
   * Sample a new transition matrix from the proposal distribution,
   * given a current candidate transition matrix.
   * In practice, this amounts to:
   *    Selecting a random β.
   *    Perturbing it a bit.
   *    Updating all the transition matrix cells that are dependent on it.
   */
  // TODO: Need testng
  // TODO: Before calling sample_from_proposal() we must call
  // AnalysisGraph::find_all_paths()
  // TODO: Before calling sample_from_proposal(), we mush assign initial βs and
  // run Tran_Mat_Cell::compute_cell() to initialize the first transistion
  // matrix.
  // TODO: Update Tran_Mat_Cell::compute_cell() to calculate the proper value.
  // At the moment it just computes sum of length of all the paths realted to
  // this cell
  void sample_from_proposal();

  void set_current_latent_state(int ts);

  double log_normpdf(double x, double mean, double sd);

  void set_log_likelihood();

  double calculate_delta_log_prior();

  void revert_back_to_previous_state();

  /**
   * Run Bayesian inference - sample from the posterior distribution.
   */
  void sample_from_posterior();

  // ==========================================================================
  // Manipulation
  // ==========================================================================

  void
  set_indicator(std::string concept, std::string indicator, std::string source);

  void delete_indicator(std::string concept, std::string indicator);

  void delete_all_indicators(std::string concept);

  /*
  // TODO: Demosntrate how to use the Node::get_indicator() method
  // with the custom exception.
  // Not sure whether we need this method in AnalaysisGraph
  // so that python side can directly access the Indicator class
  // objects and maipulate them (we need to fiture out how to map a
  // custom class from C++ into python for this) - harder
  // or
  // mirror getter and setter methods of the Indicator class
  // in AnalysisGraph and make the python side call them - easier.
  Indicator get_indicator(std::string concept, std::string indicator) {
    try {
      return graph[name_to_vertex.at(concept)].get_indicator(indicator);
    } catch (const std::out_of_range &oor) {
      fmt::print("Error: AnalysisGraph::get_indicator()\n");
      fmt::print("\tConcept: {} is not in the CAG\n", concept);
    } catch (IndicatorNotFoundException &infe) {
      std::cerr << "Error: AnalysisGraph::get_indicator()\n"
                << "\tindicator: " << infe.what()
                << " is not attached to CAG node " << concept << std::endl;
    }
  }
  */

  void replace_indicator(std::string concept,
                         std::string indicator_old,
                         std::string indicator_new,
                         std::string source);

  /*
    ==========================================================================
    Model parameterization
    *Loren: I am going to try to port this, I'll try not to touch anything up
    top
    and only push changes that compile. If I do push a change that breaks things
    you could probably just comment out this section.*
    ==========================================================================
  */

  /**
   * Map each concept node in the AnalysisGraph instance to one or more
   * tangible quantities, known as 'indicators'.
   *
   * @param n: Int representing number of indicators to attach per node.
   * Default is 1 since our model so far is configured for only 1 indicator per
   * node.
   */
  void map_concepts_to_indicators(int n = 1);

  /**
   * Parameterize the indicators of the AnalysisGraph..
   *
   */
  void parameterize(std::string country = "South Sudan",
                    std::string state = "",
                    std::string county = "",
                    int year = 2012,
                    int month = 1,
                    std::map<std::string, std::string> units = {});

  void print_nodes();

  void print_edges();

  void print_name_to_vertex();

  std::pair<Agraph_t*, GVC_t*>
  to_agraph(bool simplified_labels =
                false, /** Whether to create simplified labels or not. */
            int label_depth =
                1 /** Depth in the ontology to which simplified labels extend */
  );

  std::string to_dot();

  void
  to_png(std::string filename = "CAG.png",
         bool simplified_labels =
             false, /** Whether to create simplified labels or not. */
         int label_depth =
             1 /** Depth in the ontology to which simplified labels extend */
  );

  void print_indicators();
};


/*
* Copyright (c) 2017, Chennakesava Kadapa (c.kadapa@swansea.ac.uk).
* All rights reserved.
* Date: 17-July-2017
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* The author is not responsible for any damage.
*
*
* =========================================================================

* Create triangulation for the base grid and perform
* subtriangulation for the cut-cells

* distVTK
*       type    - vtkFloatArray
*       details - stores distance function value at a node of the background mesh

* nodeInOutVTK
*       type    - vtkIntArray
*       details - stores true/false depending upon whether a node is inside/outside
                these values are computed based on 'distVTK' data
* cutCellTypeVTK
*       type    - vtkIntArray
*       details - stores 0/1/2 depending upon whether an element is cut or not.
                If it is not then which domain (#1 or #2) the element belongs to.

* =========================================================================
*/

#include "headersVTK.h"
#include "headersBasic.h"
#include "utilfuns.h"

#include <algorithm>
#include <iostream>
#include <list>
#include <vector>
#include <fstream>
#include <limits>
#include <boost/foreach.hpp>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/AABB_tree.h>
#include <CGAL/AABB_traits.h>
#include <CGAL/AABB_triangle_primitive.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/AABB_face_graph_triangle_primitive.h>
#include <CGAL/Side_of_triangle_mesh.h>

typedef CGAL::Simple_cartesian<double> K;
//typedef CGAL::Exact_predicates_exact_constructions_kernel K;

typedef K::FT FT;
typedef K::Ray_3 Ray;
typedef K::Line_3 Line;
typedef K::Point_3 Point;
typedef K::Segment_3 Segment;
typedef K::Triangle_3 Triangle;
//typedef std::list<Triangle>::iterator Iterator;
//typedef CGAL::AABB_triangle_primitive<K, Iterator> Primitive;
//typedef CGAL::AABB_traits<K, Primitive> AABB_triangle_traits;
//typedef CGAL::AABB_tree<AABB_triangle_traits> Tree;

typedef CGAL::Polyhedron_3<K> Polyhedron;
typedef CGAL::AABB_face_graph_triangle_primitive<Polyhedron> Primitive;
typedef CGAL::AABB_traits<K, Primitive> Traits;
typedef CGAL::AABB_tree<Traits> Tree;

typedef CGAL::Side_of_triangle_mesh<Polyhedron, K> Point_inside;

typedef Polyhedron::HalfedgeDS  HalfedgeDS;

// A modifier creating a triangle with the incremental builder.
template<class HDS>
class polyhedron_builder : public CGAL::Modifier_base<HDS>
{
  public:
    std::vector<Point> &vertices;
    std::vector<int>    &faces;
    int  elType;

    polyhedron_builder( std::vector<Point> &_coords, std::vector<int> &_tris, int eT=3 ) : vertices(_coords), faces(_tris), elType(eT) {}

    void operator()( HDS& hds)
    {
        typedef typename HDS::Vertex   Vertex;
        typedef typename Vertex::Point Point;

        // create a cgal incremental builder
        CGAL::Polyhedron_incremental_builder_3<HDS> B( hds, true);

        if(elType == 3)
          B.begin_surface( vertices.size(), faces.size()/3 );
        else
          B.begin_surface( vertices.size(), faces.size()/4 );

       // add the polyhedron vertices
       for( int i=0; i<(int)vertices.size(); i++ )
       {
         B.add_vertex(vertices[i]);
         //B.add_vertex( Point( coords[i+0], coords[i+1], coords[i+2] ) );
       }

       // add the polyhedron triangles

       if(elType == 3)
       {
         for( int i=0; i<(int)faces.size(); i+=3 )
         {
           B.begin_facet();
           B.add_vertex_to_facet( faces[i+0] );
           B.add_vertex_to_facet( faces[i+1] );
           B.add_vertex_to_facet( faces[i+2] );
           B.end_facet();
         }
       }
       else
       {
         for( int i=0; i<(int)faces.size(); i+=4 )
         {
           B.begin_facet();
           B.add_vertex_to_facet( faces[i+0] );
           B.add_vertex_to_facet( faces[i+1] );
           B.add_vertex_to_facet( faces[i+3] );
           B.end_facet();

           B.begin_facet();
           B.add_vertex_to_facet( faces[i+0] );
           B.add_vertex_to_facet( faces[i+3] );
           B.add_vertex_to_facet( faces[i+2] );
           B.end_facet();
         }
       }

       // finish up the surface
       B.end_surface();
    }
};

Point_inside*  createPointer1(std::vector<Point> &vertices, std::vector<int> &faces, int eT=3)
{
    // build a polyhedron from the loaded arrays
    Polyhedron  polyhedronTemp;
    polyhedron_builder<HalfedgeDS>  poly_builder( vertices, faces, eT );
    polyhedronTemp.delegate( poly_builder );

    Tree tree;
    tree.insert(polyhedronTemp.facets_begin(), polyhedronTemp.facets_end(), polyhedronTemp);

    Point_inside*  inside_tester = new Point_inside(tree);

    return inside_tester;
}

Point_inside*  createPointer2(Polyhedron& polyhedronTemp)
{
    Tree tree;
    tree.insert(polyhedronTemp.facets_begin(), polyhedronTemp.facets_end(), polyhedronTemp);

    Point_inside*  inside_tester = new Point_inside(tree);

    return inside_tester;
}

inline Point_inside*  createPointer3(Tree& tree)
{
    Point_inside*  inside_tester = new Point_inside(tree);

    return inside_tester;
}

using namespace std;

int main(int argc, char* argv[])
{
    //////////////////////////////////////////////
    //
    // declare vtk variables
    //
    //////////////////////////////////////////////

    vtkSmartPointer<vtkDataSetMapper>        mapperVTK    =  vtkSmartPointer<vtkDataSetMapper>::New();
    vtkSmartPointer<vtkUnstructuredGrid>     uGridVTK     =  vtkSmartPointer<vtkUnstructuredGrid>::New();
    vtkSmartPointer<vtkPoints>               pointsGrid   =  vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkPoints>               pointsVTK2   =  vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkVertex>               vertexVTK    =  vtkSmartPointer<vtkVertex>::New();
    vtkSmartPointer<vtkLine>                 lineVTK      =  vtkSmartPointer<vtkLine>::New();
    vtkSmartPointer<vtkQuad>                 quadVTK      =  vtkSmartPointer<vtkQuad>::New();
    vtkSmartPointer<vtkHexahedron>           hexVTK       =  vtkSmartPointer<vtkHexahedron>::New();
    vtkSmartPointer<vtkTriangle>             triaVTK      =  vtkSmartPointer<vtkTriangle>::New();
    vtkSmartPointer<vtkPolygon>              polygonVTK   =  vtkSmartPointer<vtkPolygon>::New();
    vtkSmartPointer<vtkTetra>                tetraVTK     =  vtkSmartPointer<vtkTetra>::New();
    vtkSmartPointer<vtkPyramid>              pyramidVTK   =  vtkSmartPointer<vtkPyramid>::New();
    vtkSmartPointer<vtkWedge>                wedgeVTK     =  vtkSmartPointer<vtkWedge>::New();

    vtkSmartPointer<vtkIntArray>          nodeInOutVTK       =  vtkSmartPointer<vtkIntArray>::New();
    vtkSmartPointer<vtkIntArray>          cutCellTypeVTK       =  vtkSmartPointer<vtkIntArray>::New();
    vtkSmartPointer<vtkIntArray>          cellOrientationVTK    =  vtkSmartPointer<vtkIntArray>::New();
    vtkSmartPointer<vtkIntArray>          cellOrientationVTK2    =  vtkSmartPointer<vtkIntArray>::New();
    vtkSmartPointer<vtkFloatArray>          vecVTK       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkFloatArray>          vecVTK2       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkFloatArray>          distVTK       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkFloatArray>          scaVTK2       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkFloatArray>          cellDataVTK       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkFloatArray>          cellDataVTK2       =  vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkExtractEdges>     extractEdgesVTK  =    vtkSmartPointer<vtkExtractEdges>::New();

    vtkSmartPointer<vtkUnstructuredGrid> uGrid   =  vtkSmartPointer<vtkUnstructuredGrid>::New();
    vtkSmartPointer<vtkUnstructuredGrid> uGrid2  =  vtkSmartPointer<vtkUnstructuredGrid>::New();

    vtkSmartPointer<vtkPoints>              pointsPoly  =  vtkSmartPointer<vtkPoints>::New();

    // Add the polygon to a list of polygons
    vtkSmartPointer<vtkCellArray> polyList  =   vtkSmartPointer<vtkCellArray>::New();

    // Create a PolyData
    vtkSmartPointer<vtkPolyData> polyData =   vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPolyData> polyData2 =   vtkSmartPointer<vtkPolyData>::New();

    vtkSmartPointer<vtkXMLUnstructuredGridWriter>  writerUGridVTK =  vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
    vtkSmartPointer<vtkXMLPolyDataWriter>  writerPolyData =     vtkSmartPointer<vtkXMLPolyDataWriter>::New();
    vtkSmartPointer<vtkXMLUnstructuredGridWriter>  writeruGrid   =  vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
    vtkSmartPointer<vtkXMLUnstructuredGridWriter>  writeruGrid2  =  vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();

    vtkSmartPointer<vtkMergePoints> mergePoints =     vtkSmartPointer<vtkMergePoints>::New();

    //////////////////////////////////////////////
    //
    // generate base triangulation
    //
    //////////////////////////////////////////////

    double  x0, x1, y0, y1, z0, z1;

    int nEx, nEy, nEz, nNx, nNy, nNz, nNode, nElem;

    x0 = -1.6;    x1 =  1.6;    nEx = 50;
    y0 = -1.6;    y1 =  1.6;    nEy = 50;
    z0 = -1.6;    z1 =  1.6;    nEz = 50;

/*
    if(argc == 0)
      cerr << " Input data " << endl;
    else
    {
       x0  = atof(argv[1]);
       x1  = atof(argv[2]);
       nEx = atoi(argv[3]);

       y0  = atof(argv[4]);
       y1  = atof(argv[5]);
       nEy = atoi(argv[6]);

       z0  = atof(argv[7]);
       z1  = atof(argv[8]);
       nEz = atoi(argv[9]);
    }
*/
    nElem = nEx*nEy*nEz;

    nNx = nEx+1;
    nNy = nEy+1;
    nNz = nEz+1;

    nNode = nNx*nNy*nNz;

    uGridVTK->Reset();
    pointsGrid->Reset();
    cellDataVTK->Reset();
    cellDataVTK->Reset();
    cutCellTypeVTK->Reset();
    nodeInOutVTK->Reset();

    int  ii, jj, kk, ll, n1, n2, n3, n4, nn;
    int  ind, ind1, ind2, ind3, ind4, ind5, ind6;

    double dx = (x1-x0)/nEx;
    double dy = (y1-y0)/nEy;
    double dz = (z1-z0)/nEz;
    double xx, yy, zz, fact;

    cout << x0 << '\t' << x1 << '\t' << dx << endl;
    cout << y0 << '\t' << y1 << '\t' << dy << endl;
    cout << z0 << '\t' << z1 << '\t' << dz << endl;

    vtkIdType pts[10];

    distVTK->SetName("dist");
    distVTK->SetNumberOfTuples(nNode);
    nodeInOutVTK->SetName("InOut");
    nodeInOutVTK->SetNumberOfTuples(nNode);
    cutCellTypeVTK->SetName("cellType");
    cutCellTypeVTK->SetNumberOfTuples(nElem);
    cellOrientationVTK->SetName("elType");
    cellOrientationVTK->SetNumberOfTuples(nElem);
    cellOrientationVTK2->SetName("elType");
    cellOrientationVTK2->SetNumberOfTuples(nElem);

    //////////////////////////////////////////////
    //
    // create nodes/points
    //
    //////////////////////////////////////////////

    ind = 0;
    zz = z0;
    for(kk=0; kk<nNz; kk++)
    {
      yy = y0;
      for(jj=0;jj<nNy;jj++)
      {
        xx = x0;
        for(ii=0;ii<nNx;ii++)
        {
          pts[0] = pointsGrid->InsertNextPoint(xx, yy, zz);

          //distVTK->SetTuple1(ind, fact);
          //nodeInOutVTK->SetTuple1(ind, (fact >= 0.0));

          xx += dx;
          ind++;
        }
        yy += dy;
      }
      zz += dz;
    }

    ////////////////////////////////////////////////
    ////////////////////////////////////////////////
    //
    // read the surface and generate polydata
    //
    ////////////////////////////////////////////////
    ////////////////////////////////////////////////

    //char infile_nodes[]="sphere-nodes.dat";
    //char infile_trias[]="sphere-trias.dat";

    //char infile_nodes[]="reliefvalve3Dblock-nodes.dat";
    //char infile_trias[]="reliefvalve3Dblock-trias.dat";

    //char infile_nodes[]="turekbeam3d-nodes.dat";
    //char infile_trias[]="turekbeam3d-quads.dat";

    char infile_nodes[]="thickplate-nodes.dat";
    char infile_trias[]="thickplate-elems.dat";

    ifstream  infile(infile_nodes);

    if(infile.fail())
    {
      cout << " Could not open the input file" << endl;
      exit(1);
    }

    double  val[10];

    vtkIdType pt[12];

    while(infile >> val[0] >> val[1] >> val[2] >> val[3])
    {
       //printf("%12.6f \t %12.6f \t %12.6f \n", val[0], val[1], val[2]);

       pt[0] = pointsPoly->InsertNextPoint(val[1], val[2], val[3]);
    }

    ifstream  infile2(infile_trias);

    if(infile2.fail())
    {
       cout << " Could not open the input file" << endl;
      exit(1);
    }

    int  valInt[10], ETYPE=4;

    if(ETYPE==3)
    {
      while(infile2 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4])
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", valInt[0], valInt[1], valInt[2]);

        polygonVTK->GetPointIds()->SetNumberOfIds(3); //make a triangle

        polygonVTK->GetPointIds()->SetId(0, valInt[2]-1);
        polygonVTK->GetPointIds()->SetId(1, valInt[3]-1);
        polygonVTK->GetPointIds()->SetId(2, valInt[4]-1);

        polyList->InsertNextCell(polygonVTK);
      }
    }
    else
    {
      while(infile2 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4] >> valInt[5])
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", valInt[0], valInt[1], valInt[2]);

        //polygonVTK->GetPointIds()->SetNumberOfIds(4); //make a quad

        //polygonVTK->GetPointIds()->SetId(0, valInt[2]-1);
        //polygonVTK->GetPointIds()->SetId(1, valInt[3]-1);
        //polygonVTK->GetPointIds()->SetId(2, valInt[5]-1);
        //polygonVTK->GetPointIds()->SetId(3, valInt[4]-1);

        polygonVTK->GetPointIds()->SetNumberOfIds(3); //make a triangle

        polygonVTK->GetPointIds()->SetId(0, valInt[2]-1);
        polygonVTK->GetPointIds()->SetId(1, valInt[3]-1);
        polygonVTK->GetPointIds()->SetId(2, valInt[5]-1);

        polyList->InsertNextCell(polygonVTK);

        polygonVTK->GetPointIds()->SetNumberOfIds(3); //make a triangle

        polygonVTK->GetPointIds()->SetId(0, valInt[2]-1);
        polygonVTK->GetPointIds()->SetId(1, valInt[5]-1);
        polygonVTK->GetPointIds()->SetId(2, valInt[4]-1);

        polyList->InsertNextCell(polygonVTK);
      }
    }

    polyData->SetPoints(pointsPoly);
    polyData->SetPolys(polyList);

    writerPolyData->SetFileName("immersedpoly.vtp");
    writerPolyData->SetInputData(polyData);
    writerPolyData->Write();

    /////////////////////////////////////////////////////
    /////////////////////////////////////////////////////
    //
    // compute using VTK
    //
    /////////////////////////////////////////////////////
    /////////////////////////////////////////////////////

    time_t tstart, tend;

    tstart = time(0);

    vtkSmartPointer<vtkPolyData> pointsPolydataVTK   =   vtkSmartPointer<vtkPolyData>::New();
    pointsPolydataVTK->SetPoints(pointsGrid);

    vtkSmartPointer<vtkSelectEnclosedPoints> selectEnclosedPoints = vtkSmartPointer<vtkSelectEnclosedPoints>::New();

    selectEnclosedPoints->SetInputData(pointsPolydataVTK);
    selectEnclosedPoints->SetTolerance(0.00001);
    selectEnclosedPoints->Initialize(polyData);

    //////////////////////////////////////////////
    //
    // create cells/elements
    //
    //////////////////////////////////////////////

    double  bnds[6];
    bool flag;
    double ptTemp[3];

    vector<int>  ff(8);

    vtkIdType npts=3, cellId, cellId2, type, id;
    vtkCell  *cellVTK, *cellVTK2;

    cout << " AAAAAAAAAAAAA " << endl;

    nn = nNx*nNy;

    cellId = 0;
    ind=0;
    for(kk=0; kk<nEz; kk++)
    {
      ind5 = nn*kk;
      ind6 = nn*(kk+1);

      for(jj=0; jj<nEy; jj++)
      {
        ind1 = ind5 + nNx*jj;
        ind2 = ind5 + nNx*(jj+1);

        ind3 = ind6 + nNx*jj;
        ind4 = ind6 + nNx*(jj+1);

        for(ii=0;ii<nEx;ii++)
        {
          pts[0] = ind1+ii;          pts[4] = ind3+ii;
          pts[1] = pts[0]+1;         pts[5] = pts[4]+1;
          pts[3] = ind2+ii;          pts[7] = ind4+ii;
          pts[2] = pts[3]+1;         pts[6] = pts[7]+1;

          ff.assign(8,0);

          for(ll=0;ll<8;ll++)
          {
            hexVTK->GetPointIds()->SetId(ll, pts[ll]);
            //cout << ll << '\t' << pts[ll] << endl;
            pointsGrid->GetPoint(pts[ll], ptTemp);
            //ff[ll] = selectEnclosedPoints->IsInside(pts[ll]);
            ff[ll] = selectEnclosedPoints->IsInsideSurface(ptTemp[0], ptTemp[1], ptTemp[2]);
            //cout << ptTemp[0] << '\t' << ptTemp[1] << '\t' << ptTemp[2] << '\t' << ff[ll] << endl;
          }

          uGrid->InsertNextCell(hexVTK->GetCellType(), hexVTK->GetPointIds());

          flag = std::equal( ff.begin()+1, ff.end(), ff.begin() );

          if( flag )
          {
            cellOrientationVTK->SetTuple1(cellId, ff[0]);
          }
          else
          {
            cellOrientationVTK->SetTuple1(cellId, -1);
          }

          cellId++;
        }
      }
    }

    //////////////////////////////////////////////
    //
    // setup and write grid data
    //
    //////////////////////////////////////////////

    char fname1[50];
    sprintf(fname1,"%s%d%s", "cutFEMhex-vtk-",nEx,".vtu");

    uGrid->SetPoints(pointsGrid);
//    uGrid->GetPointData()->SetScalars(distVTK);
//    uGrid->GetPointData()->AddArray(nodeInOutVTK);

    cout << " uGrid->GetNumberOfPoints() = " <<  uGrid->GetNumberOfPoints() << endl;
    cout << " uGrid->GetNumberOfCells()  = " <<  uGrid->GetNumberOfCells() << endl;

    uGrid->GetCellData()->SetScalars(cellOrientationVTK);

    writeruGrid->SetFileName(fname1);
    writeruGrid->SetInputData(uGrid);
    writeruGrid->Write();

    tend = time(0);
    cout << "Time taken = "<< difftime(tend, tstart) <<" second(s)."<< endl;

    //////////////////////////////////////////////////////
    //////////////////////////////////////////////////////
    //
    // compute using CGAL
    //
    //////////////////////////////////////////////////////
    //////////////////////////////////////////////////////

    tstart = time(0);

    /*
    ifstream  infile3(infile_nodes);

    if(infile3.fail())
    {
      cout << " Could not open the input file" << endl;
      exit(1);
    }

    std::vector<Point> pointsCGAL;

    while(infile3 >> val[0] >> val[1] >> val[2] >> val[3])
    {
      Point a(val[1], val[2], val[3]);
      pointsCGAL.push_back(a);
    }

    ifstream  infile4(infile_trias);

    if(infile4.fail())
    {
      cout << " Could not open the input file" << endl;
      exit(1);
    }

    std::list<Triangle> triangles;

    if(ETYPE==3)
    {
      while(infile4 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4])
      {
        triangles.push_back(Triangle(pointsCGAL[valInt[2]-1], pointsCGAL[valInt[3]-1], pointsCGAL[valInt[4]-1]));
      }
    }
    else
    {
      while(infile4 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4] >> valInt[5])
      {
        triangles.push_back(Triangle(pointsCGAL[valInt[2]-1], pointsCGAL[valInt[3]-1], pointsCGAL[valInt[5]-1]));
        triangles.push_back(Triangle(pointsCGAL[valInt[2]-1], pointsCGAL[valInt[5]-1], pointsCGAL[valInt[4]-1]));
      }
    }
    */

    ifstream  infile3(infile_nodes);

    if(infile3.fail())
    {
      cout << " Could not open the input file" << endl;
      exit(1);
    }

    // create a cgal incremental builder

    //vector<double>  vertices;
    vector<Point> vertices;

    // add the polyhedron vertices
    while(infile3 >> val[0] >> val[1] >> val[2] >> val[3])
    {
      //vertices.push_back(val[1]);
      //vertices.push_back(val[2]);
      //vertices.push_back(val[3]);

      vertices.push_back(Point(val[1], val[2], val[3]));
    }

    ifstream  infile4(infile_trias);

    if(infile4.fail())
    {
      cout << " Could not open the input file" << endl;
      exit(1);
    }

    vector<int>  faces;

    // add the polyhedron triangles
    if(ETYPE==3)
    {
      while(infile4 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4])
      {
        faces.push_back(valInt[2]-1);
        faces.push_back(valInt[3]-1);
        faces.push_back(valInt[4]-1);
      }
    }
    else
    {
      while(infile4 >> valInt[0] >> valInt[1] >> valInt[2] >> valInt[3] >> valInt[4] >> valInt[5])
      {
        faces.push_back(valInt[2]-1);
        faces.push_back(valInt[3]-1);
        faces.push_back(valInt[4]-1);
        faces.push_back(valInt[5]-1);
      }
    }

    /*
    // build a polyhedron from the loaded arrays
    Polyhedron  polyhedronTemp;
    polyhedron_builder<HalfedgeDS>  poly_builder( vertices, faces, ETYPE );
    polyhedronTemp.delegate( poly_builder );

    // constructs AABB tree
    //Tree tree(polyhedronTemp.facets_begin(), polyhedronTemp.facets_end(), polyhedronTemp);

    Tree tree;
    tree.insert(polyhedronTemp.facets_begin(), polyhedronTemp.facets_end(), polyhedronTemp);

    //Point_inside inside_tester(tree);

    Point_inside  *inside_tester;
    inside_tester = new Point_inside(tree);
    */

    Point_inside  *inside_tester;

    // function #1 ... does not work
    //inside_tester = createPointer1(vertices, faces, ETYPE);

    // function #2 ... does not work
    Polyhedron  polyhedronTemp;
    polyhedron_builder<HalfedgeDS>  poly_builder( vertices, faces, ETYPE );
    polyhedronTemp.delegate( poly_builder );

    //inside_tester = createPointer2(polyhedronTemp);

    // function #3

    Tree tree;
    tree.insert(polyhedronTemp.facets_begin(), polyhedronTemp.facets_end(), polyhedronTemp);

    inside_tester = createPointer3(tree);

    int nints;
    nn = nNx*nNy;

    cellId = 0;
    ind=0;
    for(kk=0; kk<nEz; kk++)
    {
      ind5 = nn*kk;
      ind6 = nn*(kk+1);

      for(jj=0; jj<nEy; jj++)
      {
        ind1 = ind5 + nNx*jj;
        ind2 = ind5 + nNx*(jj+1);

        ind3 = ind6 + nNx*jj;
        ind4 = ind6 + nNx*(jj+1);

        for(ii=0;ii<nEx;ii++)
        {
          pts[0] = ind1+ii;          pts[4] = ind3+ii;
          pts[1] = pts[0]+1;         pts[5] = pts[4]+1;
          pts[3] = ind2+ii;          pts[7] = ind4+ii;
          pts[2] = pts[3]+1;         pts[6] = pts[7]+1;

          ff.assign(8,0);

          for(ll=0;ll<8;ll++)
          {
            hexVTK->GetPointIds()->SetId(ll, pts[ll]);

            //cout << ll << '\t' << pts[ll] << endl;

            pointsGrid->GetPoint(pts[ll], ptTemp);

            //Point  ray_begin(ptTemp[0], ptTemp[1], ptTemp[2]);
            //Point  ray_end(10.0, ptTemp[1], ptTemp[2]);

            //FT sqd = tree.squared_distance(ray_begin);
            //cout << " sqd = " << sqd << endl;
            //ff[ll] = (sqd >= 0.0);

            //count number of intersections
            //Ray ray_query(ray_begin, ray_end);
            //Segment segment_query(ray_begin, ray_end);

            //ff[ll] = tree.number_of_intersected_primitives(Ray(ray_begin, ray_end)) % 2;
            //ff[ll] = tree.number_of_intersected_primitives(segment_query) % 2;

            //ff[ll] = tree.number_of_intersected_primitives(Ray(
            //              Point(ptTemp[0], ptTemp[1], ptTemp[2]),
            //              Point(100.0, ptTemp[1], ptTemp[2]))) % 2;

            CGAL::Bounded_side res = (*inside_tester)(Point(ptTemp[0], ptTemp[1], ptTemp[2]));

            if( (res == CGAL::ON_BOUNDED_SIDE) || (res == CGAL::ON_BOUNDARY) )
            {
              ff[ll] = 1;
            }
            else
            {
              ff[ll] = 0;
            }

          }

          flag = std::equal( ff.begin()+1, ff.end(), ff.begin() );

          if( flag )
          {
            if(ff[0] == 0)
            {
              uGrid2->InsertNextCell(hexVTK->GetCellType(), hexVTK->GetPointIds());
              cellOrientationVTK2->SetTuple1(cellId, ff[0]);
              cellId++;
            }
          }
          else
          {
            uGrid2->InsertNextCell(hexVTK->GetCellType(), hexVTK->GetPointIds());
            cellOrientationVTK2->SetTuple1(cellId, -1);
            cellId++;
          }
        }
      }
    }

    //////////////////////////////////////////////
    //
    // setup and write polyData
    //
    //////////////////////////////////////////////

    char fname2[50];
    sprintf(fname2,"%s%d%s", "cutFEMhex-cgal-",nEx,".vtu");

    uGrid2->SetPoints(pointsGrid);
//    uGrid->GetPointData()->SetScalars(distVTK);
//    uGrid->GetPointData()->AddArray(nodeInOutVTK);

    cout << " uGrid2->GetNumberOfPoints() = " <<  uGrid2->GetNumberOfPoints() << endl;
    cout << " uGrid2->GetNumberOfCells()  = " <<  uGrid2->GetNumberOfCells() << endl;

    uGrid2->GetCellData()->SetScalars(cellOrientationVTK2);

    writeruGrid->SetFileName(fname2);
    writeruGrid->SetInputData(uGrid2);
    writeruGrid->Write();

    tend = time(0);
    cout << "Time taken = "<< difftime(tend, tstart) <<" second(s)."<< endl;

  return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2006, 2008 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file vanillaswap.hpp
    \brief Simple fixed-rate vs Libor swap
*/

#ifndef quantlib_vanilla_swap_hpp
#define quantlib_vanilla_swap_hpp

#include <ql/instruments/swap.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

namespace QuantLib {

    class IborIndex;

    //! Plain-vanilla swap: fix vs floating leg
    /*! \ingroup instruments

        If no payment convention is passed, the convention of the
        floating-rate schedule is used.

        \warning if <tt>Settings::includeReferenceDateCashFlows()</tt>
                 is set to <tt>true</tt>, payments occurring at the
                 settlement date of the swap might be included in the
                 NPV and therefore affect the fair-rate and
                 fair-spread calculation. This might not be what you
                 want.

        \test
        - the correctness of the returned value is tested by checking
          that the price of a swap paying the fair fixed rate is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap receiving the fair floating-rate
          spread is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap decreases with the paid fixed rate.
        - the correctness of the returned value is tested by checking
          that the price of a swap increases with the received
          floating-rate spread.
        - the correctness of the returned value is tested by checking
          it against a known good value.
    */
    class VanillaSwap : public Swap {
      public:
        class arguments;
        class results;
        class engine;
        VanillaSwap(Type type,
                    Real nominal,
                    Schedule fixedSchedule,
                    Rate fixedRate,
                    DayCounter fixedDayCount,
                    Schedule floatSchedule,
                    ext::shared_ptr<IborIndex> iborIndex,
                    Spread spread,
                    DayCounter floatingDayCount,
                    boost::optional<BusinessDayConvention> paymentConvention = boost::none,
                    boost::optional<bool> useIndexedCoupons = boost::none);
        //! \name Inspectors
        //@{
        Type type() const;
        Real nominal() const;

        const Schedule& fixedSchedule() const;
        Rate fixedRate() const;
        const DayCounter& fixedDayCount() const;

        const Schedule& floatingSchedule() const;
        const ext::shared_ptr<IborIndex>& iborIndex() const;
        Spread spread() const;
        const DayCounter& floatingDayCount() const;

        BusinessDayConvention paymentConvention() const;

        const Leg& fixedLeg() const;
        const Leg& floatingLeg() const;
        //@}

        //! \name Results
        //@{
        Real fixedLegBPS() const;
        Real fixedLegNPV() const;
        Rate fairRate() const;

        Real floatingLegBPS() const;
        Real floatingLegNPV() const;
        Spread fairSpread() const;
        //@}
        // other
        void setupArguments(PricingEngine::arguments* args) const override;
        void fetchResults(const PricingEngine::results*) const override;

      private:
        void setupExpired() const override;
        Type type_;
        Real nominal_;
        Schedule fixedSchedule_;
        Rate fixedRate_;
        DayCounter fixedDayCount_;
        Schedule floatingSchedule_;
        ext::shared_ptr<IborIndex> iborIndex_;
        Spread spread_;
        DayCounter floatingDayCount_;
        BusinessDayConvention paymentConvention_;
        // results
        mutable Rate fairRate_;
        mutable Spread fairSpread_;
    };

    //! %Arguments for simple swap calculation
    class VanillaSwap::arguments : public Swap::arguments {
      public:
        arguments() : nominal(Null<Real>()) {}
        Type type = Receiver;
        Real nominal;

        std::vector<Date> fixedResetDates;
        std::vector<Date> fixedPayDates;
        std::vector<Time> floatingAccrualTimes;
        std::vector<Date> floatingResetDates;
        std::vector<Date> floatingFixingDates;
        std::vector<Date> floatingPayDates;

        std::vector<Real> fixedCoupons;
        std::vector<Spread> floatingSpreads;
        std::vector<Real> floatingCoupons;
        void validate() const override;
    };

    //! %Results from simple swap calculation
    class VanillaSwap::results : public Swap::results {
      public:
        Rate fairRate;
        Spread fairSpread;
        void reset() override;
    };

    class VanillaSwap::engine : public GenericEngine<VanillaSwap::arguments,
                                                     VanillaSwap::results> {};

    // inline definitions

    inline Swap::Type VanillaSwap::type() const {
        return type_;
    }

    inline Real VanillaSwap::nominal() const {
        return nominal_;
    }

    inline const Schedule& VanillaSwap::fixedSchedule() const {
        return fixedSchedule_;
    }

    inline Rate VanillaSwap::fixedRate() const {
        return fixedRate_;
    }

    inline const DayCounter& VanillaSwap::fixedDayCount() const {
        return fixedDayCount_;
    }

    inline const Schedule& VanillaSwap::floatingSchedule() const {
        return floatingSchedule_;
    }

    inline const ext::shared_ptr<IborIndex>& VanillaSwap::iborIndex() const {
        return iborIndex_;
    }

    inline Spread VanillaSwap::spread() const {
        return spread_;
    }

    inline const DayCounter& VanillaSwap::floatingDayCount() const {
        return floatingDayCount_;
    }

    inline BusinessDayConvention VanillaSwap::paymentConvention() const {
        return paymentConvention_;
    }

    inline const Leg& VanillaSwap::fixedLeg() const {
        return legs_[0];
    }

    inline const Leg& VanillaSwap::floatingLeg() const {
        return legs_[1];
    }

}

#endif


#include "ViscositySolver.h"

#include <iostream>

#include <Eigen/Sparse>

#include "tbb/blocked_range.h"
#include "tbb/parallel_for.h"

#include "ComputeWeights.h"
#include "LevelSet.h"

namespace FluidSim3D
{

void ViscositySolver(double dt,
                    const LevelSet& surface,
                    VectorGrid<double>& velocity,
                    const LevelSet& solidSurface,
                    const VectorGrid<double>& solidVelocity,
                    const ScalarGrid<double>& viscosity)
{
    // For efficiency sake, this should only take in velocity on a staggered grid
    // that matches the center sampled surface and collision
    assert(surface.isGridMatched(solidSurface));
    assert(surface.isGridMatched(viscosity));
    assert(velocity.isGridMatched(solidVelocity));

    for (int axis : {0, 1, 2})
    {
        Vec3i faceSize = velocity.size(axis);
        --faceSize[axis];

        assert(faceSize == surface.size());
    }

    int volumeSamples = 3;

    ScalarGrid<double> centerVolumes(surface.xform(), surface.size(), 0, ScalarGridSettings::SampleType::CENTER);
    computeSupersampleVolumes(centerVolumes, surface, 3);

    VectorGrid<double> edgeVolumes(surface.xform(), surface.size(), Vec3d::Zero(), VectorGridSettings::SampleType::EDGE);
    for (int axis : {0, 1, 2}) computeSupersampleVolumes(edgeVolumes.grid(axis), surface, 3);

    VectorGrid<double> faceVolumes = computeSupersampledFaceVolumes(surface, 3);

    enum class MaterialLabels
    {
        SOLID_FACE,
        LIQUID_FACE,
        AIR_FACE
    };

    VectorGrid<MaterialLabels> materialFaceLabels(surface.xform(), surface.size(), Vec3t<MaterialLabels>::Constant(MaterialLabels::AIR_FACE),
                                                  VectorGridSettings::SampleType::STAGGERED);

    // Set material labels for each grid face. We assume faces along the simulation boundary
    // are solid.

    for (int faceAxis : {0, 1, 2})
    {
        tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
        {
            for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
            {
                Vec3i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

                if (face[faceAxis] == 0 || face[faceAxis] == materialFaceLabels.size(faceAxis)[faceAxis] - 1)
                    continue;

                bool isFaceInSolve = false;

                for (int direction : {0, 1})
                {
                    Vec3i cell = faceToCell(face, faceAxis, direction);
                    if (centerVolumes(cell) > 0) isFaceInSolve = true;
                }

                if (!isFaceInSolve)
                {
                    for (int edgeAxis : {0, 1, 2})
                    {
                        if (edgeAxis == faceAxis) continue;

                        for (int direction : {0, 1})
                        {
                            Vec3i edge = faceToEdge(face, faceAxis, edgeAxis, direction);

                            if (edgeVolumes(edge, edgeAxis) > 0) isFaceInSolve = true;
                        }
                    }
                }

                if (isFaceInSolve)
                {
                    if (solidSurface.triLerp(materialFaceLabels.indexToWorld(face.cast<double>(), faceAxis)) <= 0.)
                        materialFaceLabels(face, faceAxis) = MaterialLabels::SOLID_FACE;
                    else
                        materialFaceLabels(face, faceAxis) = MaterialLabels::LIQUID_FACE;
                }
            }
        });
    }

    int liquidDOFCount = 0;

    constexpr int UNLABELLED_CELL = -1;

    VectorGrid<int> liquidFaceIndices(surface.xform(), surface.size(), Vec3t<int>::Constant(UNLABELLED_CELL),
                                      VectorGridSettings::SampleType::STAGGERED);

    for (int axis : {0, 1, 2})
    {
        forEachVoxelRange(Vec3i::Zero(), materialFaceLabels.size(axis), [&](const Vec3i& face)
        {
            if (materialFaceLabels(face, axis) == MaterialLabels::LIQUID_FACE)
                liquidFaceIndices(face, axis) = liquidDOFCount++;
        });
    }

    double discreteScalar = dt / std::pow(surface.dx(), 2);

    // Pre-scale all the control volumes with coefficients to reduce
    // redundant operations when building the linear system.

    tbb::parallel_for(tbb::blocked_range<int>(0, centerVolumes.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
    {
        for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
        {
            Vec3i cell = centerVolumes.unflatten(cellIndex);

            if (centerVolumes(cell) > 0) centerVolumes(cell) *= 2. * discreteScalar * viscosity(cell);
        }
    });

    for (int edgeAxis : {0, 1, 2})
    {
        tbb::parallel_for(tbb::blocked_range<int>(0, edgeVolumes.grid(edgeAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
        {
            for (int edgeIndex = range.begin(); edgeIndex != range.end(); ++edgeIndex)
            {
                Vec3i edge = edgeVolumes.grid(edgeAxis).unflatten(edgeIndex);

                if (edgeVolumes(edge, edgeAxis) > 0)
                    edgeVolumes(edge, edgeAxis) *= discreteScalar * viscosity.triLerp(edgeVolumes.indexToWorld(edge.cast<double>(), edgeAxis));
            }
        });
    }

    std::vector<Eigen::Triplet<double>> sparseElements;
    VectorXd initialGuessVector = VectorXd::Zero(liquidDOFCount);
    VectorXd rhsVector = VectorXd::Zero(liquidDOFCount);

    {
        tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<double>>> parallelSparseElements;

        for (int faceAxis : {0, 1, 2})
        {
            tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
            {
                auto& localSparseElements = parallelSparseElements.local();

                for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
                {
                    Vec3i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

                    int liquidFaceIndex = liquidFaceIndices(face, faceAxis);

                    if (liquidFaceIndex >= 0)
                    {
                        assert(materialFaceLabels(face, faceAxis) == MaterialLabels::LIQUID_FACE);

                        // Use old velocity as an initial guess since we're solving for a new
                        // velocity field with viscous forces applied to the old velocity field.
                        initialGuessVector(liquidFaceIndex) = velocity(face, faceAxis);

                        // Build RHS with volume weights
                        double localFaceVolume = faceVolumes(face, faceAxis);

                        rhsVector(liquidFaceIndex) = localFaceVolume * velocity(face, faceAxis);

                        // Add volume weight to diagonal
                        double diagonal = localFaceVolume;

                        // Build cell centered stress terms
                        for (int divergenceDirection : {0, 1})
                        {
                            Vec3i cell = faceToCell(face, faceAxis, divergenceDirection);

                            assert(cell[faceAxis] >= 0 && cell[faceAxis] < centerVolumes.size()[faceAxis]);

                            double divergenceSign = (divergenceDirection == 0) ? -1 : 1;

                            if (centerVolumes(cell) > 0)
                            {
                                for (int gradientDirection : {0, 1})
                                {
                                    Vec3i adjacentFace = cellToFace(cell, faceAxis, gradientDirection);

                                    double gradientSign = (gradientDirection == 0) ? -1. : 1.;

                                    double coefficient = divergenceSign * gradientSign * centerVolumes(cell);

                                    int adjacentFaceIndex = liquidFaceIndices(adjacentFace, faceAxis);
                                    if (adjacentFaceIndex >= 0)
                                    {
                                        if (adjacentFaceIndex == liquidFaceIndex)
                                            diagonal -= coefficient;
                                        else
                                            localSparseElements.emplace_back(liquidFaceIndex, adjacentFaceIndex, -coefficient);
                                    }
                                    else if (materialFaceLabels(adjacentFace, faceAxis) == MaterialLabels::SOLID_FACE)
                                        rhsVector(liquidFaceIndex) += coefficient * solidVelocity(adjacentFace, faceAxis);
                                    else
                                        assert(materialFaceLabels(adjacentFace, faceAxis) == MaterialLabels::AIR_FACE);
                                }
                            }
                        }

                        for (int edgeAxis : {0, 1, 2})
                        {
                            if (edgeAxis == faceAxis) continue;

                            for (int divergenceDirection : {0, 1})
                            {
                                Vec3i edge = faceToEdge(face, faceAxis, edgeAxis, divergenceDirection);

                                if (edgeVolumes(edge, edgeAxis) > 0)
                                {
                                    double divergenceSign = (divergenceDirection == 0) ? -1 : 1;

                                    for (int gradientAxis : {0, 1, 2})
                                    {
                                        if (gradientAxis == edgeAxis) continue;

                                        int gradientFaceAxis = 3 - gradientAxis - edgeAxis;

                                        for (int gradientDirection : {0, 1})
                                        {
                                            double gradientSign = (gradientDirection == 0) ? -1 : 1;

                                            Vec3i localGradientFace = edgeToFace(edge, edgeAxis, gradientFaceAxis, gradientDirection);

                                            int gradientFaceIndex = liquidFaceIndices(localGradientFace, gradientFaceAxis);

                                            double coefficient = divergenceSign * gradientSign * edgeVolumes(edge, edgeAxis);
                                            if (gradientFaceIndex >= 0)
                                            {
                                                if (gradientFaceIndex == liquidFaceIndex)
                                                    diagonal -= coefficient;
                                                else
                                                    localSparseElements.emplace_back(liquidFaceIndex, gradientFaceIndex, -coefficient);
                                            }
                                            else if (materialFaceLabels(localGradientFace, gradientFaceAxis) == MaterialLabels::SOLID_FACE)
                                                rhsVector(liquidFaceIndex) += coefficient * solidVelocity(localGradientFace, gradientFaceAxis);
                                            else
                                                assert(materialFaceLabels(localGradientFace, gradientFaceAxis) == MaterialLabels::AIR_FACE);
                                        }
                                    }
                                }
                            }
                        }

                        localSparseElements.emplace_back(liquidFaceIndex, liquidFaceIndex, diagonal);
                    }
                    else
                        assert(materialFaceLabels(face, faceAxis) != MaterialLabels::LIQUID_FACE);
                }
            });
        }

        mergeLocalThreadVectors(sparseElements, parallelSparseElements);
    }

    SparseMatrix sparseMatrix(liquidDOFCount, liquidDOFCount);
    sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());

    Eigen::ConjugateGradient<SparseMatrix, Eigen::Upper | Eigen::Lower> solver;
    solver.compute(sparseMatrix);
    solver.setTolerance(1E-3);

    if (solver.info() != Eigen::Success)
    {
        std::cout << "   Solver failed to build" << std::endl;
        return;
    }

    VectorXd solutionVector = solver.solveWithGuess(rhsVector, initialGuessVector);

    if (solver.info() != Eigen::Success)
    {
        std::cout << "   Solver failed to converge" << std::endl;
        return;
    }
    else
    {
        std::cout << "    Solver iterations:     " << solver.iterations() << std::endl;
        std::cout << "    Solver error: " << solver.error() << std::endl;
    }

    for (int faceAxis : {0, 1, 2})
    {
        tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
        {
            for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
            {
                Vec3i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

                int liquidFaceIndex = liquidFaceIndices(face, faceAxis);
                if (liquidFaceIndex >= 0)
                {
                    assert(materialFaceLabels(face, faceAxis) == MaterialLabels::LIQUID_FACE);
                    velocity(face, faceAxis) = solutionVector(liquidFaceIndex);
                }
            }
        });
    }
}

}


#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
#include <descartes_light/solvers/bgl/impl/event_visitors.hpp>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <boost/graph/dijkstra_shortest_paths.hpp>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType>
SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
  typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
  predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<std::size_t>::max());
  PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map);

  // Perform the search
  boost::dijkstra_shortest_paths(graph_,
                                 source_,
                                 predecessor_it_map,
                                 distance_prop_map,
                                 weight_prop_map,
                                 index_prop_map,
                                 std::less<>(),
                                 std::plus<>(),
                                 std::numeric_limits<FloatType>::max(),
                                 static_cast<FloatType>(0.0),
                                 boost::default_dijkstra_visitor(),
                                 color_prop_map);

  // Find lowest cost node in last rung
  auto target = std::min_element(ladder_rungs_.back().begin(),
                                 ladder_rungs_.back().end(),
                                 [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
                                   return graph_[a].distance < graph_[b].distance;
                                 });

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[*target].distance;

  return result;
}

template <typename FloatType>
SearchResult<FloatType> BGLEfficientDijkstraSVSESolver<FloatType>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph_);
  auto weight_prop_map = boost::get(boost::edge_weight, graph_);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);

  typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
  typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
  predecessors_.resize(boost::num_vertices(graph_), std::numeric_limits<std::size_t>::max());
  PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors_.begin(), index_prop_map);

  const long last_rung_idx = static_cast<long>(ladder_rungs_.size() - 1);
  auto visitor = boost::make_dijkstra_visitor(early_terminator<FloatType>(last_rung_idx));

  // Perform the search
  try
  {
    boost::dijkstra_shortest_paths(graph_,
                                   source_,
                                   predecessor_it_map,
                                   distance_prop_map,
                                   weight_prop_map,
                                   index_prop_map,
                                   std::less<>(),
                                   std::plus<>(),
                                   std::numeric_limits<FloatType>::max(),
                                   static_cast<FloatType>(0.0),
                                   visitor,
                                   color_prop_map);
  }
  catch (const VertexDesc<FloatType>& target)
  {
    SearchResult<FloatType> result;

    // Reconstruct the path from the predecesor map; remove the artificial start state
    const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
    result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
    result.trajectory.erase(result.trajectory.begin());

    result.cost = graph_[target].distance;

    return result;
  }

  // If the visitor never threw the vertex descriptor, there was an issue with the search
  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
}

}  // namespace descartes_light

#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * https://github.com/yesint/pteros
 *
 * (C) 2009-2020, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "pteros/extras/gnm.h"
#include "pteros/core/pteros_error.h"
#include "pteros/core/distance_search.h"
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ctime>

using namespace std;
using namespace pteros;
using namespace Eigen;

void GNM::compute(Selection& sel, float cutoff){
    int i,j,k;

    cout << "Running GNM..."<<endl;

    N = sel.size();
    eigenvalues.resize(N-1);
    eigenvectors.resize(N,N-1);

    cout << "Constructing GNM Kirkgoff matrix. Cut-off: " << cutoff << " Size: " << N << endl;
    MatrixXf kirk(N,N); //Kirgoff matrix
    float d, time1, time2;

    kirk.fill(0.0);
    // Compute off-diagonal elements
    /*
    for(i=0;i<N-1;++i)
        for(j=i+1;j<N;++j){
            d = (sel.xyz(i)-sel.xyz(j)).norm();
            if(d<=cutoff){
                kirk(i,j)= -1.0;
                kirk(j,i)= -1.0;
            }
        }
    */

    vector<Eigen::Vector2i> bon;
    search_contacts(cutoff,sel,bon);
    for(int i=0; i<bon.size(); ++i){
        kirk(bon[i](0),bon[i](1)) = kirk(bon[i](1),bon[i](0)) = -1.0;
    }

    // Compute diagonal elements
    for(i=0;i<N;++i) kirk(i,i) = -kirk.col(i).sum();

    cout << "Computing eigenvectors...";
    time1 = clock();

    Eigen::SelfAdjointEigenSolver<MatrixXf> solver(kirk);
    eigenvalues = solver.eigenvalues();
    eigenvectors = solver.eigenvectors();
    // Vectors are already sorted, cool :)

    time2 = clock();
    cout << " Done in " << (time2-time1)/CLOCKS_PER_SEC << " s." << endl;
}

GNM::GNM(Selection& sel, float cutoff){
    compute(sel,cutoff);
}

void GNM::write_eigenvectors(string fname, int v1, int v2){
    if(v1<0 || v2>N-2 || v2<v1) throw Pteros_error("Can't write these eigenvectors!");
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");
    int i,j;
    for(i=0;i<N;++i){
        f << i << " ";
        for(j=v1;j<=v2;++j) f << eigenvectors(i,j) << " ";
        f << endl;
    }
    f.close();
}

void GNM::compute_c_matrix(bool normalize){
    int i,j,k;
    cout << "Calculating correlations..." << endl;
    c.resize(N,N);
    c.fill(0.0);
    for(k=1;k<N;++k) // For all eigenvectors except first, which is zero
        for(i=0;i<N;++i)
            for(j=i;j<N;++j)
                c(i,j) = c(i,j) + eigenvectors(i,k)*eigenvectors(j,k)/eigenvalues(k);

    // Set lower triangle of the matrix
    for(i=0;i<N-1;++i)
        for(j=i+1;j<N;++j)
            c(j,i) = c(i,j);

    if(normalize)
        for(i=0;i<N;++i)
            for(j=0;j<N;++j)
                c(i,j) = c(i,j)/sqrt(c(i,i)*c(j,j));
}

void GNM::compute_p_matrix(){
    if(!c.size()) compute_c_matrix();
    cout << "Calculating correlations of correlation patterns...";
    int i,j;
    float time1,time2;

    time1 = clock();

    p.resize(N,N);
    p.fill(0.0);

    // See if c-matrix is ready
    if(!c.cols()) compute_c_matrix();

    // Pre-compute m and s
    VectorXf m(N), s(N);

    for(i=0;i<N;++i){
        m(i) = c.col(i).sum()/float(N);
        s(i) = c.col(i).array().pow(2).sum()/float(N) - pow(m(i),2);
    }

    for(i=0;i<N;++i)
        for(j=i;j<N;++j){
            p(i,j) = (c.col(i).array() * c.col(j).array()).sum() /float(N) - m(i)*m(j);
            p(i,j) = p(i,j) / sqrt(s(i)*s(j));
            p(j,i) = p(i,j);
        }

    time2 = clock();
    cout << " Done in " << (time2-time1)/CLOCKS_PER_SEC << " s." << endl;
}

void GNM::write_c_matrix(string fname){
    // See if c-matrix is ready
    if(!c.cols()) compute_c_matrix();

    int i,j;
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");
    for(i=0;i<N;++i){
        for(j=0;j<N;++j)
            f << c(i,j) << " ";
        f << endl;
    }
    f.close();
}

void GNM::write_p_matrix(string fname){
    // See if p-matrix is ready
    if(!p.cols()) compute_p_matrix();

    int i,j;
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");

    for(i=0;i<N;++i){
        for(j=0;j<N;++j)
            f << p(i,j) << " ";
        f << endl;
    }
    f.close();
}


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_CHOLESKY_LPDF_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_CHOLESKY_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/mat/meta/operands_and_partials.hpp>
#include <stan/math/prim/mat/fun/dot_self.hpp>
#include <stan/math/prim/mat/fun/log.hpp>
#include <stan/math/prim/mat/fun/mdivide_left_tri.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/meta/vector_seq_view.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/max_size_mvt.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {
/**
 * The log of the multivariate normal density for the given y, mu, and
 * a Cholesky factor L of the variance matrix.
 * Sigma = LL', a square, semi-positive definite matrix.
 *
 * Analytic expressions taken from
 * http://qwone.com/~jason/writing/multivariateNormal.pdf
 * written by Jason D. M. Rennie.
 *
 * All expressions are adapted to avoid (most) inversions and maximal
 * reuse of intermediates.
 *
 * @param y A scalar vector
 * @param mu The mean vector of the multivariate normal distribution.
 * @param L The Cholesky decomposition of a variance matrix
 * of the multivariate normal distribution
 * @return The log of the multivariate normal density.
 * @throw std::domain_error if LL' is not square, not symmetric,
 * or not semi-positive definite.
 * @tparam T_y Type of scalar.
 * @tparam T_loc Type of location.
 * @tparam T_covar Type of scale.
 */
template <bool propto, typename T_y, typename T_loc, typename T_covar>
typename return_type<T_y, T_loc, T_covar>::type multi_normal_cholesky_lpdf(
    const T_y& y, const T_loc& mu, const T_covar& L) {
  static const char* function = "multi_normal_cholesky_lpdf";
  typedef typename scalar_type<T_covar>::type T_covar_elem;
  typedef typename return_type<T_y, T_loc, T_covar>::type T_return;
  typedef typename stan::partials_return_type<T_y, T_loc, T_covar>::type
      T_partials_return;
  typedef Eigen::Matrix<T_partials_return, Eigen::Dynamic, Eigen::Dynamic>
      matrix_partials_t;
  typedef Eigen::Matrix<T_partials_return, Eigen::Dynamic, 1> vector_partials_t;
  typedef Eigen::Matrix<T_partials_return, 1, Eigen::Dynamic>
      row_vector_partials_t;

  vector_seq_view<T_y> y_vec(y);
  vector_seq_view<T_loc> mu_vec(mu);
  const size_t size_vec = max_size_mvt(y, mu);

  const int size_y = y_vec[0].size();
  const int size_mu = mu_vec[0].size();
  if (likely(size_vec > 1)) {
    // check size consistency of all random variables y
    int size_y_old = size_y;
    for (size_t i = 1, size_ = length_mvt(y); i < size_; i++) {
      int size_y_new = y_vec[i].size();
      check_size_match(function,
                       "Size of one of the vectors of "
                       "the random variable",
                       size_y_new,
                       "Size of another vector of the "
                       "random variable",
                       size_y_old);
      size_y_old = size_y_new;
    }
    // check size consistency of all means mu
    int size_mu_old = size_mu;
    for (size_t i = 1, size_ = length_mvt(mu); i < size_; i++) {
      int size_mu_new = mu_vec[i].size();
      check_size_match(function,
                       "Size of one of the vectors of "
                       "the location variable",
                       size_mu_new,
                       "Size of another vector of the "
                       "location variable",
                       size_mu_old);
      size_mu_old = size_mu_new;
    }
  }

  check_size_match(function, "Size of random variable", size_y,
                   "size of location parameter", size_mu);
  check_size_match(function, "Size of random variable", size_y,
                   "rows of covariance parameter", L.rows());
  check_size_match(function, "Size of random variable", size_y,
                   "columns of covariance parameter", L.cols());

  for (size_t i = 0; i < size_vec; i++) {
    check_finite(function, "Location parameter", mu_vec[i]);
    check_not_nan(function, "Random variable", y_vec[i]);
  }

  if (unlikely(size_y == 0))
    return T_return(0.0);

  T_partials_return logp(0.0);
  operands_and_partials<T_y, T_loc, T_covar> ops_partials(y, mu, L);

  if (include_summand<propto>::value)
    logp += NEG_LOG_SQRT_TWO_PI * size_y * size_vec;

  const matrix_partials_t inv_L_dbl
      = mdivide_left_tri<Eigen::Lower>(value_of(L));

  if (include_summand<propto, T_y, T_loc, T_covar_elem>::value) {
    for (size_t i = 0; i < size_vec; i++) {
      vector_partials_t y_minus_mu_dbl(size_y);
      for (int j = 0; j < size_y; j++)
        y_minus_mu_dbl(j) = value_of(y_vec[i](j)) - value_of(mu_vec[i](j));

      const row_vector_partials_t half
          = (inv_L_dbl.template triangularView<Eigen::Lower>() * y_minus_mu_dbl)
                .transpose();
      const vector_partials_t scaled_diff
          = (half * inv_L_dbl.template triangularView<Eigen::Lower>())
                .transpose();

      logp -= 0.5 * dot_self(half);

      if (!is_constant_struct<T_y>::value) {
        for (int j = 0; j < size_y; j++)
          ops_partials.edge1_.partials_vec_[i](j) -= scaled_diff(j);
      }
      if (!is_constant_struct<T_loc>::value) {
        for (int j = 0; j < size_y; j++)
          ops_partials.edge2_.partials_vec_[i](j) += scaled_diff(j);
      }
      if (!is_constant_struct<T_covar>::value) {
        ops_partials.edge3_.partials_ += scaled_diff * half;
      }
    }
  }

  if (include_summand<propto, T_covar_elem>::value) {
    logp += inv_L_dbl.diagonal().array().log().sum() * size_vec;
    if (!is_constant_struct<T_covar>::value) {
      ops_partials.edge3_.partials_ -= size_vec * inv_L_dbl.transpose();
    }
  }

  return ops_partials.build(logp);
}

template <typename T_y, typename T_loc, typename T_covar>
inline typename return_type<T_y, T_loc, T_covar>::type
multi_normal_cholesky_lpdf(const T_y& y, const T_loc& mu, const T_covar& L) {
  return multi_normal_cholesky_lpdf<false>(y, mu, L);
}

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


#include "source/newtonian/two_dimensional/hdsim2d.hpp"
#include "source/newtonian/common/hllc.hpp"
#include "source/newtonian/common/ideal_gas.hpp"
#include "source/newtonian/two_dimensional/amr.hpp"
#include "source/newtonian/two_dimensional/modular_flux_calculator.hpp"
#include "source/newtonian/two_dimensional/simple_cell_updater.hpp"
#include "source/newtonian/two_dimensional/simple_cfl.hpp"
#include "source/newtonian/two_dimensional/simple_extensive_updater.hpp"
#include "source/newtonian/two_dimensional/point_motions/lagrangian.hpp"
#include "source/newtonian/two_dimensional/point_motions/round_cells.hpp"
#include "source/newtonian/two_dimensional/interpolations/LinearGaussImproved.hpp"
#include "source/tessellation/VoronoiMesh.hpp"
#include "source/misc/mesh_generator.hpp"
#include "source/newtonian/two_dimensional/periodic_edge_velocities.hpp"
#include "source/newtonian/two_dimensional/geometric_outer_boundaries/PeriodicBox.hpp"
#include "source/newtonian/two_dimensional/ghost_point_generators/PeriodicGhostGenerator.hpp"
#include "source/newtonian/two_dimensional/source_terms/zero_force.hpp"
#include "source/newtonian/two_dimensional/hdf5_diagnostics.hpp"
#include <boost/random/uniform_int_distribution.hpp>
#include "source/misc/simple_io.hpp"

namespace {
class ConserveRefine : public CellsToRefine
{
public:
	vector<size_t> ToRefine(Tessellation const& tess, vector<ComputationalCell> const& /*cells*/, double time,
	TracerStickerNames const& /*ts*/)const
	{
		boost::random::mt19937_64 gen(static_cast<uint64_t>(time*10000));
		boost::random::uniform_int_distribution<> dist(0, tess.GetPointNo()-1);
		vector<size_t> res(10);
		for (size_t i = 0; i < res.size(); ++i)
		  res[i] = static_cast<size_t>(dist(gen));
		return res;
	}
};

class ConserveRemove : public CellsToRemove
{
public:
	std::pair<vector<size_t>, vector<double> > ToRemove(Tessellation const& tess, vector<ComputationalCell> const& /*cells*/, double time,
	TracerStickerNames const& /*ts*/)const
	{
		boost::random::mt19937_64 gen(static_cast<uint64_t>(time * 11000));
		boost::random::uniform_int_distribution<> dist(0,tess.GetPointNo()-1);
		std::pair<vector<size_t>, vector<double> > res(vector<size_t>(10), vector<double>(10));
		for (size_t i = 0; i < res.first.size(); ++i)
		{
		  res.first[i] = static_cast<size_t>(dist(gen));
			res.second[i] = static_cast<double>(dist(gen));
		}
		return res;
	}
};

double TotalMass(hdsim const& sim)
{
	double res = 0;
	for (int i = 0; i < sim.getTessellation().GetPointNo(); ++i)
		res += sim.getTessellation().GetVolume(i)*sim.getAllCells()[static_cast<size_t>(i)].density;
	return res;
}

vector<ComputationalCell> calc_cells(size_t n)
{
	vector<ComputationalCell> res(n);
	for (size_t i = 0; i < n; ++i)
	{
		res[i].density = 1;
		res[i].pressure = 1;
		res[i].velocity = Vector2D(1, 0);
	}
	return res;
}
}

int main(void)
{
	int np = 30;
	vector<Vector2D> points = cartesian_mesh(np, np, Vector2D(-1, -1), Vector2D(1, 1));
	PeriodicBox outer(Vector2D(-1, -1), Vector2D(1, 1));
	VoronoiMesh tess(points, outer);
	vector<ComputationalCell> cells = calc_cells(static_cast<size_t>(tess.GetPointNo()));
	Hllc rs;
	IdealGas eos(5. / 3.);
	PeriodicGhostGenerator ghost;
	PeriodicEdgeVelocities vedge;
	LinearGaussImproved interp(eos, ghost);
	SimpleCellUpdater cu;
	SimpleExtensiveUpdater eu;
	SimpleCFL tsf(0.3);
	ModularFluxCalculator fc(interp, rs);
	Lagrangian pm;
	SlabSymmetry pg;
	ZeroForce force;

	hdsim sim(tess, outer, pg, cells, eos, pm, vedge, force, tsf, fc, eu, cu);

	ConserveRefine refine;
	ConserveRemove remove;
	ConservativeAMR amr(refine, remove,true, &interp);

	for (size_t i = 0; i < 50; ++i)
	{
		sim.TimeAdvance2Heun();
		amr(sim);
	}

	write_number(TotalMass(sim), "mass.txt",9);
	write_snapshot_to_hdf5(sim, "final.h5");

	return 0;
}