#ifndef VOLINT_H
#define VOLINT_H

        /*******************************************************
        *                                                      *
        *  volInt.c                                            *
        *                                                      *
        *  This code computes volume integrals needed for      *
        *  determining mass properties of polyhedral bodies.   *
        *                                                      *
        *  For more information, see the accompanying README   *
        *  file, and the paper                                 *
        *                                                      *
        *  Brian Mirtich, "Fast and Accurate Computation of    *
        *  Polyhedral Mass Properties," journal of graphics    *
        *  tools, volume 1, number 1, 1996.                    *
        *                                                      *
        *  This source code is public domain, and may be used  *
        *  in any way, shape or form, free of charge.          *
        *                                                      *
        *  Copyright 1995 by Brian Mirtich                     *
        *                                                      *
        *  mirtich@cs.berkeley.edu                             *
        *  http://www.cs.berkeley.edu/~mirtich                 *
        *                                                      *
        *******************************************************/

/*
        Revision history

        26 Jan 1996     Program creation.

         3 Aug 1996     Corrected bug arising when polyhedron density
                        is not 1.0.  Changes confined to function main().
                        Thanks to Zoran Popovic for catching this one.

        27 May 1997     Corrected sign error in translation of inertia
                        product terms to center of mass frame.  Changes
                        confined to function main().  Thanks to
                        Chris Hecker.
*/

// Modified for tractor_converter

#include <boost/container_hash/hash.hpp>

#include <stdexcept>

#include <algorithm>
#include <cmath>
#include <utility>
#include <limits>
#include <vector>
#include <deque>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <string>
#include <functional>
#include <numeric>
#include <iterator>

namespace volInt{

namespace exception{
  struct negative_volume : public virtual std::runtime_error
  {
    using std::runtime_error::runtime_error;
  };
  struct zero_volume : public virtual std::runtime_error
  {
    using std::runtime_error::runtime_error;
  };
} // namespace exception

namespace invalid{
  const int vert_id =         -1;
  const int vertNorm_id =     -1;

  const int bodyColorOffset = -1;
  const int bodyColorShift =  -1;

  const int ref_vert_ind =    -1;

  const int wheel_id =        -1;
  const int weapon_id =       -1;
  const int wheel_weapon_id = -1;
} // namespace invalid

enum class rotation_axis : std::size_t {x = 0, y = 1, z = 2};

double degrees_to_radians(double degrees);

double sicher_angle_to_radians(int sicher_angle);
int radians_to_sicher_angle(double radians);

void rotate_point_by_axis(std::vector<double> &point,
                          double angle_sin,
                          double angle_cos,
                          rotation_axis axis);
void rotate_point_by_axis(std::vector<double> &point,
                          double angle,
                          rotation_axis axis);

std::vector<double> vector_scale(double norm, const std::vector<double> &vec);
void vector_scale_self(double norm, std::vector<double> &vec);

void vector_invert_self(std::vector<double> &vec);

std::vector<double> vector_plus(const std::vector<double> &first,
                                const std::vector<double> &second);
void vector_plus_self(std::vector<double> &first,
                      const std::vector<double> &second);
std::vector<double> vector_minus(const std::vector<double> &first,
                                 const std::vector<double> &second);
void vector_minus_self(std::vector<double> &first,
                       const std::vector<double> &second);

std::vector<double> vector_multiply(const std::vector<double> &first,
                                    const std::vector<double> &second);
void vector_multiply_self(std::vector<double> &first,
                          const std::vector<double> &second);
std::vector<double> vector_multiply(const std::vector<double> &vec,
                                    double num);
void vector_multiply_self(std::vector<double> &vec,
                          double num);

std::vector<double> vector_divide(const std::vector<double> &first,
                                  const std::vector<double> &second);
void vector_divide_self(std::vector<double> &first,
                        const std::vector<double> &second);
std::vector<double> vector_divide(const std::vector<double> &vec,
                                  double num);
void vector_divide_self(std::vector<double> &vec,
                        double num);

double vector_length(const std::vector<double> &vec);

double vector_length_between(
  const std::vector<double> &first,
  const std::vector<double> &second);

double vector_dot_product(
  const std::vector<double> &first,
  const std::vector<double> &second);

double vector_angle(
  const std::vector<double> &first,
  const std::vector<double> &second);

std::vector<double> vector_2d_divide(const std::vector<double> &vec,
                                     double num);

double vector_2d_length(const std::vector<double> &vec);

template<typename T>
std::vector<std::vector<T>> get_groups_of_connected_items(
  std::vector<T> orig_vec,
  std::function<bool(T first, T second)> check_connected_func)
{
  std::vector<std::vector<T>> groups;
  std::size_t orig_vec_size = orig_vec.size();
  groups.reserve(orig_vec_size);

  std::unordered_set<T> items_to_check(orig_vec.begin(), orig_vec.end());

  for(std::size_t cur_group_id = 0; !items_to_check.empty(); ++cur_group_id)
  {
    groups.push_back(std::vector<T>());
    groups[cur_group_id].reserve(orig_vec_size);

    T cur_item = *items_to_check.begin();
    items_to_check.erase(items_to_check.begin());

    std::unordered_set<T> connected_items_to_check({cur_item});
    connected_items_to_check.reserve(orig_vec_size);

    // Iterating over first element in group and all connected elements.
    while(!connected_items_to_check.empty())
    {
      T cur_item = *connected_items_to_check.begin();
      connected_items_to_check.erase(connected_items_to_check.begin());
      groups[cur_group_id].push_back(cur_item);

      std::unordered_set<T> new_connected_items;
      new_connected_items.reserve(orig_vec_size);

      for(auto cur_item_to_cmp : items_to_check)
      {
        if(check_connected_func(cur_item, cur_item_to_cmp))
        {
          new_connected_items.insert(cur_item_to_cmp);
        }
      }

      // Erasing all newly found connected items from items_to_check.
      for(auto new_connected_item : new_connected_items)
      {
        items_to_check.erase(items_to_check.find(new_connected_item));
      }

      connected_items_to_check.insert(new_connected_items.begin(),
                                      new_connected_items.end());
    }
  }

  groups.shrink_to_fit();

  return groups;
}

namespace calc_norms{
  unsigned long long int normal_to_key(const std::vector<double> &norm);
  std::vector<double> key_to_normal(unsigned long long int key);
} // namespace calc_norms

// ============================================================================
// Macros.
// ============================================================================

#define VOLINT_SQR(x)  ((x) * (x))
#define VOLINT_CUBE(x) ((x) * (x) * (x))

// ============================================================================
// Constants.
// ============================================================================

#define VOLINT_X 0
#define VOLINT_Y 1
#define VOLINT_Z 2

const double vector_scale_val = 1.0;

//// VANGERS SOURCE
//// How angle conversions work in Vangers code.
//#define M_PI 3.14159265358979323846
//
//Pi_len = 11
//const int Pi = 1 << Pi_len;
//2048
//
//#define GTOR(x) (double(x) * (M_PI / double(Pi)))
//#define RTOG(x) (round(x *   (double(Pi) / M_PI)))

#ifndef M_PI
  #define M_PI 3.14159265358979323846
#endif
const int sicher_angle_Pi = 2048; // "1 << Pi_len" where "Pi_len = 11".

const unsigned int min_float_precision = 3;
const double distinct_distance = 1.0 / std::pow(10, min_float_precision);
const double sqr_distinct_distance = VOLINT_SQR(distinct_distance);
const double density = 1.0;

const std::size_t axes_num = 3;
const std::size_t axes_2d_num = 2;

const std::vector<std::vector<std::size_t>> axes_by_plane =
  {
    {1, 2}, // x axis.
    {0, 2}, // y axis.
    {0, 1}, // z axis.
  };

const std::vector<std::vector<std::size_t>> axes_by_plane_continuous =
  {
    {1, 2}, // x axis.
    {2, 0}, // y axis.
    {0, 1}, // z axis.
  };

namespace color_ids {
  const unsigned int zero_reserved =    0;
  const unsigned int body =             1;
  const unsigned int max_colors_ids =   25;
  const unsigned int invalid_color_id = 1000003;
} // namespace color_ids

namespace calc_norms{
  const std::size_t expected_connected_polygons_per_vertex = 10;
  const std::size_t expected_connected_polygons_per_polygon = 10;

  const double to_integer_multiply = std::pow(10, min_float_precision);
  const unsigned long long int upper_bound =
    std::round(2 * vector_scale_val * to_integer_multiply);

  const unsigned long long int upper_bound_shift =
    static_cast<unsigned long long int>(std::log2(upper_bound)) + 1;

  const std::vector<unsigned long long int> to_key_shift =
    {
      0,
      upper_bound_shift,
      2 * upper_bound_shift,
    };

  const unsigned long long int key_to_normal_mask =
    (1 << upper_bound_shift) - 1;
} // namespace calc_norms

namespace generate_bound{
  typedef std::deque<std::size_t> layer_vert_inds;
  typedef std::map<std::size_t, layer_vert_inds> layers_inds_of_axis;
  typedef std::vector<layers_inds_of_axis> layers_inds_by_axis;

  const std::size_t expected_inter_verts_per_edge = 10;

  const std::size_t plane_extrs_num = 4;
  // Assumes that get_planes_4_extreme_points() generates points in this order.
  //   y
  // 3   2
  //       x
  // 0   1
  const std::vector<std::vector<std::size_t>> extr_lines =
    {
      {0, 1},
      {1, 2},
      {2, 3},
      {3, 0},
      {0, 1, 2, 3},
    };
  const std::size_t plane_middle_extr_num = extr_lines.size();

  const std::size_t end_z_layers_num_mechos = 2;
  const std::size_t end_z_layers_num_other = 3;

  enum class model_type{mechos, other};

  namespace model{

    const std::size_t verts_per_poly = 4;

    const std::vector<std::size_t> zero_reserved_face_inds = {4, 5, 6, 7};

    const std::size_t z_layers_num = 3;
    const std::size_t num_verts_per_z_layer = 9;
    const std::size_t num_verts = z_layers_num * num_verts_per_z_layer;
    // 6 sides * 4 polygons per side.
    const std::size_t num_faces = 6 * 4;

    //    y
    // e3 m2 e2
    // m3 m4 m1 x
    // e0 m0 e1
    // Converting to:
    //   y
    // 0 1 2
    // 3 4 5 x
    // 6 7 8
    const std::vector<std::size_t> extr_to_end =   {6, 8, 2, 0};
    const std::vector<std::size_t> middle_to_end = {7, 5, 1, 3, 4};

    const std::vector<std::vector<int>> face_ind_to_vert_inds =
      {
        // Getting top and low sides.
        // top
        // <-
        //  /\
        // 0 1 2
        // 3 4 5
        // 6 7 8
        {0, 3, 4, 1},
        {1, 4, 5, 2},
        {3, 6, 7, 4},
        {4, 7, 8, 5},
        // low
        // ->
        //  \/
        // 18 19 20
        // 21 22 23
        // 24 25 26
        {18, 19, 22, 21},
        {19, 20, 23, 22},
        {21, 22, 25, 24},
        {22, 23, 26, 25},

        // Getting front and back sides.
        // back
        // <-
        //  /\
        // 6  7  8
        // 15 16 17
        // 24 25 26
        {6,  15, 16, 7},
        {7,  16, 17, 8},
        {15, 24, 25, 16},
        {16, 25, 26, 17},
        // front
        // ->
        //  \/
        // 0  1  2
        // 9  10 11
        // 18 19 20
        {0,  1,  10, 9},
        {1,  2,  11, 10},
        {9,  10, 19, 18},
        {10, 11, 20, 19},

        // Getting left and right sides.
        // left
        // <-
        //  /\
        // 0  3  6
        // 9  12 15
        // 18 21 24
        {0,  9,  12, 3},
        {3,  12, 15, 6},
        {9,  18, 21, 12},
        {12, 21, 24, 15},
        // right
        // ->
        //  \/
        // 2  5  8
        // 11 14 17
        // 20 23 26
        {2,  5,  14, 11},
        {5,  8,  17, 14},
        {11, 14, 23, 20},
        {14, 17, 26, 23},
      };

    // low
    //        y
    //     18 19 20
    // -x  21 22 23  x
    //     24 25 26
    //       -y
    const std::vector<std::vector<std::size_t>> min_verts_to_adjust_by_wheel =
      {
        {18, 21, 24}, // -x
        {24, 25, 26}, // -y
      };
    const std::vector<std::vector<std::size_t>> max_verts_to_adjust_by_wheel =
      {
        {20, 23, 26}, // x
        {18, 19, 20}, // y
      };
    const std::unordered_map<std::size_t, std::vector<std::size_t>>
      min_layer_verts_to_center_by_extremes =
        {
          {22, {18, 20, 24, 26}},
          {19, {18, 20}},
          {23, {20, 26}},
          {25, {24, 26}},
          {21, {18, 24}},
        };
  } // namespace model
} // namespace generate_bound

// ============================================================================
// Data structures.
// ============================================================================

struct model_extreme_points
{

  model_extreme_points();
  model_extreme_points(const std::vector<double> &max,
                       const std::vector<double> &min);
  model_extreme_points(std::vector<double> &&max, std::vector<double> &&min);

  std::vector<double>       &max();
  const std::vector<double> &max() const;
  std::vector<double>       &min();
  const std::vector<double> &min() const;

  double xmax() const;
  double ymax() const;
  double zmax() const;

  double xmin() const;
  double ymin() const;
  double zmin() const;

  void set_xmax(double new_xmax);
  void set_ymax(double new_ymax);
  void set_zmax(double new_zmax);

  void set_xmin(double new_xmin);
  void set_ymin(double new_ymin);
  void set_zmin(double new_zmin);

  void get_most_extreme_cmp_cur(const model_extreme_points &other);
  void get_most_extreme_cmp_cur(const std::vector<double>  &point);
  void get_most_extreme(const std::vector<std::vector<double>> &points);

  std::vector<double> get_center();

  std::pair<std::vector<double>, std::vector<double>> extreme_points_pair;

};

struct model_offset
{

  model_offset();
  model_offset(const std::vector<double> &offset_point_arg);
  model_offset(std::vector<double>      &&offset_point_arg);

  double x_off() const;
  double y_off() const;
  double z_off() const;

  void set_x_off(double new_x_off);
  void set_y_off(double new_y_off);
  void set_z_off(double new_z_off);

  std::vector<double> offset_point;

};

typedef struct face
{
  face(int numVerts_arg);

  int numVerts;
  unsigned int color_id;
  int wheel_id;
  int weapon_id;
  std::vector<double> norm;
  double w;
  std::vector<int> verts;
  std::vector<int> vertNorms;
} FACE;

typedef struct polyhedron
{

  polyhedron();
  polyhedron(
    int numVerts_arg,
    int numVertNorms_arg,
    int numFaces_arg,
    int verts_per_poly_arg);

  void invertVertNorms();
  void reverse_polygons_orientation();

  std::vector<double> face_calc_normal(std::size_t face_ind);
  double face_calc_offset_w(           std::size_t face_ind);
  void faces_calc_params(); // Must be called again if model was moved.
  void faces_calc_params_inv_neg_vol();

  double get_vertex_angle(std::size_t face_ind, std::size_t vert_ind);

  void recalc_vertNorms(double max_smooth_angle);

  double check_volume();

  void get_extreme_points();

  std::vector<double> get_model_center();

  void move_model_to_point(const std::vector<double> &point);
  void move_model_to_point_inv_neg_vol(const std::vector<double> &point);

  void move_coord_system_to_point(const std::vector<double> &point);
  void move_coord_system_to_point_inv_neg_vol(
    const std::vector<double> &point);
  void move_coord_system_to_center();

  void rotate_by_axis(double angle, rotation_axis axis);

  void set_color_id(unsigned int new_color_id,
                    int new_wheel_id =  invalid::wheel_id,
                    int new_weapon_id = invalid::weapon_id);

  bool find_ref_points();

  void calculate_rmax();
  void calculate_c3d_properties();

  std::vector<std::vector<std::vector<double>>>
    get_planes_4_extreme_points() const;
  static std::vector<std::unordered_map<std::size_t, double>>
    get_verts_plane_lengths_rel_points(
      std::size_t perpendicular_axis,
      const std::vector<std::vector<double>> &verts_arg,
      const std::vector<std::vector<double>> &points_2d,
      const generate_bound::layer_vert_inds &vert_inds);
  static generate_bound::layer_vert_inds get_min_length_layer_points(
    const std::vector<std::unordered_map<std::size_t, double>> &
      verts_plane_lengths_rel_points);
  static double get_plane_area_from_points(
    std::size_t perpendicular_axis,
    const std::vector<std::vector<double>> &verts_arg,
    const generate_bound::layer_vert_inds &vert_inds);
  static std::vector<std::vector<double>> get_extr_middle_points(
    std::size_t perpendicular_axis,
    const std::vector<std::vector<double>> &verts_arg,
    const generate_bound::layer_vert_inds &layer_extrs);
  polyhedron extr_inds_to_bound(
    const std::vector<std::vector<double>> &verts_arg,
    const generate_bound::layers_inds_of_axis &extr_inds,
    const generate_bound::layers_inds_of_axis &middle_inds,
    generate_bound::model_type type,
    const model_extreme_points *wheel_params_extremes = nullptr) const;
  polyhedron generate_bound_model(
    const generate_bound::model_type type,
    const std::size_t layers_num,
    const double area_threshold_multiplier,
    const model_extreme_points *wheel_params_extremes = nullptr) const;

  std::pair<std::vector<double>, std::vector<double>> &extreme_points_pair();
  const std::pair<std::vector<double>, std::vector<double>> &
    extreme_points_pair() const;
  std::vector<double>       &max_point();
  const std::vector<double> &max_point() const;
  std::vector<double>       &min_point();
  const std::vector<double> &min_point() const;

  double xmax() const;
  double ymax() const;
  double zmax() const;

  double xmin() const;
  double ymin() const;
  double zmin() const;

  void set_xmax(double new_xmax);
  void set_ymax(double new_ymax);
  void set_zmax(double new_zmax);

  void set_xmin(double new_xmin);
  void set_ymin(double new_ymin);
  void set_zmin(double new_zmin);

  std::vector<double>       &offset_point();
  const std::vector<double> &offset_point() const;

  double x_off() const;
  double y_off() const;
  double z_off() const;

  void set_x_off(double new_x_off);
  void set_y_off(double new_y_off);
  void set_z_off(double new_z_off);

  int numVerts, numVertNorms, numFaces, numVertTotal, numVertsPerPoly;
  model_extreme_points extreme_points;
  model_offset offset;
  double rmax;
  double volume;
  std::vector<double> rcm;
  std::vector<std::vector<double>> J;
  int bodyColorOffset, bodyColorShift;
  std::vector<std::vector<double>> verts;
  std::vector<std::vector<double>> vertNorms;
  std::vector<face> faces;

  std::pair<int, int>        ref_vert_one_ind;
  const std::vector<double> *ref_vert_one;
  std::pair<int, int>        ref_vert_two_ind;
  const std::vector<double> *ref_vert_two;
  std::pair<int, int>        ref_vert_three_ind;
  const std::vector<double> *ref_vert_three;

  std::vector<double> ref_vert_two_rel_to_one;
  std::vector<double> ref_vert_three_rel_to_one;

  double ref_angle;

  std::unordered_set<std::size_t> wheels;
  std::unordered_set<std::size_t> wheels_steer;
  std::unordered_set<std::size_t> wheels_non_steer;
  std::unordered_set<std::size_t> wheels_ghost;
  std::unordered_set<std::size_t> wheels_non_ghost;

  // Holds wheel id in case model itself is a wheel.
  // Otherwise value is invalid::wheel_id, which is -1.
  int wheel_id;

  // Used only when converting from *.obj to *.m3d/*.a3d.
  std::string wavefront_obj_path;

  // Used only when converting from *.m3d/*.a3d to *.obj.
  bool volume_overwritten;
  bool rcm_overwritten;
  bool J_overwritten;
} POLYHEDRON;

// ============================================================================
// Globals.
// ============================================================================

static int A; // alpha
static int B; // beta
static int C; // gamma

// Projection integrals.
static double P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb;

// Face integrals.
static double Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca;

// Volume integrals.
static double T0, T1[3], T2[3], TP[3];

// ============================================================================
// Compute mass properties.
// ============================================================================

// Compute various integrations over projection of face.
void compProjectionIntegrals(POLYHEDRON *p, FACE *f);

void compFaceIntegrals(FACE *f);

void compVolumeIntegrals(POLYHEDRON *p);

} // namespace volInt

#endif // VOLINT_H


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define FUSION_MAX_VECTOR_SIZE 25
#include <iostream>
//#include <fstream>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/solvers.hh"

#include "algorithm/giant_gbit.hh"
#include "fem/gridmanager.hh"
#include "fem/lagrangespace.hh"
//#include "fem/hierarchicspace.hh"   // ContinuousHierarchicMapper
#include "io/vtk.hh"
#include "utilities/kaskopt.hh"
#include "utilities/gridGeneration.hh" //  createUnitSquare

using namespace Kaskade;

#include "sst.hh"

struct InitialValue
{
  using Scalar = double;
  static constexpr int components = 1;
  using ValueType = Dune::FieldVector<Scalar,components>;

  InitialValue(int c): component(c) {}

  template <class Cell> int order(Cell const&) const { return std::numeric_limits<int>::max(); }
  template <class Cell>
  ValueType value(Cell const& cell,
  Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> const& localCoordinate) const
  {
  // Dune::FieldVector<typename Cell::Geometry::ctype,Cell::Geometry::coorddimension> x = cell.geometry().global(localCoordinate);
  if (component==0)
    return 1.0e9;
  else if (component==1)
    return 1.0e9;
  else if (component==2)
    return 1.0e13;
  else if (component==3)
    return 1.0e7;
  else
    assert("wrong index!\n"==0);
  return 0;

  }

private:
  int component;
};

int main(int argc, char *argv[])
{
  using Scalar = double;
  using namespace boost::fusion;

  std::cout << "Start sst transfer tutorial program with inexact damped Newton iteration " << std::endl;

  boost::timer::cpu_timer totalTimer;

  int verbosityOpt = 1;
  bool dump = true;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosityOpt, dump);

  int refinements = getParameter(pt, "refinement", 5),
      order =  getParameter(pt, "order", 2);
//      verbosity   = getParameter(pt, "verbosity", 0);
  Scalar tol = getParameter(pt, "tolerance", 1.0e-10),
         rho = getParameter(pt, "safetyfactor",0.0625);
  //   IterateType iterateType = IterateType::CG;
  //   PrecondType precondType = PrecondType::NONE;
  std::string empty;

  std::cout << "refinements of original mesh   : " << refinements << std::endl;
  std::cout << "discretization order           : " << order << std::endl;
  std::cout << "tolerance for Newton iteration : " << tol << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  //direct = getParameter(pt, s, 0);

  //   s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  //   iterateType = static_cast<IterateType>(getParameter(pt, s, 0));
  //   s = "names.preconditioner." + getParameter(pt, "solver.preconditioner", empty);
  //   precondType = static_cast<PrecondType>(getParameter(pt, s, 0));

  constexpr int dim=2;
  using Grid = Dune::UGGrid<dim>;
  using LeafView = Grid::LeafGridView;
  using H1Space = FEFunctionSpace<ContinuousLagrangeMapper<Scalar,LeafView> >;
// using H1Space = FEFunctionSpace<ContinuousHierarchicMapper<Scalar,LeafView> >;
  using Spaces = boost::fusion::vector<H1Space const*>;
  using VariableDescriptions = boost::fusion::vector<Variable<SpaceIndex<0>,Components<1>,VariableId<0> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<1> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<2> >,
                               Variable<SpaceIndex<0>,Components<1>,VariableId<3> > >;
  using VariableSet = VariableSetDescription<Spaces,VariableDescriptions>;
  using Functional = SSTFunctional<Scalar,VariableSet>;
  using NonlinearSolver = Giant<Grid,Functional,VariableSet,Spaces>;

  GridManager<Grid> gridManager( createUnitSquare<Grid>() );
  gridManager.globalRefine(refinements);
  std::cout << std::endl << "Grid: " << gridManager.grid().size(0) << " triangles, " << std::endl;
  std::cout << "      " << gridManager.grid().size(1) << " edges, " << std::endl;
  std::cout << "      " << gridManager.grid().size(2) << " points" << std::endl;

  // construction of finite element space for the scalar solution T
  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),
               order);
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  std::string varNames[4] = { "u0", "u1", "u2", "u3" };
  VariableSet variableSet(spaces,varNames);

  Functional F;
  VariableSet::VariableSet x(variableSet);

  F.scaleInitialValue<0>(InitialValue(0),x);
  F.scaleInitialValue<1>(InitialValue(1),x);
  F.scaleInitialValue<2>(InitialValue(2),x);
  F.scaleInitialValue<3>(InitialValue(3),x);

  LeafView leafGridView = gridManager.grid().leafGridView();
  writeVTKFile(x,"graph/sst_giant_start",IoOptions().setOrder(std::min(order,2)).setPrecision(7));
  gridManager.enforceConcurrentReads(false);

//   constexpr int nvars = Functional::AnsatzVars::noOfVariables;
//   constexpr int n     = variableSet.degreesOfFreedom(0,nvars);
//   std::vector<double> scale(n);
//   x.write(scale.begin());
//   for (int i=0;i<n;i++) scale[i]=2*scale[i];

//   std::fstream monitorStream;
//   monitorStream.open("sst_giant.mon",std::fstream::out);
//   if ( !monitorStream.is_open() )
//   {
//      std::cout << " Failed to open monitorStream file sst_giant.mon" << std::endl;
//      exit(-1);
//   };

  NonlinearSolver nonlinearSolver;

  s = "names.nonlinType." + getParameter(pt, "solver.nonlinType", empty);
  NonlinearSolver::NonlinProblemType
    nonlinType=static_cast<NonlinearSolver::NonlinProblemType>(getParameter(pt,s,(int) NonlinearSolver::NonlinProblemType::highlyNonlinear));

  int preconFillLevel=getParameter(pt,"preconlevel",0);
  nonlinearSolver.setTolerance(tol);
//   nonlinearSolver.setMaximumNoIterations(50);
  nonlinearSolver.setPreconFillLevel(preconFillLevel);
  nonlinearSolver.setSafetyFactor(rho);
//   nonlinearSolver.setErrorLevel(NonlinearSolver::verbose);
  nonlinearSolver.setMonitorLevel(NonlinearSolver::verbose);
  nonlinearSolver.setDataLevel(NonlinearSolver::verbose);
//  nonlinearSolver.setErrorStream(std::cout);
//  nonlinearSolver.setMonitorStream(std::cout);
//   nonlinearSolver.setMonitorStream(monitorStream);
   nonlinearSolver.setNonlinProblemType( nonlinType );
//   nonlinearSolver.setRestricted(false);
//  nonlinearSolver.setScalingVector(&scale);
  nonlinearSolver.setOutFilePrefix((std::string) "graph/sst_giant_");

  boost::timer::cpu_timer nonlinTimer;
  nonlinearSolver.giantGbit(gridManager,F,variableSet,&x,spaces);
  std::cout << "computing time for nonlinear solver: " << boost::timer::format(nonlinTimer.elapsed()) << "\n";
  struct NonlinearSolver::NleqInfo info=nonlinearSolver.getInfo();
//  monitorStream << " The total number of iterative linear solver steps done is " <<
  std::cout << " The total number of iterative linear solver steps done is " <<
               info.noOrdLinIt+info.noSimLinIt << std::endl;
  std::string finalOutputFilename;
  if ( info.returnCode==0 )
    finalOutputFilename="graph/sst_giant_solution";
  else
    finalOutputFilename="graph/sst_giant_final";
  writeVTKFile(x,finalOutputFilename,IoOptions().setOrder(std::min(order,2)).setPrecision(7));

  std::cout << "total computing time: " << boost::timer::format(totalTimer.elapsed()) << "\n";
  std::cout << "End sst transfer tutorial program" << std::endl;
}


#ifndef STAN_OPTIMIZATION_NEWTON_HPP
#define STAN_OPTIMIZATION_NEWTON_HPP

#include <stan/model/grad_hess_log_prob.hpp>
#include <stan/model/log_prob_grad.hpp>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/Eigenvalues>
#include <vector>

namespace stan {
namespace optimization {

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_d;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector_d;

// Negates any positive eigenvalues in H so that H is negative
// definite, and then solves Hu = g and stores the result into
// g. Avoids problems due to non-log-concave distributions.
inline void make_negative_definite_and_solve(matrix_d& H, vector_d& g) {
  Eigen::SelfAdjointEigenSolver<matrix_d> solver(H);
  matrix_d eigenvectors = solver.eigenvectors();
  vector_d eigenvalues = solver.eigenvalues();
  vector_d eigenprojections = eigenvectors.transpose() * g;
  for (int i = 0; i < g.size(); i++) {
    eigenprojections[i] = -eigenprojections[i] / fabs(eigenvalues[i]);
  }
  g = eigenvectors * eigenprojections;
}

template <typename M>
double newton_step(M& model, std::vector<double>& params_r,
                   std::vector<int>& params_i,
                   std::ostream* output_stream = 0) {
  std::vector<double> gradient;
  std::vector<double> hessian;

  double f0 = stan::model::grad_hess_log_prob<true, false>(
      model, params_r, params_i, gradient, hessian);
  matrix_d H(params_r.size(), params_r.size());
  for (size_t i = 0; i < hessian.size(); i++) {
    H(i) = hessian[i];
  }
  vector_d g(params_r.size());
  for (size_t i = 0; i < gradient.size(); i++)
    g(i) = gradient[i];
  make_negative_definite_and_solve(H, g);
  //         H.ldlt().solveInPlace(g);

  std::vector<double> new_params_r(params_r.size());
  double step_size = 2;
  double min_step_size = 1e-50;
  double f1 = -1e100;

  while (f1 < f0) {
    step_size *= 0.5;
    if (step_size < min_step_size)
      return f0;

    for (size_t i = 0; i < params_r.size(); i++)
      new_params_r[i] = params_r[i] - step_size * g[i];
    try {
      f1 = stan::model::log_prob_grad<true, false>(model, new_params_r,
                                                   params_i, gradient);
    } catch (std::exception& e) {
      // FIXME:  this is not a good way to handle a general exception
      f1 = -1e100;
    }
  }
  for (size_t i = 0; i < params_r.size(); i++)
    params_r[i] = new_params_r[i];

  return f1;
}

}  // namespace optimization
}  // namespace stan
#endif


#include <cmath>
#include <complex>
#include <ctime>
#include <string>
// Boost random generator
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
// Eigen dense matrices
#include <Eigen/Dense>

#include "function_dispatcher.h"
#include "json_object.h"
#include "numeric_utils.h"
#include "wittig_sinha.h"

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed, double height,
                                     unsigned int num_floors, double total_time)
    : StochasticModel(),
      exposure_category_{exposure_category},
      gust_speed_{gust_speed * 0.44704}, // Convert from mph to m/s
      bldg_height_{height},
      num_floors_{num_floors},
      seed_value_{std::numeric_limits<int>::infinity()},
      local_x_{std::vector<double>(1, 1.0)},
      local_y_{std::vector<double>(1, 1.0)},
      freq_cutoff_{5.0},
      time_step_{1.0 / (2.0 * freq_cutoff_)} {
  model_name_ = "WittigSinha";
  num_times_ =
      static_cast<unsigned int>(std::ceil(total_time / time_step_)) % 2 == 0
          ? static_cast<unsigned int>(std::ceil(total_time / time_step_))
          : static_cast<unsigned int>(std::ceil(total_time / time_step_) + 1);

  // Calculate range of frequencies based on cutoff frequency
  num_freqs_ = num_times_ / 2;
  frequencies_.resize(num_freqs_);

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    frequencies_[i] = (i + 1) * freq_cutoff_ / num_freqs_;
  }

  // Calculate heights of each floor
  heights_ = std::vector<double>(num_floors_);
  heights_[0] = bldg_height_ / num_floors_;

  for (unsigned int i = 1; i < heights_.size(); ++i) {
    heights_[i] = heights_[i - 1] + bldg_height_ / num_floors_;
  }

  // Calculate velocity profile
  friction_velocity_ =
      Dispatcher<double, const std::string&, const std::vector<double>&, double,
                 double, std::vector<double>&>::instance()
          ->dispatch("ExposureCategoryVel", exposure_category, heights_, 0.4,
                     gust_speed, wind_velocities_);
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed, double height,
                                     unsigned int num_floors, double total_time,
                                     int seed_value)
    : WittigSinha(exposure_category, gust_speed, height, num_floors,
                  total_time)
{
  seed_value_ = seed_value;
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed,
                                     const std::vector<double>& heights,
                                     const std::vector<double>& x_locations,
                                     const std::vector<double>& y_locations,
                                     double total_time)
    : StochasticModel(),
      exposure_category_{exposure_category},
      gust_speed_{gust_speed * 0.44704}, // Convert from mph to m/s
      seed_value_{std::numeric_limits<int>::infinity()},
      heights_{heights},
      local_x_{x_locations},
      local_y_{y_locations},
      freq_cutoff_{5.0},
      time_step_{1.0 / (2.0 * freq_cutoff_)}
{
  model_name_ = "WittigSinha";
  num_times_ =
      static_cast<unsigned int>(std::ceil(total_time / time_step_)) % 2 == 0
          ? static_cast<unsigned int>(std::ceil(total_time / time_step_))
          : static_cast<unsigned int>(std::ceil(total_time / time_step_) + 1);

  // Calculate range of frequencies based on cutoff frequency
  num_freqs_ = num_times_ / 2;
  frequencies_.resize(num_freqs_);

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    frequencies_[i] = i * freq_cutoff_ / num_freqs_;
  }

  // Calculate velocity profile
  friction_velocity_ =
      Dispatcher<double, const std::string&, const std::vector<double>&, double,
                 double, std::vector<double>&>::instance()
          ->dispatch("ExposureCategoryVel", exposure_category, heights_, 0.4,
                     gust_speed, wind_velocities_);
}

stochastic::WittigSinha::WittigSinha(std::string exposure_category,
                                     double gust_speed,
                                     const std::vector<double>& heights,
                                     const std::vector<double>& x_locations,
                                     const std::vector<double>& y_locations,
                                     double total_time, int seed_value)
  : WittigSinha(exposure_category, gust_speed, heights, x_locations, y_locations, total_time)
{
  seed_value_ = seed_value;
}

utilities::JsonObject stochastic::WittigSinha::generate(const std::string& event_name, bool units) {
  // Initialize wind velocity vectors
  std::vector<std::vector<std::vector<std::vector<double>>>> wind_vels(
      local_x_.size(),
      std::vector<std::vector<std::vector<double>>>(
          local_y_.size(),
          std::vector<std::vector<double>>(
              heights_.size(), std::vector<double>(num_times_, 0.0))));

  Eigen::MatrixXcd complex_random_vals(num_freqs_, heights_.size());

  // Loop over heights to find time histories
  try {
    for (unsigned int i = 0; i < local_x_.size(); ++i) {
      for (unsigned int j = 0; j < local_y_.size(); ++j) {
        // Generate complex random numbers to use for calculation of discrete
        // time series
        complex_random_vals = complex_random_numbers();
        for (unsigned int k = 0; k < heights_.size(); ++k) {
          wind_vels[i][j][k] = gen_location_hist(complex_random_vals, k, units);
        }
      }
    }
  } catch (const std::exception& e) {
    std::cerr << "\nERROR: In stochastic::WittigSinha::generate: "
              << e.what() << std::endl;
  }

  // Create JsonObject for event
  auto event = utilities::JsonObject();
  event.add_value("dT", time_step_);
  event.add_value("numSteps", num_times_);

  // Consider case when only looking at floor loads, so only have time histories as
  // one location along the z-axis
  if (local_x_.size() == 1 && local_y_.size() == 1) {
    // Arrays of patterns and time histories for each floor
    std::vector<utilities::JsonObject> pattern_array(heights_.size());
    std::vector<utilities::JsonObject> event_array(1);
    std::vector<utilities::JsonObject> time_history_array(heights_.size());
    auto time_history = utilities::JsonObject();
    event_array[0].add_value("type", "Wind");
    event_array[0].add_value("subtype", model_name_);

    for (unsigned int i = 0; i < heights_.size(); ++i) {
      // Create pattern
      pattern_array[i].add_value("name", std::to_string(i + 1));
      pattern_array[i].add_value("timeSeries", std::to_string(i + 1));
      pattern_array[i].add_value("type", "WindFloorLoad");
      pattern_array[i].add_value("floor", std::to_string(i + 1));
      pattern_array[i].add_value("dof", 1);
      pattern_array[i].add_value("profileVelocity", wind_velocities_[i]);

      // Create time histories
      time_history.add_value("name", std::to_string(i + 1));
      time_history.add_value("dT", time_step_);
      time_history.add_value("type", "Value");
      time_history.add_value("data", wind_vels[0][0][i]);
      time_history_array[i] = time_history;
      time_history.clear();
    }

    event_array[0].add_value("timeSeries", time_history_array);
    event_array[0].add_value("pattern", pattern_array);
    event.add_value("Events", event_array);
  } else {
    throw std::runtime_error(
        "ERROR: In stochastic::WittigSinha::generate: Currently, only supports "
        "time histories along z-axis at single location\n");
  }

  return event;
}

bool stochastic::WittigSinha::generate(const std::string& event_name,
                                       const std::string& output_location,
                                       bool units) {

  bool status = true;
  // Generate time histories at specified locations
  try {
    auto json_output = generate(event_name, units);
    json_output.write_to_file(output_location);
  } catch (const std::exception& e) {
    std::cerr << e.what();
    status = false;
    throw;
  }

  return status;
}

Eigen::MatrixXd stochastic::WittigSinha::cross_spectral_density(double frequency) const {
  // Coefficient for coherence function
  double coherence_coeff = 10.0;
  Eigen::MatrixXd cross_spectral_density =
      Eigen::MatrixXd::Zero(heights_.size(), heights_.size());

  for (unsigned int i = 0; i < cross_spectral_density.rows(); ++i) {
    cross_spectral_density(i, i) =
        200.0 * friction_velocity_ * friction_velocity_ * heights_[i] /
        (wind_velocities_[i] *
         std::pow(1.0 + 50.0 * frequency * heights_[i] / wind_velocities_[i],
                  5.0 / 3.0));
  }

  for (unsigned int i = 0; i < cross_spectral_density.rows(); ++i) {
    for (unsigned int j = i + 1; j < cross_spectral_density.cols(); ++j) {
      cross_spectral_density(i, j) =
          std::sqrt(cross_spectral_density(i, i) *
                    cross_spectral_density(j, j)) *
          std::exp(-coherence_coeff * frequency *
                   std::abs(heights_[i] - heights_[j]) /
                   (0.5 * (wind_velocities_[i] + wind_velocities_[j]))) *
          0.999;
    }
  }

  // Get diagonal of cross spectral density matrix--avoids compiler errors where type
  // of diagonal matrix is not correctly deduced
  Eigen::MatrixXd diag_mat = cross_spectral_density.diagonal().asDiagonal();

  return cross_spectral_density.transpose() + cross_spectral_density - diag_mat;
}

Eigen::MatrixXcd stochastic::WittigSinha::complex_random_numbers() const {
  // Construct random number generator for standard normal distribution
  static unsigned int history_seed = static_cast<unsigned int>(std::time(nullptr));
  history_seed = history_seed + 10;

  auto generator =
    seed_value_ != std::numeric_limits<int>::infinity()
    ? boost::random::mt19937(static_cast<unsigned int>(seed_value_ + 10))
    : boost::random::mt19937(history_seed);

  boost::random::normal_distribution<> distribution;
  boost::random::variate_generator<boost::random::mt19937&,
                                   boost::random::normal_distribution<>>
      distribution_gen(generator, distribution);

  // Generate white noise consisting of complex numbers
  Eigen::MatrixXcd white_noise(heights_.size(), num_freqs_);

  for (unsigned int i = 0; i < white_noise.rows(); ++i) {
    for (unsigned int j = 0; j < white_noise.cols(); ++j) {
      white_noise(i, j) = std::complex<double>(
          distribution_gen() * std::sqrt(0.5),
          distribution_gen() * std::sqrt(std::complex<double>(-0.5)).imag());
    }
  }

  // Iterator over all frequencies and generate complex random numbers
  // for discrete time series simulation
  Eigen::MatrixXd cross_spec_density_matrix(heights_.size(), heights_.size());
  Eigen::MatrixXcd complex_random(num_freqs_, heights_.size());

  for (unsigned int i = 0; i < frequencies_.size(); ++i) {
    // Calculate cross-spectral density matrix for current frequency
    cross_spec_density_matrix = cross_spectral_density(frequencies_[i]);

    // Find lower Cholesky factorization of cross-spectral density
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> lower_cholesky;

    try {
      auto llt = cross_spec_density_matrix.llt();
      lower_cholesky = llt.matrixL();

      if (llt.info() == Eigen::NumericalIssue) {
        throw std::runtime_error(
            "\nERROR: In stochastic::WittigSinha::generate method: Cross-Spectral Density "
            "matrix is not positive semi-definite\n");
      }
    } catch (const std::exception& e) {
      std::cerr << "\nERROR: In time history generation: " << e.what()
                << std::endl;
    }

    // This is Equation 5(a) from Wittig & Sinha (1975)
    complex_random.row(i) = num_freqs_ *
                            std::sqrt(2.0 * freq_cutoff_ / num_freqs_) *
                            lower_cholesky * white_noise.col(i);
  }

  return complex_random;
}

std::vector<double> stochastic::WittigSinha::gen_location_hist(
    const Eigen::MatrixXcd& random_numbers, unsigned int column_index,
    bool units) const {

  // This following block implements what is expressed in Equations 7 & 8
  Eigen::VectorXcd complex_full_range = Eigen::VectorXcd::Zero(2 * num_freqs_);

  complex_full_range.segment(1, num_freqs_) =
      random_numbers.block(0, column_index, num_freqs_, 1);

  complex_full_range.segment(num_freqs_ + 1, num_freqs_ - 1) =
      random_numbers.block(0, column_index, num_freqs_ - 1, 1)
          .reverse()
          .conjugate();

  complex_full_range(num_freqs_) = std::abs(random_numbers(num_freqs_ - 1, column_index));

  // Calculate wind speed using real portion of inverse Fast Fourier Transform
  // full range of random numbers
  std::vector<double> node_time_history(complex_full_range.size());
  numeric_utils::inverse_fft(complex_full_range, node_time_history);

  // Check if time histories need to be converted to ft/s
  if (units) {
    for (auto & val : node_time_history) {
      val = val * 3.28084;
    }
  }

  return node_time_history;
}


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

#include <boost/simd/arithmetic/functions/fast_hypot.hpp>
#include <boost/simd/include/functions/scalar/sqrt.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/is_inf.hpp>
#include <boost/simd/include/functions/scalar/ldexp.hpp>
#include <boost/simd/include/functions/scalar/fma.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::fast_hypot(static_cast<result_type>(a0),
                                     static_cast<result_type>(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && !defined(BOOST_SIMD_NO_INFINITIES)
      A0 x =  boost::simd::abs(a0);
      A0 y =  boost::simd::abs(a1);
      if (boost::simd::is_inf(x+y)) return Inf<float>();
      if (boost::simd::is_inf(x)) return Inf<float>();
      if (boost::simd::is_inf(y)) return Inf<float>();
      if (y > x) std::swap(x, y);
      if (x*Eps<A0>() >=  y) return x;
      return x*boost::simd::sqrt(One<A0>()+boost::simd::sqr(y/x));
#else
      return boost::simd::sqrt(boost::simd::fma(a0, a0, a1*a1));
#endif
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::fast_hypot_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                              (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
#if !defined(BOOST_SIMD_NO_NANS) && !defined(BOOST_SIMD_NO_INFINITIES)
      // flibc do that in ::fast_hypotf(a0, a1) in asm with no more speed!
      // proper impl as for double is 30% slower
      return static_cast<result_type>(boost::simd::sqrt(fma(static_cast<double>(a0), static_cast<double>(a0),
                                                            boost::simd::sqr(static_cast<double>(a1)))));
#else
      return boost::simd::sqrt(boost::simd::fma(a0, a0, a1*a1));
#endif
    }
  };
} } }

#endif


// ---------------------------------------------------------------------
//
// Copyright (c) 2014 - 2019 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

/////////////////////////////// INCLUDES /////////////////////////////////////

#include "ibamr/IBFEPatchRecoveryPostProcessor.h"
#include "ibamr/IBHierarchyIntegrator.h"
#include "ibamr/namespaces.h" // IWYU pragma: keep

#include "ibtk/IBTK_CHKERRQ.h"
#include "ibtk/IndexUtilities.h"
#include "ibtk/LEInteractor.h"
#include "ibtk/libmesh_utilities.h"

#include "SAMRAI_config.h"

#include "libmesh/boundary_info.h"
#include "libmesh/dense_vector.h"
#include "libmesh/dof_map.h"
#include "libmesh/equation_systems.h"
#include "libmesh/fe_base.h"
#include "libmesh/fe_interface.h"
#include "libmesh/mesh.h"
#include "libmesh/periodic_boundaries.h"
#include "libmesh/periodic_boundary.h"
#include "libmesh/petsc_vector.h"
#include "libmesh/quadrature.h"
#include "libmesh/string_to_enum.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <boost/multi_array.hpp>
IBTK_ENABLE_EXTRA_WARNINGS

IBTK_DISABLE_EXTRA_WARNINGS
#include "Eigen/Dense"
IBTK_ENABLE_EXTRA_WARNINGS

using namespace libMesh;

/////////////////////////////// NAMESPACE ////////////////////////////////////

namespace IBAMR
{
/////////////////////////////// STATIC ///////////////////////////////////////

namespace
{
unsigned int
num_polynomial_basis_fcns(const unsigned int dim, const unsigned int order)
{
    unsigned int num_basis_fcns = 0;
    unsigned int order_p_1 = order + 1;
    switch (dim)
    {
    case 1:
        num_basis_fcns = order_p_1;
        break;
    case 2:
        num_basis_fcns = order_p_1 * order_p_1 + order_p_1;
        num_basis_fcns /= 2;
        break;
    case 3:
        num_basis_fcns = order_p_1 * order_p_1 * order_p_1 + 3 * order_p_1 * order_p_1 + 2 * order_p_1;
        num_basis_fcns /= 6;
        break;
    default:
        TBOX_ERROR("only supports dim = 1, 2, or 3\n");
    }
    return num_basis_fcns;
} // num_polynomial_basis_fcns

void
evaluate_polynomial_basis_fcns(Eigen::VectorXd& P,
                               const libMesh::Point& x_center,
                               const libMesh::Point& x_eval,
                               const unsigned int dim,
                               const unsigned int order)
{
    TBOX_ASSERT(static_cast<unsigned int>(P.size()) == num_polynomial_basis_fcns(dim, order));

    // Compute powers of the components of x up to the specified order.
    libMesh::Point x = x_center - x_eval;
    boost::multi_array<double, 2> x_pow(boost::extents[dim][order + 1]);
    for (unsigned int d = 0; d < dim; ++d)
    {
        x_pow[d][0] = 1.0;
        for (unsigned int k = 1; k <= order; ++k)
        {
            x_pow[d][k] = x(d) * x_pow[d][k - 1];
        }
    }

    // Evaluate the complete polynomial basis functions of the specified order.
    static const unsigned int X_IDX = 0;
    static const unsigned int Y_IDX = 1;
    static const unsigned int Z_IDX = 2;
    switch (dim)
    {
    case 1:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow, ++k)
        {
            unsigned int x_exp = total_pow;
            P(k) = x_pow[X_IDX][x_exp];
        }
        break;
    case 2:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow)
        {
            for (unsigned int x_exp = 0; x_exp <= total_pow; ++x_exp, ++k)
            {
                unsigned int y_exp = total_pow - x_exp;
                P(k) = x_pow[X_IDX][x_exp] * x_pow[Y_IDX][y_exp];
            }
        }
        break;
    case 3:
        for (unsigned int total_pow = 0, k = 0; total_pow <= order; ++total_pow)
        {
            for (unsigned int x_exp = 0; x_exp <= total_pow; ++x_exp)
            {
                for (unsigned int y_exp = 0; y_exp <= total_pow - x_exp; ++y_exp, ++k)
                {
                    unsigned int z_exp = total_pow - (x_exp + y_exp);
                    P(k) = x_pow[X_IDX][x_exp] * x_pow[Y_IDX][y_exp] * x_pow[Z_IDX][z_exp];
                }
            }
        }
        break;
    default:
        TBOX_ERROR("only supports dim = 1, 2, or 3\n");
    }
    return;
} // evaluate_polynomial_basis_fcns
} // namespace

/////////////////////////////// PUBLIC ///////////////////////////////////////

IBFEPatchRecoveryPostProcessor::IBFEPatchRecoveryPostProcessor(MeshBase* mesh, FEDataManager* fe_data_manager)
    : d_mesh(mesh),
      d_fe_data_manager(fe_data_manager),
      d_periodic_boundaries(nullptr),
      d_interp_order(INVALID_ORDER),
      d_quad_order(INVALID_ORDER)
{
    // Active local elements.
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();

    // Determine the number of quadrature/interpolation points in each element.
    //
    // We use full-order Gaussian quadrature rules (i.e. third-order Gauss
    // quadrature for first-order elements and fifth-order Gauss quadrature for
    // second-order elements) in all elements to avoid special treatment at
    // boundary nodes.
    bool first_order_elems = false;
    bool second_order_elems = false;
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const unsigned int dim = elem->dim();
        TBOX_ASSERT(dim == d_mesh->mesh_dimension());
        Order elem_order = elem->default_order();
        if (elem_order == FIRST) first_order_elems = true;
        if (elem_order == SECOND) second_order_elems = true;
        TBOX_ASSERT(elem_order == FIRST || elem_order == SECOND);
    }
    if (first_order_elems && second_order_elems)
    {
        TBOX_ERROR(
            "cannot have both first- and second-order elements in the same "
            "mesh.\n");
    }
    d_interp_order = first_order_elems ? FIRST : SECOND;
    d_quad_order = first_order_elems ? THIRD : FIFTH;
    return;
} // IBFEPatchRecoveryPostProcessor

IBFEPatchRecoveryPostProcessor::~IBFEPatchRecoveryPostProcessor()
{
    // intentionally blank
    return;
} // ~IBFEPatchRecoveryPostProcessor

void
IBFEPatchRecoveryPostProcessor::initializeFEData(const PeriodicBoundaries* const periodic_boundaries)
{
    d_periodic_boundaries = periodic_boundaries;

    const Parallel::Communicator& comm = d_mesh->comm();
    const int mpi_rank = comm.rank();
    const int mpi_size = comm.size();

    // Active local elements.
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();

    // Determine the element patches associated with each node N, which is
    // defined to be the collection of elements that contain node N.
    //
    // Unlike the standard Z-Z patch recovery algorithm, we use "tight" element
    // patches for non-vertex nodes.
    std::unique_ptr<PointLocatorBase> point_locator = PointLocatorBase::build(TREE, *d_mesh);
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        for (unsigned int n = 0; n < elem->n_nodes(); ++n)
        {
            // Only set up patches for local nodes.
            const Node* const node = elem->get_node_ptr(n);
            if (node->processor_id() != mpi_rank) continue;

            // Only set up patches once for each node.
            const dof_id_type node_id = node->id();
            if (d_local_elem_patches.find(node_id) != d_local_elem_patches.end()) continue;

            // Find the elements that touch this node.
            ElemPatch& elem_patch = d_local_elem_patches[node_id];
            std::set<const Elem*> elems;
            elem->find_point_neighbors(*node, elems);
            for (std::set<const Elem*>::const_iterator it = elems.begin(); it != elems.end(); ++it)
            {
                elem_patch.insert(boost::make_tuple(*it, CompositePeriodicMapping(), CompositePeriodicMapping()));
            }

            // Account for periodic boundaries.
            bool done = false || !d_periodic_boundaries;
            while (!done)
            {
                ElemPatch periodic_neighbors;
                for (ElemPatch::const_iterator it = elem_patch.begin(); it != elem_patch.end(); ++it)
                {
                    const Elem* const elem = it->get<0>();
                    const CompositePeriodicMapping& forward_mapping = it->get<1>();
                    const CompositePeriodicMapping& inverse_mapping = it->get<2>();
                    const libMesh::Point p = apply_composite_periodic_mapping(forward_mapping, *node);
                    TBOX_ASSERT(elem->contains_point(p));
                    for (unsigned int i = 0; i < elem->n_neighbors(); ++i)
                    {
                        if (!elem->neighbor_ptr(i))
                        {
                            const std::vector<boundary_id_type>& boundary_ids =
                                d_mesh->boundary_info->boundary_ids(elem, i);
                            for (std::vector<boundary_id_type>::const_iterator j = boundary_ids.begin();
                                 j != boundary_ids.end();
                                 ++j)
                            {
                                const boundary_id_type boundary_id = *j;
                                const PeriodicBoundaryBase* const periodic_boundary =
                                    d_periodic_boundaries->boundary(boundary_id);
                                if (periodic_boundary)
                                {
                                    const libMesh::Point periodic_image = periodic_boundary->get_corresponding_pos(p);
                                    const Elem* neighbor =
                                        d_periodic_boundaries->neighbor_ptr(boundary_id, *point_locator, elem, i);
                                    if (elem->level() < neighbor->level())
                                    {
                                        neighbor = neighbor->parent();
                                    }
                                    if (neighbor->contains_point(periodic_image))
                                    {
                                        std::set<const Elem*> elems;
                                        neighbor->find_point_neighbors(periodic_image, elems);
                                        for (std::set<const Elem*>::const_iterator k = elems.begin(); k != elems.end();
                                             ++k)
                                        {
                                            const Elem* const elem = *k;
                                            if (elem_patch.find(elem) == elem_patch.end() &&
                                                periodic_neighbors.find(elem) == periodic_neighbors.end())
                                            {
                                                CompositePeriodicMapping forward = forward_mapping;
                                                CompositePeriodicMapping inverse = inverse_mapping;
                                                forward.insert(
                                                    forward.end(),
                                                    periodic_boundary->clone(PeriodicBoundaryBase::FORWARD).release());
                                                inverse.insert(
                                                    inverse.begin(),
                                                    periodic_boundary->clone(PeriodicBoundaryBase::INVERSE).release());
                                                periodic_neighbors.insert(boost::make_tuple(elem, forward, inverse));
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
                elem_patch.insert(periodic_neighbors.begin(), periodic_neighbors.end());
                periodic_neighbors.clear();
                done = periodic_neighbors.empty();
            }
        }
    }

    // Setup mappings used to fill global indexing data structures.
    //
    // We use full-order Gaussian quadrature rules (i.e. third-order Gauss
    // quadrature for first-order elements and fifth-order Gauss quadrature for
    // second-order elements) in all elements to avoid special treatment at
    // boundary nodes.
    d_n_qp_global = 0;
    d_n_qp_local = 0;
    d_qp_global_offset = 0;
    const unsigned int n_elem = d_mesh->n_elem();
    d_elem_n_qp.resize(n_elem, 0);
    d_elem_qp_global_offset.resize(n_elem, 0);
    d_elem_qp_local_offset.resize(n_elem, 0);
    std::unique_ptr<QBase> qrule;
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const unsigned int dim = elem->dim();
        bool reinit_qrule = false;
        if (!qrule.get() || qrule->get_dim() != dim || qrule->get_order() != d_quad_order)
        {
            qrule = QBase::build(QGAUSS, dim, d_quad_order);
            reinit_qrule = true;
        }
        else if (qrule->get_elem_type() != elem->type() || qrule->get_p_level() != elem->p_level())
        {
            reinit_qrule = true;
        }
        if (reinit_qrule) qrule->init(elem->type(), elem->p_level());
        unsigned int n_qp = qrule->n_points();
        const dof_id_type elem_id = elem->id();
        d_elem_n_qp[elem_id] = n_qp;
        d_elem_qp_local_offset[elem_id] = d_n_qp_local;
        d_n_qp_local += n_qp;
        d_elem_sigma[elem->id()].resize(n_qp);
        d_elem_pressure[elem->id()].resize(n_qp);
    }
    std::vector<int> n_qp_per_proc(mpi_size);
    n_qp_per_proc[mpi_rank] = d_n_qp_local;
    comm.sum(n_qp_per_proc);
    d_qp_global_offset = std::accumulate(n_qp_per_proc.begin(), n_qp_per_proc.begin() + mpi_rank, 0);
    d_n_qp_global = std::accumulate(n_qp_per_proc.begin() + mpi_rank, n_qp_per_proc.end(), d_qp_global_offset);
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        d_elem_qp_global_offset[elem_id] = d_elem_qp_local_offset[elem_id] + d_qp_global_offset;
    }
    comm.sum(d_elem_qp_global_offset);
    comm.sum(d_elem_qp_local_offset);

    // Set up element patch L2 projection matrices.
    unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::MatrixXd M(num_basis_fcns, num_basis_fcns);
    Eigen::VectorXd P(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    d_local_patch_proj_solver.resize(d_local_elem_patches.size());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::iterator it = d_local_elem_patches.begin(); it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        ElemPatch& elem_patch = it->second;
        M.setZero();
        for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
        {
            const Elem* const elem = el_it->get<0>();
            const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
            fe->reinit(elem);
            for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
            {
                evaluate_polynomial_basis_fcns(
                    P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                M += P * P.transpose();
            }
        }
        d_local_patch_proj_solver[k] = M.colPivHouseholderQr();
        if (!d_local_patch_proj_solver[k].isInvertible())
        {
            TBOX_ERROR(
                "IBFEPatchRecoveryPostProcessor could not construct L2 "
                "reconstruction for element patch associated with node "
                << node_id << "\n");
        }
    }
    return;
} // initializeFEData

System*
IBFEPatchRecoveryPostProcessor::initializeCauchyStressSystem()
{
    EquationSystems* equation_systems = d_fe_data_manager->getEquationSystems();
    System* sigma_system = &equation_systems->add_system<System>("CAUCHY_STRESS_RECOVERY_SYSTEM");
    for (unsigned int i = 0; i < NDIM; ++i)
    {
        for (unsigned int j = i; j < NDIM; ++j)
        {
            std::string var_name = "sigma_";
            var_name += (i == 0 ? 'x') : i == 1 ? 'y' : 'z');
            var_name += (j == 0 ? 'x') : j == 1 ? 'y' : 'z');

            sigma_system->add_variable(var_name), d_interp_order, LAGRANGE);
        }
    }
    return sigma_system;
} // initializeCauchyStressSystem

System*
IBFEPatchRecoveryPostProcessor::initializePressureSystem()
{
    EquationSystems* equation_systems = d_fe_data_manager->getEquationSystems();
    System* p_system = &equation_systems->add_system<System>("PRESSURE_RECOVERY_SYSTEM");
    p_system->add_variable("p", d_interp_order, LAGRANGE);
    return p_system;
} // initializePressureSystem

void
IBFEPatchRecoveryPostProcessor::registerCauchyStressValue(const Elem* const elem,
                                                          const QBase* const qrule,
                                                          const unsigned int qp,
                                                          const TensorValue<double>& sigma)
{
    const Parallel::Communicator& comm = d_mesh->comm();
    if (elem->processor_id() != comm.rank() || !elem->active())
    {
        TBOX_ERROR("must register stresses only for active local elements\n");
    }
    TBOX_ASSERT(elem->default_order() == d_interp_order);
    TBOX_ASSERT(qrule->type() == QGAUSS);
    TBOX_ASSERT(qrule->get_order() == d_quad_order);
    TBOX_ASSERT(qrule->get_elem_type() == elem->type());
    TBOX_ASSERT(qrule->get_p_level() == elem->p_level());
    TBOX_ASSERT(qp < d_elem_n_qp[elem->id()]);
    d_elem_sigma[elem->id()][qp] = sigma;
    return;
} // registerCauchyStressValue

void
IBFEPatchRecoveryPostProcessor::registerPressureValue(const Elem* const elem,
                                                      const QBase* const qrule,
                                                      const unsigned int qp,
                                                      const double p)
{
    const Parallel::Communicator& comm = d_mesh->comm();
    if (elem->processor_id() != comm.rank() || !elem->active())
    {
        TBOX_ERROR("must register pressures only for active local elements\n");
    }
    TBOX_ASSERT(elem->default_order() == d_interp_order);
    TBOX_ASSERT(qrule->type() == QGAUSS);
    TBOX_ASSERT(qrule->get_order() == d_quad_order);
    TBOX_ASSERT(qrule->get_elem_type() == elem->type());
    TBOX_ASSERT(qrule->get_p_level() == elem->p_level());
    TBOX_ASSERT(qp < d_elem_n_qp[elem->id()]);
    d_elem_pressure[elem->id()][qp] = p;
    return;
} // registerPressureValue

void
IBFEPatchRecoveryPostProcessor::reconstructCauchyStress(System& sigma_system)
{
    const unsigned int sigma_sys_num = sigma_system.number();
    NumericVector<double>& sigma_vec = *sigma_system.solution;

    // Communicate the stored values of the Cauchy stress.
    static const unsigned int NVARS = (NDIM * (NDIM + 1)) / 2;
    std::vector<double> sigma_vals(NVARS * d_n_qp_global, 0.0);
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        const int global_offset = d_elem_qp_global_offset[elem_id];
        for (unsigned int qp = 0; qp < d_elem_n_qp[elem_id]; ++qp)
        {
            const TensorValue<double>& stress = d_elem_sigma[elem_id][qp];
            for (unsigned int i = 0, k = 0; i < NDIM; ++i)
            {
                for (unsigned int j = i; j < NDIM; ++j, ++k)
                {
                    sigma_vals[NVARS * (global_offset + qp) + k] = stress(i, j);
                }
            }
        }
    }
    const Parallel::Communicator& comm = d_mesh->comm();
    comm.sum(sigma_vals);

    // Perform element patch L2 projections.
    const unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::VectorXd P(num_basis_fcns), a(num_basis_fcns), f(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::const_iterator it = d_local_elem_patches.begin();
         it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        const ElemPatch& elem_patch = it->second;
        Eigen::ColPivHouseholderQR<Eigen::MatrixXd>& patch_proj_solver = d_local_patch_proj_solver[k];
        for (unsigned int var = 0; var < NVARS; ++var)
        {
            // Solve for the coefficients of the reconstruction.
            f.setZero();
            for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
            {
                const Elem* const elem = el_it->get<0>();
                const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
                const dof_id_type elem_id = elem->id();
                const int global_offset = d_elem_qp_global_offset[elem_id];
                fe->reinit(elem);
                for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
                {
                    evaluate_polynomial_basis_fcns(
                        P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                    f += P * sigma_vals[NVARS * (global_offset + qp) + var];
                }
            }
            a = patch_proj_solver.solve(f);

            // Evaluate the reconstruction at the node.
            const int dof_index = node.dof_number(sigma_sys_num, var, 0);
            sigma_vec.set(dof_index, a(0));
        }
    }
    return;
} // reconstructCauchyStress

void
IBFEPatchRecoveryPostProcessor::reconstructPressure(System& p_system)
{
    const unsigned int p_sys_num = p_system.number();
    NumericVector<double>& p_vec = *p_system.solution;

    // Communicate the stored values of the Cauchy stress.
    std::vector<double> pressure_vals(d_n_qp_global, 0.0);
    const MeshBase::const_element_iterator el_begin = d_mesh->active_local_elements_begin();
    const MeshBase::const_element_iterator el_end = d_mesh->active_local_elements_end();
    for (MeshBase::const_element_iterator el_it = el_begin; el_it != el_end; ++el_it)
    {
        const Elem* const elem = *el_it;
        const dof_id_type elem_id = elem->id();
        const int global_offset = d_elem_qp_global_offset[elem_id];
        for (unsigned int qp = 0; qp < d_elem_n_qp[elem_id]; ++qp)
        {
            pressure_vals[global_offset + qp] = d_elem_pressure[elem_id][qp];
        }
    }
    const Parallel::Communicator& comm = d_mesh->comm();
    comm.sum(pressure_vals);

    // Perform element patch L2 projections.
    const unsigned int dim = d_mesh->mesh_dimension();
    const unsigned int num_basis_fcns = num_polynomial_basis_fcns(dim, d_interp_order);
    Eigen::VectorXd P(num_basis_fcns), a(num_basis_fcns), f(num_basis_fcns);
    std::unique_ptr<FEBase> fe(FEBase::build(dim, FEType(d_interp_order, LAGRANGE)));
    const std::vector<libMesh::Point>& q_point = fe->get_xyz();
    std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, dim, d_quad_order);
    fe->attach_quadrature_rule(qrule.get());
    unsigned int k = 0;
    for (std::map<dof_id_type, ElemPatch>::const_iterator it = d_local_elem_patches.begin();
         it != d_local_elem_patches.end();
         ++it, ++k)
    {
        const dof_id_type node_id = it->first;
        const Node& node = d_mesh->node(node_id);
        const ElemPatch& elem_patch = it->second;
        Eigen::ColPivHouseholderQR<Eigen::MatrixXd>& patch_proj_solver = d_local_patch_proj_solver[k];

        // Solve for the coefficients of the reconstruction.
        f.setZero();
        for (ElemPatch::const_iterator el_it = elem_patch.begin(); el_it != elem_patch.end(); ++el_it)
        {
            const Elem* const elem = el_it->get<0>();
            const CompositePeriodicMapping& inverse_mapping = el_it->get<2>();
            const dof_id_type elem_id = elem->id();
            const int global_offset = d_elem_qp_global_offset[elem_id];
            fe->reinit(elem);
            for (unsigned int qp = 0; qp < qrule->n_points(); ++qp)
            {
                evaluate_polynomial_basis_fcns(
                    P, node, apply_composite_periodic_mapping(inverse_mapping, q_point[qp]), dim, d_interp_order);
                f += P * pressure_vals[global_offset + qp];
            }
        }
        a = patch_proj_solver.solve(f);

        // Evaluate the reconstruction at the node.
        const unsigned int var = 0;
        const int dof_index = node.dof_number(p_sys_num, var, 0);
        p_vec.set(dof_index, a(0));
    }
    return;
} // reconstructPressure

/////////////////////////////// PROTECTED ////////////////////////////////////

/////////////////////////////// PRIVATE //////////////////////////////////////

/////////////////////////////// NAMESPACE ////////////////////////////////////

} // namespace IBAMR

//////////////////////////////////////////////////////////////////////////////


/*
 *
 * Copyright (c) Kresimir Fresl & Toon Knapen 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SPSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SPSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B with A symmetric matrix
    // stored in packed format
    //
    /////////////////////////////////////////////////////////////////////

    /*
     * spsv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric matrix stored in packed
     * format and X and B are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^T,  if UPLO = 'U',
     *   A = L * D * L^T,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is symmetric and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks.
     * The factored form of A is then used to solve the system
     * of equations A * X = B.
     */

    namespace detail {

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 float* ap, int* ipiv,
                 float* b, int const ldb, int* info)
      {
        LAPACK_SSPSV (&uplo, &n, &nrhs, ap, ipiv, b, &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 double* ap, int* ipiv,
                 double* b, int const ldb, int* info)
      {
        LAPACK_DSPSV (&uplo, &n, &nrhs, ap, ipiv, b, &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 traits::complex_f* ap, int* ipiv,
                 traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CSPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void spsv (char const uplo, int const n, int const nrhs,
                 traits::complex_d* ap, int* ipiv,
                 traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZSPSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (ap), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmA, typename MatrB, typename IVec>
      inline
      int spsv (SymmA& a, IVec& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<SymmA>::matrix_structure,
          traits::symmetric_packed_t
        >::value));
        BOOST_STATIC_ASSERT((boost::is_same<
          typename traits::matrix_traits<MatrB>::matrix_structure,
          traits::general_t
        >::value));
#endif

        int const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));

        char uplo = traits::matrix_uplo_tag (a);
        int info;
        spsv (uplo, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int spsv (SymmA& a, IVec& i, MatrB& b) {
      assert (traits::matrix_size1 (a) == traits::vector_size (i));
      return detail::spsv (a, i, b);
    }

    template <typename SymmA, typename MatrB>
    inline
    int spsv (SymmA& a, MatrB& b) {
      // with 'internal' pivot vector

      int info = -101;
      traits::detail::array<int> i (traits::matrix_size1 (a));

      if (i.valid())
        info = detail::spsv (a, i, b);
      return info;
    }

    /*
     * sptrf() computes the factorization of a symmetric matrix A
     * in packed storage using the  Bunch-Kaufman diagonal pivoting
     * method. The form of the factorization is
     *    A = U * D * U^T  or  A = L * D * L^T
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is symmetric and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail {

      inline
      void sptrf (char const uplo, int const n,
                  float* ap, int* ipiv, int* info)
      {
        LAPACK_SSPTRF (&uplo, &n, ap, ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  double* ap, int* ipiv, int* info)
      {
        LAPACK_DSPTRF (&uplo, &n, ap, ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  traits::complex_f* ap, int* ipiv, int* info)
      {
        LAPACK_CSPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

      inline
      void sptrf (char const uplo, int const n,
                  traits::complex_d* ap, int* ipiv, int* info)
      {
        LAPACK_ZSPTRF (&uplo, &n, traits::complex_ptr (ap), ipiv, info);
      }

    }

    template <typename SymmA, typename IVec>
    inline
    int sptrf (SymmA& a, IVec& i) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::sptrf (uplo, n, traits::matrix_storage (a),
                     traits::vector_storage (i), &info);
      return info;
    }

    /*
     * sptrs() solves a system of linear equations A*X = B with
     * a symmetric matrix A in packed storage using the factorization
     *    A = U * D * U^T   or  A = L * D * L^T
     * computed by sptrf().
     */

    namespace detail {

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  float const* a, int const* ipiv,
                  float* b, int const ldb, int* info)
      {
        LAPACK_SSPTRS (&uplo, &n, &nrhs, a, ipiv, b, &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  double const* a, int const* ipiv,
                  double* b, int const ldb, int* info)
      {
        LAPACK_DSPTRS (&uplo, &n, &nrhs, a, ipiv, b, &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_f const* a, int const* ipiv,
                  traits::complex_f* b, int const ldb, int* info)
      {
        LAPACK_CSPTRS (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void sptrs (char const uplo, int const n, int const nrhs,
                  traits::complex_d const* a, int const* ipiv,
                  traits::complex_d* b, int const ldb, int* info)
      {
        LAPACK_ZSPTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sptrs (SymmA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::matrix_size1 (b));
      assert (n == traits::vector_size (i));

      char uplo = traits::matrix_uplo_tag (a);
      int info;
      detail::sptrs (uplo, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
                     traits::matrix_storage (a),
                     traits::vector_storage (i),
#else
                     traits::matrix_storage_const (a),
                     traits::vector_storage_const (i),
#endif
                     traits::matrix_storage (b),
                     traits::leading_dimension (b),
                     &info);
      return info;
    }

    namespace detail {
      inline
      void sptri (char const uplo, int const n,
          float* ap, int* ipiv, float* work, int* info)
      {
        LAPACK_SSPTRI (&uplo, &n, ap, ipiv, work, info);
      }

      inline
      void sptri (char const uplo, int const n,
          double* ap, int* ipiv, double* work, int* info)
      {
        LAPACK_DSPTRI (&uplo, &n, ap, ipiv, work, info);
      }

      inline
      void sptri (char const uplo, int const n,
          traits::complex_f* ap, int* ipiv, traits::complex_f* work, int* info)
      {
        LAPACK_CSPTRI (&uplo, &n, traits::complex_ptr (ap),
            ipiv, traits::complex_ptr (work), info);
      }

      inline
      void sptri (char const uplo, int const n,
          traits::complex_d* ap, int* ipiv, traits::complex_d* work, int* info)
      {
        LAPACK_ZSPTRI (&uplo, &n, traits::complex_ptr (ap),
            ipiv, traits::complex_ptr (work), info);
      }
    } // namespace detail

    template <typename SymmA, typename IVec>
    inline
    int sptri (SymmA& a, IVec& ipiv)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_packed_t
      >::value));
#endif

      int const n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));
      assert (n == traits::vector_size (ipiv));

      char uplo = traits::matrix_uplo_tag (a);
      int info;

      typedef typename SymmA::value_type value_type;
      traits::detail::array<value_type> work(traits::matrix_size1(a));

      detail::sptri (uplo, n, traits::matrix_storage (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
          traits::vector_storage (ipiv),
#else
          traits::vector_storage_const (ipiv),
#endif
          traits::vector_storage (work),
          &info);
      return info;
    }

  } // namespace lapack

}}} // namespace boost::numeric::bindings

#endif


#include <boost/python.hpp>
#include <scitbx/array_family/tiny_types.h>
#include <scitbx/array_family/small.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/ref.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <scitbx/vec3.h>
#include <scitbx/mat3.h>
#include <cctype>
#include <complex>
#include <cmath>

namespace fractbx {
  namespace ext {

    // discover mandelbrot set

    static size_t iterations(std::complex<double> c)
    {
      std::complex<double> z = 0;
      size_t jmax = 2048;
      for (size_t j = 0; j < jmax; j++) {
        z = z * z + c;
        if (std::abs(z) > 2) {
          return j;
        }
      }
      return jmax;
    }

    static scitbx::af::versa< int, scitbx::af::c_grid<2> > make_flex(size_t n)
    {
      scitbx::af::c_grid<2> grid(n, n);
      scitbx::af::versa< int, scitbx::af::c_grid<2> > result(grid, 0);
      double scale = 4.0 / (n - 1);
      int off = (n - 1) / 2;
      for(int j = 0; j < n; j++) {
        for(int i = 0; i < n; i++) {
          std::complex<double> c(scale * (i - off), scale * (j - off));
          result(j, i) = iterations(c);
        }
      }
      return result;
    }

    void init_module()
    {
      using namespace boost::python;
      def("make_flex", make_flex, (arg("size")));
    }

  }
} // namespace fractbx::ext

BOOST_PYTHON_MODULE(fractbx_ext)
{
  fractbx::ext::init_module();
}


#pragma once

#include <armadillo>

class Pipeline;

/*!
 * \brief Discretizer is an abstract class, the base class the implementation of
 * the discretization of the two different sets of governing equations; internal
 * energy and enthalpy.
 *
 * The governing equations are discretized using a implicit backward difference
 * method, as detailed in
 *  - Gas composition tracking in transient pipeline flow, Chaczykowski et. al., Journal of Natural Gas Science and Engineering 55 (2018)
 *
 * This finite difference scheme discretizes the governing properties flow,
 * pressure and temperature onto a grid. The derivatives and values for each
 * grid section \f$I\f$ are approximated by different combination of the flow,
 * pressure and temperatures at the grid points, \f$y_i\f$ and \f$y_{i+1}\f$,
 * as follows
 *
 * \f[
 *     \frac{\partial y(I, t_{n+1})}{\partial t} = \frac{y_{i+1}^{n+1} + y_{i}^{n+1} - y_{i+1}^{n} - y_{i}^{n}}{2\Delta t}
 * \f]
 *
 * \f[
 *     \frac{\partial y(I, t_{n+1})}{\partial x} = \frac{y_{i+1}^{n+1} - y_{i}^{n+1}}{\Delta x}
 * \f]
 *
 * \f[
 *     y(I, t_{n+1}) = \frac{y_{i+1}^{n+1} + y_{i}^{n+1}}{2}
 * \f]
 *
 * This sets up a matrix equation \f$Ax = b\f$, where the matrix \f$A\f$
 * contains the coeffcients of each term \f$y_i\f$, the vector \f$x\f$ contain
 * the unknowns \f$\dot m_i\f$ (flow), \f$p_i\f$ (pressure) and \f$T_i\f$
 * (temperature), and the vector \f$b\f$ contain the constant/known terms
 * (from boundary conditions and other knowns).
 *
 * \see MatrixEquation
 */
class Discretizer
{
public:
    //! Have to declare virtual destructor to avoid compiler warnings.
    //! Only declared here, to avoid the inline compiler-generated default destructor.
    virtual ~Discretizer();

    /*!
     * \brief Construct from number of grid points and number of equations and
     * variables.
     *
     * Allocates the matrices Discretizer::m_term_i, Discretizer::m_term_ipp,
     * and the vector Discretizer::m_boundaryTerm.
     *
     * \param nGridPoints Number of grid points
     * \param nEquationsAndVariables Number of equations and variables
     */
    Discretizer(
            const arma::uword nGridPoints,
            const arma::uword nEquationsAndVariables);

    /*!
     * \brief Pure virtual function, stencil for subclasses. This method
     * calculates in the coefficients of \f$y_i\f$ and \f$y_{i+1}\f$ and stores
     * them in Discretizer::m_term_i and Discretizer::m_term_ipp.
     * \param dt Time step
     * \param currentState Current pipeline state
     * \param newState New/guess pipeline state
     */
    virtual void discretize(
            const arma::uword dt,
            const Pipeline& currentState,
            const Pipeline& newState) = 0;

    const arma::cube& term_i() const { return m_term_i; } //!< Get coefficients of \f$y_i\f$
    const arma::cube& term_ipp() const { return m_term_ipp; } //!< Get coefficients of \f$y_{i+1}\f$
    const arma::mat& boundaryTerms() const { return m_boundaryTerm; } //!< Get constant terms

protected:
    // TODO: better description of how m_term_i and m_term_ipp are organized
    /*!
     * \brief The coefficients of \f$y_i\f$ in the discretized governing equations.
     *
     * This is an `arma::cube`, which is organized as follows.
     * `m_term_i(grid point, equation number, variable number)`,
     * where grid points are the usual grid points, equations are the
     * continuity (0), momentum (1) and energy equations (2), and the variables
     * are in order flow (0), pressure (1) and temperature(1).
     * So for example if you want the coefficient at grid point number 5, in the
     * energy equation, for flow, you want
     * `m_term_i(4, 2, 0)` (zero-indexed).
     */
    arma::cube m_term_i;

    /*!
     * \brief The coefficients of \f$y_i\f$ in the discretized governing equations.
     *
     * See Discretizer::m_term_i for a description of the organization of this.
     */
    arma::cube m_term_ipp;

    /*!
     * \brief The constant/known terms in the discretized governing equations.
     *
     * This is a matrix, organized as
     * `m_boundaryTerm(grid point, equation number)`.
     */
    arma::mat m_boundaryTerm;

    double m_gravity = 9.81; //!< Gravity
};