#include <mmtbx/masks/atom_mask.h>
#include <mmtbx/masks/util.h>
#include <mmtbx/masks/grid_symop.h>
#include <cctbx/maptbx/structure_factors.h>
#include <cctbx/maptbx/gridding.h>
#include <cctbx/sgtbx/direct_space_asu/proto/small_vec_math.h>
#include <cctbx/sgtbx/direct_space_asu/proto/asymmetric_unit.h>
#include <scitbx/fftpack/real_to_complex_3d.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/tiny_types.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/random.hpp>
#include <numeric>
#include <sstream>

#if defined(_MSC_VER)
#undef max
#undef min
#endif

namespace mmtbx { namespace masks {

  using namespace cctbx::sgtbx::asu;
  typedef double f_t;

  namespace {

    inline int ifloor(f_t x)
    {
      return scitbx::math::float_int_conversions<f_t, int>::ifloor(x);
    }

    inline int iceil(f_t x)
    {
      return scitbx::math::float_int_conversions<f_t, int>::iceil(x);
    }

    inline scitbx::vec3<int> iceil(const scitbx::vec3<double> &v)
    {
      return scitbx::vec3<int> (
          scitbx::math::float_int_conversions<double, int>::iceil(v[0]),
          scitbx::math::float_int_conversions<double, int>::iceil(v[1]),
          scitbx::math::float_int_conversions<double, int>::iceil(v[2])
          );
    }

    inline scitbx::vec3<int> ifloor(const scitbx::vec3<double> &v)
    {
      return scitbx::vec3<int> (
          scitbx::math::float_int_conversions<double, int>::ifloor(v[0]),
          scitbx::math::float_int_conversions<double, int>::ifloor(v[1]),
          scitbx::math::float_int_conversions<double, int>::ifloor(v[2])
          );
    }

  }

  inline void translate_into_cell(scitbx::int3 &num, const scitbx::int3 &den)
  {
    for(unsigned char i=0; i<3; ++i)
    {
      int tn = num[i];
      const int td = den[i];
      tn %= td;
      if( tn < 0 )
        tn += td;
      num[i] = tn;
    }
  }

  // seems to be of the same speed as the above
  inline void translate_into_cell_2(scitbx::int3 &num, const scitbx::int3 &den)
  {
    for(unsigned char i=0; i<3; ++i)
    {
      int tn = num[i];
      const int td = den[i];
      while( tn<0 )
        tn += td;
      while( tn >= td )
        tn -= td;
      num[i] = tn;
    }
  }

  unsigned short site_symmetry_order(
    const std::vector<cctbx::sgtbx::grid_symop> &symops,
    const scitbx::int3 &num, const scitbx::int3 &den )
  {
    unsigned short nops = 0;
    // num must be  inside cell
    for(size_t i=0; i<symops.size(); ++i)
    {
      scitbx::int3 sv = symops[i].apply_to( num );
      translate_into_cell(sv, den);
      if( scitbx::eq_all(sv , num) )
        ++nops;
    }
    MMTBX_ASSERT( nops>0U );
    return nops;
  }

  void atom_mask::mask_asu()
  {
    unsigned short order = group.order_z();
    MMTBX_ASSERT( order>0 );
    const scitbx::int3 n = this->grid_size();
    MMTBX_ASSERT( n[0]>0 && n[1]>0 && n[2] >0 );
    // determine expanded asu limits, due to symops arithmetics
    const int max_int = std::numeric_limits<int>::max();
    scitbx::af::int3 max_grid(max_int, max_int, max_int);
    // prepare grid adapted integer symmetry operators
    std::vector<cctbx::sgtbx::grid_symop> symops;
    for(size_t i=0; i<order; ++i)
    {
      cctbx::sgtbx::grid_symop grsym( group(i), n );
      symops.push_back(grsym);
      scitbx::af::int3 grmx;
      grsym.get_grid_limits(grmx);
      for(unsigned char j=0; j<3U; ++j)
        max_grid[j] = std::min(max_grid[j], grmx[j]);
    }
    MMTBX_ASSERT( symops.size() == order );
    scitbx::int3 imn, imx;
    this->get_asu_boundaries(imn, imx); // [imn, imx)

    scitbx::int3 emn, emx; //  enclosed box boundaries [emn, emx]
    const bool has_enclosed_box = asu.enclosed_box_corners(emn, emx, n);
    this->debug_has_enclosed_box = has_enclosed_box;
    if( has_enclosed_box )
    {
      MMTBX_ASSERT( scitbx::gt_all( emn, imn ) );
      MMTBX_ASSERT( scitbx::lt_all( emx, imx ) );
    }

    asymmetric_unit<direct,optimized> opt_asu(this->asu, n);
    scitbx::int3 smn, smx;
    this->get_expanded_asu_boundaries(smn,smx); // [smn, smx)
    // determine expanded asu limits, due to is_inside arithmetics
    scitbx::af::long3 grmx = opt_asu.get_optimized_grid_limits();
    for(unsigned char j=0; j<3U; ++j)
    {
      if( grmx[j] < max_grid[j] )
        max_grid[j] = grmx[j];
      if( std::abs(smn[j])>max_grid[j] || std::abs(smx[j])>max_grid[j] )
      {
        std::ostringstream str;
        str << "Mask's expanded asymmetric cell dimensions: [" << smn << ", "
          << smx << ") are too large. 64 bit OS and/or software may be needed.";
        throw error(str.str());
      }
    }
    const af::ref<data_type, grid_t > data_ref = data.ref();
    data_type *d_ptr = data_ref.begin();
    size_t cell_volume = 0;
    for(long i=smn[0]; i<smx[0]; ++i)
    {
      for(long j=smn[1]; j<smx[1]; ++j)
      {
        for(long k=smn[2]; k<smx[2]; ++k, ++d_ptr)
        {
          // set points wthin shrink_truncation_radius around asu to
          // an arbitrary unique positive value
          data_type &dr = *d_ptr;
          dr.set_outside();
          const scitbx::int3 pos(i,j,k);
          if( !(scitbx::ge_all(pos, imn) && scitbx::lt_all(pos, imx)) )
            continue;
          unsigned short nops = 0;
          if( has_enclosed_box )
          {
            if( scitbx::ge_all(pos, emn) && scitbx::le_all(pos, emx) )
              nops = order;
          }
          if( nops==0 )
          {
            const short ww = opt_asu.where_is(pos);
            if( ww == 1 ) // inside NOT on the face
              nops = order;
            else if( ww==-1 ) // inside on the face
            {
              scitbx::int3 pos_in_cell(pos);
              translate_into_cell(pos_in_cell, n);
              nops = site_symmetry_order(symops, pos_in_cell, n);
              MMTBX_ASSERT( nops>0 );
              MMTBX_ASSERT( order%nops == 0);
              nops = order / nops;
            }
          }
          if( nops!=0 )
          {
            MMTBX_ASSERT( dr.multiplicity()==0 || dr.multiplicity() == nops
                  || dr.is_outside() );
            MMTBX_ASSERT( dr.layer()==0 || dr.layer() == n_layers+1
                  || dr.is_outside());
            dr.set(n_layers+1, nops);
            cell_volume += static_cast<size_t>(nops);
          }
        } // z-loop
      } // y-loop
    } // x-loop

    MMTBX_ASSERT( cell_volume > 0 );
    if( cell_volume != this->grid_size_1d() )
    {
      // volume(asu)*group_order != volume(cell)
      std::ostringstream str;
      str << "volume(asymmetric unit)*group_order != volume(unit cell).\n"
          << "Maybe because the mask grid size: " << this->grid_size()
          << " is incompatible with\n"
             "the space group symmetry";
      throw error( str.str() );
    }

  }

  void atom_mask::compute(
    const coord_array_t & sites_frac,
    const double_array_t & atom_radii,
    const shells_array_t &shells)
  {
    if( shells.size() >= max_n_layers )
    {
      std::stringstream str;
      str << "Number of radial shells for the mask must be less than "
        << max_n_layers << ".  Provided: " << shells.size();
      throw error(str.str());
    }
    this->n_layers = shells.size() + 1U;
    boost::posix_time::ptime
      tb = boost::posix_time::microsec_clock::local_time(), te;
    boost::posix_time::time_duration tdif;
    af::const_ref<data_type, grid_t > data_cref = data.const_ref();
    this->atoms_to_asu(sites_frac, atom_radii);
    te = boost::posix_time::microsec_clock::local_time();
    tdif = te - tb;
    debug_atoms_to_asu_time = tdif.total_milliseconds();
    tb = boost::posix_time::microsec_clock::local_time();
    // masking is the slowest part of this routine, for not optimized asus
    this->mask_asu();
    te = boost::posix_time::microsec_clock::local_time();
    tdif = te - tb;
    debug_mask_asu_time = tdif.total_milliseconds();
    tb = boost::posix_time::microsec_clock::local_time();
    this->compute_accessible_surface(this->asu_atoms, shells);
    te = boost::posix_time::microsec_clock::local_time();
    tdif = te - tb;
    debug_accessible_time = tdif.total_milliseconds();
    tb = boost::posix_time::microsec_clock::local_time();
    this->compute_contact_surface();
    te = boost::posix_time::microsec_clock::local_time();
    tdif = te - tb;
    debug_contact_time = tdif.total_milliseconds();
  }

  void atom_mask::get_expanded_asu_boundaries(scitbx::double3 &low,
    scitbx::double3 &high) const
  {
    low = this->expanded_box[0];
    high = this->expanded_box[1];
  }

  void atom_mask::get_asu_boundaries(scitbx::int3 &low,
    scitbx::int3 &high) const
  {
    low = this->asu_low;
    high = this->asu_high;
  }

  void atom_mask::get_expanded_asu_boundaries(scitbx::int3 &low,
    scitbx::int3 &high) const
  {
    low = scitbx::int3( this->data.accessor().origin() );
    high = scitbx::int3( this->data.accessor().last() );
  }

  inline scitbx::double3 conv_(const rvector3_t r)
  {
    return scitbx::double3( boost::rational_cast<double,int>(r[0]),
      boost::rational_cast<double,int>(r[1]),
      boost::rational_cast<double,int>(r[2]));
  }

  void atom_mask::determine_boundaries()
  {
    const scitbx::int3 n = this->grid_size();
    MMTBX_ASSERT( n[0]>0 && n[1]>0 && n[2]>0 );
    cctbx::sgtbx::asu::rvector3_t mn, mx;
    this->asu.box_corners(mn,mx);
    this->expanded_box[0] = conv_(mn);
    this->expanded_box[1] = conv_(mx);
    MMTBX_ASSERT( scitbx::ge_all(expanded_box[1], expanded_box[0]) );
    scitbx::mul(mn, n);
    scitbx::mul(mx, n);
    // inclusive: [imn, imx]
    this->asu_low = scitbx::floor(mn);
    this->asu_high = scitbx::ceil(mx); // asu boundaries
    MMTBX_ASSERT( scitbx::gt_all(this->asu_high, this->asu_low) );
    this->asu_high += scitbx::int3(1,1,1); // now typical C++: [imn,imx)
    // the following is only assumed in atoms_to_asu
    // MMTBX_ASSERT( scitbx::le_all(this->asu_high, n+1) );
    // MMTBX_ASSERT( scitbx::ge_all(this->asu_low, -n-1) );

    // expand asu by shrink_truncation_radius
    const scitbx::af::tiny<double,6> rcell = cell.reciprocal_parameters();
    const scitbx::double3 rp(rcell[0], rcell[1], rcell[2]);
    // TODO: try different constant 1.005
    scitbx::double3 shrink_box = rp * (shrink_truncation_radius*1.05);
    this->expanded_box[0] -= shrink_box;
    this->expanded_box[1] += shrink_box;
    scitbx::mul( shrink_box, n );
    // expanded asu boundaries
    scitbx::int3 emn, emx;
    emn = this->asu_low + ifloor( -shrink_box );
    emx = this->asu_high + iceil( shrink_box ) - scitbx::int3(1,1,1);
    for(short idim=0; idim<3; ++idim)
      if( emx[idim]<asu_high[idim] )
        ++emx[idim];
    MMTBX_ASSERT( scitbx::ge_all(emx, emn) && scitbx::ge_all(emx,this->asu_high)
      && scitbx::le_all(emn, this->asu_low) );
    grid_t interval(emn,emx);
    double n_bytes = sizeof(mask_array_t::value_type);
    for( unsigned char j=0; j<3U; ++j)
      n_bytes *= interval.all()[j];
    std::size_t max_int = std::numeric_limits<std::size_t>::max()-3;
    if( n_bytes > max_int || n_bytes<=0.0 )
    {
      std::ostringstream str;
      str << "mask dimensions: [" << emn << ",  " << emx << ") are too large.\n"
        << " It requires " << n_bytes << " Bytes. Maxumum possible: " << max_int
        << ". 64 bit OS and/or software may be required.";
      throw error(str.str());
    }
    this->data.resize(interval);
  }

  void atom_mask::atoms_to_asu(
    const coord_array_t & sites_frac,
    const double_array_t & atom_radii)
  {
    if( sites_frac.size() != atom_radii.size() )
    {
      std::ostringstream str;
      str << "Mask calculation: number of atomic coordinates and radii"
        " must be the same. Provided: coordinates= " << sites_frac.size()
        << "  radii= " << atom_radii.size();
      throw error(str.str());
    }
    this->asu_atoms.clear();
    const scitbx::af::tiny<double,6> rcell = cell.reciprocal_parameters();
    const scitbx::double3 rp(rcell[0], rcell[1], rcell[2]);

    scitbx::af::shared< cctbx::sgtbx::rt_mx > symops_ = group.all_ops();
    scitbx::af::const_ref< cctbx::sgtbx::rt_mx > symops = symops_.const_ref();
    const size_t order = symops.size();
    MMTBX_ASSERT( order == group.order_z() );

    const signed char n_corners = 2;
    scitbx::double3 asu_box[n_corners];
    this->get_expanded_asu_boundaries(asu_box[0], asu_box[1]);
    // TODO: the assumption that asu is within [-1,1] has been removed. The
    // asu can span multiple cells, that is not very good. It might lead to
    // asu enclosing box being bigger than the unit cell, and require more
    // atoms to be tested for intersection. Maybe need to create a
    // transformation to bring asu into [-1,1]
    const scitbx::vec3<int> cell_min = ifloor(asu_box[0]),
      cell_max = iceil(asu_box[1]);
    for(size_t iat=0; iat<sites_frac.size(); ++iat)
    {
      const scitbx::double3 at = sites_frac[iat];
      const double at_r =  atom_radii[iat];
      const double radius = at_r + solvent_radius;

      MMTBX_ASSERT( radius >= 0.0 );
      scitbx::double3 box = rp * (radius*1.05);
      scitbx::vec3<int> ibox = ifloor(box) + 2;
      for(short idim=0; idim<3; ++idim)
        if( ibox[idim] < 1 )
          ibox[idim] = 1;
      const scitbx::vec3<int> cmin = cell_min - ibox,
        cmax = cell_max + ibox;

      for(size_t isym=0; isym<order; ++isym)
      {
        scitbx::double3 sym_at = symops[isym]*at;
        sym_at -= scitbx::floor(sym_at);
        scitbx::vec3<int> cell;
        // In previous version there was mapping onto full cell
        // so only atoms in the adjacent [1,+1] cells needed
        // to be tested for the intersection with the asu.
        // Because if the atom in the farther cell [+-n]
        // is intersecting with the asu then so does the
        // closer one.
        // In this version, there is no mapping, so need
        // to collect all intersecting atoms
        for(cell[0] = cmin[0]; cell[0]<=cmax[0]; ++cell[0])
        {
          for(cell[1] = cmin[1]; cell[1]<=cmax[1]; ++cell[1])
          {
            for(cell[2] = cmin[2]; cell[2]<=cmax[2]; ++cell[2])
            {
              const scitbx::double3 sym_at_cell = sym_at + cell;
              cctbx::sgtbx::asu::intersection_kind  intersection
                = cctbx::sgtbx::asu::none;
              const scitbx::double3 atom_box[n_corners] = { sym_at_cell - box,
                sym_at_cell + box };
              CCTBX_ASSERT( scitbx::ge_all(atom_box[1], atom_box[0]) );

              // TODO: improve intersection check to minimize
              // number of intersecting atoms
              if( scitbx::ge_all(atom_box[1], asu_box[0])
                  && scitbx::le_all(atom_box[0], asu_box[1]) )
                intersection =  partially;

              if( intersection != cctbx::sgtbx::asu::none )
              {
                // due to that there is no mapping onto full cell
                // all atoms intersecting with the asu are required
                this->asu_atoms.push_back(atom_t(sym_at_cell,at_r));
                // TODO: possible optimization?
                // test for fully intersecting atom:
                //   is_inside( every corner of atom box)
                //   and the atom box needs to be expanded by
                //   shrink_truncation_radius
                // then break out from the symmetry loop
              }
            } // cell[2] loop
          } // cel[1] loop
        } // cell[0]
      }
    }
  }

  namespace {
    typedef scitbx::af::c_grid_padded<3> padded_grid_t;
    typedef scitbx::af::versa<double, padded_grid_t >
      versa_3d_padded_real_array;
    typedef scitbx::af::versa<std::complex<double>, padded_grid_t >
      versa_3d_padded_complex_array;
  }

  versa_3d_padded_real_array
  atom_mask::mask_data_whole_uc(unsigned char layer)
  {
    if( n_layers == 0 )
      throw error("Must compute mask before calculating structure "
          "factors");
    const bool has_layers =  n_layers > 1;
    if( layer==0 && has_layers )
      throw error("Mask has several layers. "
          "Must specify non-zero layer for structure factors.");
    if( layer==0 && !has_layers )
      layer = 1;
    if( layer>n_layers )
      throw "Wrong mask solvent layer";
    ++layer; // solvent layers start from 2 in this->data
    const mask_array_t &msk = this->get_mask();
    const scitbx::int3 grid_full_cell = this->grid_size();
    scitbx::fftpack::real_to_complex_3d<double> fft(grid_full_cell);

    // m_real : physical dims, n_real - focus dims, m_real >= n_real
    const scitbx::int3 mdim = fft.m_real(), ndim = fft.n_real();
    MMTBX_ASSERT( ndim == grid_full_cell );
    MMTBX_ASSERT( scitbx::le_all( ndim, mdim ) );
    double n_bytes = sizeof(double);
    for(unsigned char j=0; j<3U; ++j)
      n_bytes *= mdim[j];
    const std::size_t max_int = std::numeric_limits<std::size_t>::max()-3;
    if( n_bytes > max_int || n_bytes <= 0.0 )
    {
      std::ostringstream str;
      str << "mask fft size: " << mdim << " is too large.\n"
        << " It requires " << n_bytes << " Bytes. Maxumum possible: " << max_int
        << ". 64 bit OS and software may be required.";
      throw error(str.str());
    }
    const padded_grid_t pad( mdim, ndim );
    // TODO: optimize?  padded_real could be a very huge array; filling it with
    // 0 could be the slowest part of this routine apart from fft
    versa_3d_padded_real_array padded_real(pad, 0.0);
    // convert non-padded asu-sized integer mask to padded full-cell
    // sized real array
    scitbx::af::ref<double, padded_grid_t > prref = padded_real.ref();
    scitbx::af::const_ref<data_type, grid_t > mskref = msk.const_ref();
    scitbx::int3 imn, imx;
    this->get_asu_boundaries(imn, imx); // [imn, imx)
    std::vector<long> kc(imx[2]-imn[2]);
    long kk = imn[2];
    for(std::vector<long>::iterator it=kc.begin(); it!=kc.end(); ++it, ++kk)
    {
      long k_c = kk % ndim[2];
      if( k_c <0 )
        k_c += ndim[2];
      *it = k_c;
    }
    MMTBX_ASSERT( kk==imx[2] );
    scitbx::vec3<size_t> cn = prref.accessor().all();
    scitbx::vec3<long> n( mskref.accessor().all() );
    const long nynz = n[1]*n[2], cnynz = cn[1]*cn[2];
    long j_c_b = imn[1] % ndim[1];
    if( j_c_b<0 )
      j_c_b += ndim[1];
    j_c_b *= cn[2];
    const long j_c_e = ndim[1] * cn[2];
    const data_type *p_asu_x = &mskref(imn);
    for(long i=imn[0]; i<imx[0]; ++i, p_asu_x += nynz )
    {
      long i_c = i % ndim[0];
      if( i_c<0 )
        i_c += ndim[0];
      const long ind_c_x = i_c*cnynz;
      long ind_c_y = ind_c_x + j_c_b;
      const long ind_c_y_e = ind_c_x + j_c_e;
      const data_type *const p_asu_y_e = p_asu_x + (imx[1]-imn[1])*n[2];
      for(const data_type *p_asu_y = p_asu_x; p_asu_y!=p_asu_y_e;
        p_asu_y += n[2], ind_c_y += cn[2] )
      {
        if( ind_c_y == ind_c_y_e )
          ind_c_y = ind_c_x;
        const data_type *p_asu_z = p_asu_y;
        for(std::vector<long>::const_iterator k_c=kc.begin(); k_c!=kc.end();
          ++k_c, ++p_asu_z)
        {
          const data_type t = *p_asu_z;
          MMTBX_ASSERT( t.is_valid_for_fft() );
          if( t.layer() == layer )
          {
            MMTBX_ASSERT( t.multiplicity() > 0 && !t.is_outside() );
            prref[ ind_c_y + *k_c ] = t.multiplicity();
          }
        }
      }
    }
    return padded_real;
  }

  scitbx::af::shared< std::complex<double> > atom_mask::structure_factors(
    const scitbx::af::const_ref< cctbx::miller::index<> > &indices,
    unsigned char layer)
  {
    versa_3d_padded_real_array padded_real =
      atom_mask::mask_data_whole_uc(layer);
    const scitbx::int3 grid_full_cell = this->grid_size();
    scitbx::fftpack::real_to_complex_3d<double> fft(grid_full_cell);
    const scitbx::int3 mdim = fft.m_real(), ndim = fft.n_real();
    MMTBX_ASSERT( ndim == grid_full_cell );
    MMTBX_ASSERT( scitbx::le_all( ndim, mdim ) );

    boost::posix_time::ptime
      tb = boost::posix_time::microsec_clock::local_time(), te;
    fft.forward(padded_real); // in-place forward FFT
    const padded_grid_t pad_complex( fft.n_complex(), fft.n_complex() );
    versa_3d_padded_complex_array result(padded_real.handle(), pad_complex );
    boost::posix_time::time_duration tdif =
      boost::posix_time::microsec_clock::local_time() - tb;
    debug_fft_time = tdif.total_milliseconds();

    const cctbx::maptbx::structure_factors::from_map<double> the_from_map(
      group,
      false, // anomalous flag
      indices,
      result.const_ref(),
      true); // conjugate_flag
    const double scale = cell.volume()
      / ( ndim.product() * static_cast<double>(group.order_z()) );
    // result.size() could be approx 1000 * the_from_map.data().size()
    // this does not work :( the_from_map.data() *= scale;
    scitbx::af::ref< std::complex<double> > dref = the_from_map.data().ref();
    // scaling takes no time
    for(scitbx::af::ref< std::complex<double> >::iterator i=dref.begin();
      i!=dref.end(); ++i)
      (*i) *= scale;
    return the_from_map.data();
  }

  void atom_mask::determine_gridding(cctbx::sg_vec3 &grid, double resolution,
    double factor) const
  {
    MMTBX_ASSERT( factor > 0.0 );
    double step = resolution/factor;
    if(step < 0.15) step = 0.15; // XXX arbitrary, see also masks.py
    step = std::min(0.8, step); // XXX arbitrary, can lead to very large maps
    const double d_min = 2.0*step, resolution_factor = 0.5;
    const cctbx::sgtbx::search_symmetry_flags use_all(
        true, // use_space_group_symmetry
        0,    // use_space_group_ltr, not used if use_space_group_symmetry==true
        true, // use_seminvariants
        true, // use_normalizer_k2l
        true  // use_normalizer_l2n
    );
    grid = cctbx::maptbx::determine_gridding<int>(cell, d_min,
      resolution_factor, use_all, group.type());
  }

  void atom_mask::compute_accessible_surface(const atom_array_t & atoms,
      const shells_array_t  &shells)
  {
    cctbx::uctbx::unit_cell const& unit_cell = this->cell;
    // Severe code duplication: cctbx/maptbx/average_densities.h
    af::ref<data_type, grid_t > data_ref = data.ref();
    scitbx::int3 n_g = this->grid_size();
    const int nx = n_g[0];
    const int ny = n_g[1];
    const int nz = n_g[2];
    MMTBX_ASSERT( nx>0 && ny>0 && nz>0 );
    const f_t mr1= static_cast<f_t>(unit_cell.metrical_matrix()[0]); // a*a
    const f_t mr5= static_cast<f_t>(unit_cell.metrical_matrix()[1]); // b*b
    const f_t mr9= static_cast<f_t>(unit_cell.metrical_matrix()[2]); // c*c
    // a*b*cos(gamma)
    const f_t mr2= static_cast<f_t>(unit_cell.metrical_matrix()[3]);
    // a*c*cos(beta)
    const f_t mr3= static_cast<f_t>(unit_cell.metrical_matrix()[4]);
    // c*b*cos(alpha)
    const f_t mr6= static_cast<f_t>(unit_cell.metrical_matrix()[5]);
    const f_t tmr2 = mr2*2; //2*a*b*cos(gamma);
    const f_t tmr3 = mr3*2; //2*a*c*cos(beta);
    const f_t tmr6 = mr6*2; //2*b*c*cos(alpha);
    const f_t sx = 1/static_cast<f_t>(nx);
    const f_t tsx= sx*2;
    const f_t sxsq=mr1*sx*sx;
    const f_t sy = 1/static_cast<f_t>(ny);
    const f_t tsy= sy*2;
    const f_t sysq=mr5*sy*sy;
    const f_t sz = 1/static_cast<f_t>(nz);
    const f_t tsz= sz*2;
    const f_t szsq=mr9*sz*sz;
    const f_t w1=mr1*sx*tsx; const f_t w4=mr5*sy*tsy;
    const f_t w2=mr2*sx*tsy; const f_t w5=mr6*sy*tsz;
    const f_t w3=mr3*sx*tsz; const f_t w6=mr9*sz*tsz;
    const f_t tsxg1=tsx*mr1; const f_t tsyg4=tsy*mr2; const f_t tszg3=tsz*mr3;
    const f_t tsxg4=tsx*mr2; const f_t tsyg5=tsy*mr5; const f_t tszg8=tsz*mr6;
    const f_t tsxg7=tsx*mr3; const f_t tsyg8=tsy*mr6; const f_t tszg9=tsz*mr9;
    f_t rp[3];
    for(unsigned i=0;i<3;i++) {
      rp[i] = static_cast<f_t>(unit_cell.reciprocal_parameters()[i]);
    }
    scitbx::int3 asu_min, asu_max;
    this->get_expanded_asu_boundaries(asu_min, asu_max);
    const scitbx::vec3<long> sz_asu(data_ref.accessor().all());
    const long sz_yz = sz_asu[1]*sz_asu[2];
    const long asu_off = sz_yz* asu_min[0] + sz_asu[2]* asu_min[1] + asu_min[2];
    f_t shells_rad = 0.0;
    for(shells_array_t::const_iterator sh=shells.begin(); sh!=shells.end();
        ++sh)
    {
      if( *sh < 0.0 || (*sh)>=std::numeric_limits<double>::max() )
      {
        std::stringstream str;
        str << "Mask calculation: inapropriate radial shell width = " << *sh;
        throw error(str.str());
      }
      shells_rad += *sh;
    }
    MMTBX_ASSERT( shells_rad>=0.0 );
    shells_array_t layers_sq(shells.size()+2);
    for(std::size_t i_site=0;i_site<atoms.size();i_site++) {
      cctbx::fractional<> const& site = atoms[i_site].first;
      const f_t xfi=static_cast<f_t>(site[0]);
      const f_t yfi=static_cast<f_t>(site[1]);
      const f_t zfi=static_cast<f_t>(site[2]);
      const f_t atmrad = atoms[i_site].second;
      MMTBX_ASSERT( atmrad >= 0.0 );
      const f_t radsq=static_cast<f_t>(atmrad*atmrad);
      const f_t prot_cutoff=static_cast<f_t>(atmrad+solvent_radius);
      const f_t shell_cutoff = prot_cutoff + shells_rad;
      MMTBX_ASSERT( layers_sq.size() == shells.size()+2 );
      MMTBX_ASSERT( layers_sq.size() == n_layers+1 );
      layers_sq[0] = radsq;
      layers_sq[1] = prot_cutoff*prot_cutoff;
      f_t csq = prot_cutoff;
      for(unsigned ii=0; ii<shells.size(); ++ii)
      {
        MMTBX_ASSERT( shells[ii]>=0.0 );
        csq += shells[ii];
        layers_sq[2+ii] = csq*csq;
      }
      const f_t cutoff=shell_cutoff;
      const f_t coas = cutoff*rp[0];
      int x1box=ifloor(nx*(xfi-coas));
      int x2box=1+iceil(nx*(xfi+coas));
      const f_t cobs = cutoff*rp[1];
      int y1box=ifloor(ny*(yfi-cobs));
      int y2box=1+iceil(ny*(yfi+cobs));
      const f_t cocs = cutoff*rp[2];
      int z1box=ifloor(nz*(zfi-cocs));
      int z2box=1+iceil(nz*(zfi+cocs));
      x1box = std::max( x1box, static_cast<int>(asu_min[0]));
      y1box = std::max( y1box, static_cast<int>(asu_min[1]));
      z1box = std::max( z1box, static_cast<int>(asu_min[2]));
      x2box = std::min( x2box, static_cast<int>(asu_max[0]));
      y2box = std::min( y2box, static_cast<int>(asu_max[1]));
      z2box = std::min( z2box, static_cast<int>(asu_max[2]));
      const f_t sxbcen=xfi-x1box*sx;
      const f_t sybcen=yfi-y1box*sy;
      const f_t szbcen=zfi-z1box*sz;
      const f_t distsm=mr1*sxbcen*sxbcen+mr5*sybcen*sybcen+mr9*szbcen*szbcen
            +tmr2*sxbcen*sybcen+tmr3*sxbcen*szbcen+tmr6*sybcen*szbcen;
      const f_t w7=tsxg1*sxbcen+tsxg4*sybcen+tsxg7*szbcen;
      const f_t w8=tsyg4*sxbcen+tsyg5*sybcen+tsyg8*szbcen;
      const f_t w9=tszg3*sxbcen+tszg8*sybcen+tszg9*szbcen;
      f_t distsx = distsm;
      f_t s1xx = sxsq - w7;
      f_t s1xy = sysq - w8;
      f_t s1xz = szsq - w9;
      const long ind_x_max = sz_yz*x2box - asu_off;
      const long yba1 = y1box*sz_asu[2], yba2 = y2box*sz_asu[2];
      for(long ind_x = x1box*sz_yz - asu_off ; ind_x<ind_x_max;
        ind_x+=sz_yz)
      {
        f_t s2yz = s1xz;
        f_t s2_incr = s1xy;
        f_t s2 = distsx;
        const long ind_y_max = ind_x + yba2;
        for(long ind_y=ind_x+yba1; ind_y<ind_y_max; ind_y+=sz_asu[2])
        {
          f_t s3_incr = s2yz;
          f_t dist = s2;
          const long ind_z_max = ind_y+z2box;
          for(long ind_z=ind_y+z1box; ind_z<ind_z_max; ++ind_z)
          {
            const f_t dist_c  = dist;
            for(unsigned ii=0; ii<layers_sq.size(); ++ii)
            {
              if( dist_c < layers_sq[ii] )
              {
                data_type& dr = data_ref[ind_z];
                if( dr.layer() > ii )
                  dr.set(ii, ii==0?0:dr.multiplicity());
                break;
              }
            }
            dist += s3_incr;
            s3_incr += w6;
          } // z-box loop
          s2 += s2_incr;
          s2_incr += w4;
          s2yz += w5;
        } // y-box loop
        distsx += s1xx;
        s1xx += w1;
        s1xy += w2;
        s1xz += w3;
      } // x-box loop
    } //atom loop
    return;
  }

  typedef af::const_ref< cctbx::sgtbx::rt_mx > symop_array;

  void find_neighbors(
      std::vector<long> &table,
      cctbx::uctbx::unit_cell const& unit_cell,
      af::c_grid<3>::index_type const& gridding_n_real,
      const scitbx::vec3<long> &na,
      double shrink_truncation_radius)
  {
    int low[3];
    int high[3];
    for(unsigned i=0;i<3;i++) {
      double x = shrink_truncation_radius
               * unit_cell.reciprocal_parameters()[i]
               * gridding_n_real[i];
      low[i] = ifloor(-x);
      high[i] = iceil(x);
    }
    const int n0 = static_cast<int>(gridding_n_real[0]);
    const int n1 = static_cast<int>(gridding_n_real[1]);
    const int n2 = static_cast<int>(gridding_n_real[2]);
    const f_t shrink_truncation_radius_sq = shrink_truncation_radius
                                    * shrink_truncation_radius;
    cctbx::fractional<f_t> frac;
    for(int p0=low[0];p0<=high[0];p0++)
    {
      frac[0] = static_cast<f_t>(p0) / n0;
      for(int p1=low[1];p1<=high[1];p1++)
      {
        frac[1] = static_cast<f_t>(p1) / n1;
        for(int p2=low[2];p2<=high[2];p2++)
        {
          frac[2] = static_cast<f_t>(p2) / n2;
          f_t dist_sq = unit_cell.length_sq(frac);
          if (dist_sq < shrink_truncation_radius_sq)
            table.push_back(p0*(na[1]*na[2]) + p1*na[2]  + p2);
        } // z
      } // y
    } // x
  }

  void atom_mask::compute_contact_surface()
  {
    typedef double f_t;
    cctbx::uctbx::unit_cell const& unit_cell = this->cell;

    size_t nsolv = 0;
    af::ref<data_type, asu_grid_t > data_ref = data.ref();
    std::size_t data_size = data_ref.size();
    af::small<std::size_t,max_n_layers+2U> nsolvs;
    nsolvs.resize(this->n_solvent_layers()+2U,0UL);
    if(shrink_truncation_radius == 0) {
      for(std::size_t ilxyz=0;ilxyz<data_size;ilxyz++)
      {
        data_type &d = data_ref[ilxyz];
        if( d.is_contact() || d.is_outside() )
          d.set_zero();
        nsolv += static_cast<size_t>(d.multiplicity());
        nsolvs[d.layer()] += static_cast<size_t>(d.multiplicity());
      }
      contact_surface_fraction = accessible_surface_fraction
        = static_cast<double>(nsolv) / this->grid_size_1d();
    }
    else
    {
      af::versa<data_type, asu_grid_t > datacopy = data.deep_copy();
      const af::const_ref<data_type, asu_grid_t > datacopy_ref
        = datacopy.const_ref();
      const scitbx::vec3<long> n(datacopy_ref.accessor().all());
      std::vector<long> neighbors;
      find_neighbors(neighbors, unit_cell, this->grid_size(), n,
        shrink_truncation_radius);
      MMTBX_ASSERT(neighbors.size()>0U);
      size_t n_access=0;
      const data_type *datacopy_ptr = datacopy_ref.begin();
      for(data_type *data_ptr = data_ref.begin(); data_ptr!=data_ref.end();
        ++data_ptr, ++datacopy_ptr)
      {
        const data_type d_copy = *datacopy_ptr;
        data_type &dr = *data_ptr;
        if( d_copy.is_outside() )
          dr.set_zero();
        else if( d_copy.is_solvent() )
          n_access += static_cast<size_t>( d_copy.multiplicity() );
        else if( d_copy.is_contact() )
        {
          for(std::vector<long>::const_iterator neighbor=neighbors.begin();
            neighbor!=neighbors.end(); ++neighbor)
          {
            // neighbor could be outside the asu, but must be inside the
            // expanded asu
            if( datacopy_ptr[*neighbor].is_solvent() )
            {
              MMTBX_ASSERT( dr.is_contact() );
              dr.set_nearest_solvent();
              MMTBX_ASSERT( dr.multiplicity() > 0 );
              MMTBX_ASSERT( dr.is_solvent() );
              goto end_of_neighbors_loop;
            }
          }
          dr.set_zero();
          end_of_neighbors_loop:;
        }
        nsolv += dr.multiplicity();
        nsolvs[dr.layer()] += static_cast<size_t>(dr.multiplicity());
      } // data_ref array
      // currently data is not padded 3-D array, data.size is correct here
      contact_surface_fraction = static_cast<double>(nsolv)/this->grid_size_1d();
      accessible_surface_fraction = static_cast<double>(n_access)
        / this->grid_size_1d();
    }

    layer_volume_fractions_.resize(this->n_solvent_layers(),0.0);
    std::size_t nsolvt=0;
    for(unsigned short j=2; j<nsolvs.size(); ++j)
    {
      layer_volume_fractions_[j-2] = nsolvs[j]/static_cast<double>(this->grid_size_1d());
      nsolvt += nsolvs[j];
    }
    MMTBX_ASSERT( nsolv == nsolvt );
  }

  // writing as xplor map is stolen from iotbx/xplor/boost_python/xplor_ext.cpp
  // which is inaccessible from C++
  namespace
  {
    template <unsigned Width>
    struct format_e
    {
      static void
      throw_error()
      {
        throw scitbx::error("Floating-point value too large for format.");
      }

      format_e(const char* fmt, double val)
      {
#if !defined(BOOST_MSVC)
        s = buf;
        std::sprintf(buf, fmt, val);
        if (*(s + Width)) throw_error();
#else
        s = buf + 1;
        std::sprintf(s, fmt, val);
        char* p = s + Width;
        if (*p) {
          p++;
          if (*p) throw_error();
        }
        else {
          s--;
          *s = ' ';
        }
        if (*(p-3) != '0') throw_error();
        *(p-3) = *(p-2);
        *(p-2) = *(p-1);
        *(p-1) = '\0';
#endif
      }

      char buf[32];
      char* s;
    };

    FILE*
    write_head(
      std::string const& file_name,
      cctbx::uctbx::unit_cell const& unit_cell,
      const scitbx::af::int3 &n,
      const scitbx::af::int3 &first,
      const scitbx::af::int3 &last)
    {
      FILE* fh = fopen(file_name.c_str(), "wb");
      MMTBX_ASSERT(fh != 0);
      fprintf(fh, "\n%8d !NTITLE\n", 1);
      fprintf(fh, "%-264s\n", " REMARKS atom_mask::xplor_write...");
      fprintf(fh, " %7d %7d %7d %7d %7d %7d %7d %7d %7d\n", n[0], first[0],
          last[0], n[1], first[1], last[1], n[2], first[2], last[2]);
      for(std::size_t i=0;i<6;i++) {
        fprintf(fh, "%s",
          format_e<12>("%12.5E", unit_cell.parameters()[i]).s);
      }
      fprintf(fh, "\n");
      fprintf(fh, "ZYX\n");
      return fh;
    }

    void
    write_tail(
      FILE* fh,
      double average,
      double standard_deviation)
    {
      fprintf(fh, "   -9999\n");
      fprintf(fh, "%s%s\n",
        format_e<12>("%12.4E", average).s,
        format_e<12>("%12.4E", standard_deviation).s);
      fclose(fh);
    }

  } // namespace {

  void atom_mask::xplor_write_map(std::string const& file_name,
      unsigned char layer, bool invert)
  {
    if( n_layers == 0 )
      throw error("Must compute mask before saving it.");
    const bool has_layers =  n_layers > 1;
    if( layer==0 && has_layers )
      throw error("Mask has several layers. "
          "Must specify non-zero layer to save.");
    if( layer==0 && !has_layers )
      layer = 1;
    if( layer>n_layers )
      throw error("Wrong mask solvent layer");
    ++layer; // solvent layers start from 2 in this->data
    const double one = (invert ? 0.0 : 1.0),
          zero = (invert ? 1.0 : 0.0);
    const mask_array_t &msk = this->get_mask();
    scitbx::af::const_ref<data_type, grid_t > mskref = msk.const_ref();
    scitbx::int3 imn, imx;
    this->get_asu_boundaries(imn, imx); // [imn, imx)
    FILE* fh = write_head(file_name, this->cell, this->grid_size(), imn,
        imx-scitbx::int3(1,1,1));
    double mean = 0.0, esd = 0.0;
    std::size_t n=0;
    for(long iz=imn[2]; iz<imx[2]; ++iz)
    {
      fprintf(fh, "%8lu\n", static_cast<unsigned long>(iz));
      int i_fld = 0;
      for(long iy=imn[1]; iy<imx[1]; ++iy)
      {
        for(long ix=imn[0]; ix<imx[0]; ++ix)
        {
          const scitbx::int3 pos(ix,iy,iz);
          const data_type t = mskref(pos);
          const double d = ((t.layer() == layer) ? one : zero);
          mean += d;
          esd += d*d;
          ++n;
          fprintf(fh, "%s", format_e<12>("%12.5E", d).s);
          i_fld++;
          if (i_fld == 6)
          {
            fprintf(fh, "\n");
            i_fld = 0;
          }
        } // x-loop
      } // y-loop
      if (i_fld > 0) {
        fprintf(fh, "\n");
      }
    } // z-loop
    MMTBX_ASSERT(n>=1.0);
    mean /= n;
    esd = esd / n - mean*mean;
    MMTBX_ASSERT(esd>=0.0);
    esd = std::sqrt(esd);
    write_tail(fh, mean, esd);
  }

  bool is_nice(const cctbx::sgtbx::rot_mx &rot_mx)
  {
    int den = rot_mx.den();
    CCTBX_ASSERT( den!=0 );
    const std::size_t sz = rot_mx.num().size();
    CCTBX_ASSERT( sz==9 );
    for(int j=0; j<sz; ++j)
    {
      const int e = rot_mx[j];
      if( e != den && e!=-den && e!=0 )
        return false;
    }
    return true;
  }

  inline bool is_nice(const cctbx::sgtbx::rt_mx &rt_mx)
  {
    return is_nice(rt_mx.r());
  }

  bool is_nice(const cctbx::sgtbx::space_group &g)
  {
    for(int i=0; i<g.n_smx(); ++i)
    {
      if( !is_nice(g.smx(i)) )
        return false;
    }
    return true;
  }

  void generate_groups(std::set<std::string> &halls,
      const std::string &group_symbol, int ncb)
  {
    halls.clear();
    cctbx::sgtbx::space_group_symbols symbol(group_symbol);
    const cctbx::sgtbx::space_group sg_def(symbol);
    CCTBX_ASSERT( is_nice(sg_def) );
    halls.insert("Hall: "+sg_def.type().hall_symbol());
    int n = 0;
    int na = 0;
    int nf = 0;
    boost::mt19937 rng; // produces randomness out of thin air
    const int i_max = 2;
    boost::uniform_int<> six(-i_max,i_max);  // distribution
    const double n_max = 1.5*std::pow(2.0*i_max+1.0,9.0);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
             die(rng, six);  // glues randomness with mapping
    while( true )
    {
      n = n + 1;
      if( n>ncb*100000 || static_cast<double>(n)>n_max )
        break;
      cctbx::sgtbx::rot_mx rot_mx;
      for(int i=0; i<rot_mx.num().size(); ++i)
        rot_mx[i] = die();
      cctbx::sgtbx::tr_vec tr;
      for(int i=0; i<tr.num().size(); ++i)
        tr[i] = die();
      if( !((rot_mx.determinant()== 1) && rot_mx.is_valid()) )
        continue;
      const cctbx::sgtbx::rt_mx rt_mx( rot_mx ); //,  tr)
      if( !rt_mx.is_valid() )
        continue;
      std::string h("Hall: ");
      cctbx::sgtbx::space_group g;
      try
      {
        const cctbx::sgtbx::change_of_basis_op cb(rt_mx);
        if( (!cb.is_valid()) || cb.is_identity_op() )
          continue;
        g = sg_def.change_basis(cb);
        const cctbx::sgtbx::space_group_type t = g.type();
        h += t.hall_symbol();
      }
      catch( const std::exception &)
      {
        nf = nf+1;
        continue;
      }
      if( !is_nice(g) )
        continue;
      halls.insert(h);
      na = na + 1;
      if( halls.size()>=ncb+1 )
        break;
    } // while(true)
    return;
  }

}} // namespace mmtbx::masks


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Wake panels creation
// Compute wake panel collocation point, surface, vertices, normal, longitudinal, transverse and perpendicular
// vectors from data contained into sGrid
// Wake panels extend to a constant x coordinate, based on the geometry
//
// I/O:
// - sGrid: temporary dynamic array containing body panel vertices
// - bPan: body panels (structure)
// - wPan: wake panels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "create_wake.h"

#define  NDIM 3

using namespace std;
using  namespace Eigen;

void create_wake(MatrixX3d &sGrid, Network &bPan, Network &wPan) {

    // Temporary variables
    double norm = 0;
    Vector3d v1(NDIM), v2(NDIM);
    double dwnstrm;
    int c2, c3;

    //// Begin
    cout << "Creating wake panels... " << flush;

    // Set number of panels (currently only handles predefined wakes)
    wPan.nC = 2;
    wPan.nC_ = 1;
    wPan.nS = bPan.nS;
    wPan.nS_ = bPan.nS_;
    wPan.nP = wPan.nS_;

    // Resize network matrices
    wPan.CG.resize(wPan.nP, NDIM);
    wPan.v0.resize(wPan.nP, NDIM);
    wPan.v1.resize(wPan.nP, NDIM);
    wPan.v2.resize(wPan.nP, NDIM);
    wPan.v3.resize(wPan.nP, NDIM);
    wPan.S.resize(wPan.nP, 1);
    wPan.n.resize(wPan.nP, NDIM);
    wPan.l.resize(wPan.nP, NDIM);
    wPan.t.resize(wPan.nP, NDIM);
    wPan.p.resize(wPan.nP, NDIM);

    // Downstream coordinate of wake panels = tip x-coord+ 5*chord
    dwnstrm = sGrid(sGrid.rows()-1,0) + 5*sGrid(0,0);

    // Collocations points, corners and vectors
    for (int j = 0; j < wPan.nP; ++j) {

        c2 = j * bPan.nC;
        c3 = (j+1) * bPan.nC;

        wPan.v0(j,0) = dwnstrm; // pts[i][k]
        wPan.v1(j,0) = sGrid(c2,0); // pts[i+1][k]
        wPan.v2(j,0) = sGrid(c3,0); // pts[i+1][k+1]
        wPan.v3(j,0) = dwnstrm; // pts[i][k+1]
        wPan.v0(j,1) = sGrid(c2,1);
        wPan.v1(j,1) = sGrid(c2,1);
        wPan.v2(j,1) = sGrid(c3,1);
        wPan.v3(j,1) = sGrid(c3,1);
        wPan.v0(j,2) = sGrid(c2,2);
        wPan.v1(j,2) = sGrid(c2,2);
        wPan.v2(j,2) = sGrid(c3,2);
        wPan.v3(j,2) = sGrid(c3,2);

        wPan.CG(j,0) = (wPan.v0(j,0) + wPan.v1(j,0) + wPan.v2(j,0) + wPan.v3(j,0)) / 4;
        wPan.CG(j,1) = (wPan.v1(j,1) + wPan.v2(j,1)) / 2;
        wPan.CG(j,2) = (wPan.v1(j,2) + wPan.v2(j,2)) / 2;

        wPan.l(j,0) = (wPan.v0(j,0) + wPan.v3(j,0) - wPan.v1(j,0) - wPan.v2(j,0)) / 4;
        wPan.l(j,1) = 0;
        wPan.l(j,2) = 0;
        wPan.l.row(j) /= wPan.l.row(j).norm();

        wPan.t(j,0) = (wPan.v2(j,0) - wPan.v1(j,0)) / 2;
        wPan.t(j,1) = (wPan.v2(j,1) - wPan.v1(j,1)) / 2;
        wPan.t(j,2) = (wPan.v2(j,2) - wPan.v1(j,2)) / 2;
        wPan.t.row(j) /= wPan.t.row(j).norm();
    }
    // Surfaces and normals
    for (int j = 0; j < wPan.nP; ++j) {
        v1(0) = wPan.v2(j,0) - wPan.v0(j,0);
        v2(0) = wPan.v1(j,0) - wPan.v3(j,0);
        v1(1) = wPan.v2(j,1) - wPan.v0(j,1);
        v2(1) = wPan.v1(j,1) - wPan.v3(j,1);
        v1(2) = wPan.v2(j,2) - wPan.v0(j,2);
        v2(2) = wPan.v1(j,2) - wPan.v3(j,2);

        wPan.n.row(j) = v1.cross(v2);
        norm = wPan.n.row(j).norm();

        wPan.S(j) = norm/2;
        wPan.n.row(j) /= norm;
    }
    // Perpendicular vector
    for (int j = 0; j < wPan.nP; ++j)
        wPan.p.row(j) = wPan.n.row(j).cross(wPan.l.row(j));

    //// Control display
    cout << "Done!" << endl;
    cout << "Wake panels extend to " << dwnstrm << " in the x direction" << endl;
    #ifdef VERBOSE
        cout << "Collocation points: " << wPan.CG.rows() << 'X' << wPan.CG.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.CG(i,0) << ' ' << wPan.CG(i,1) << ' ' << wPan.CG(i,2) << endl;
        cout << "Panel surfaces: " << wPan.S.rows() << 'X' << wPan.S.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.S(i) << endl;
        cout << "Unit normals: " << wPan.n.rows() << 'X' << wPan.n.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.n(i,0) << ' ' << wPan.n(i,1) << ' ' << wPan.n(i,2) << endl;
        cout << "Unit longitudinal vectors: " << wPan.l.rows() << 'X' << wPan.l.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.l(i,0) << ' ' << wPan.l(i,1) << ' ' << wPan.l(i,2) << endl;
        cout << "Unit transverse vectors: " << wPan.t.rows() << 'X' << wPan.t.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.t(i,0) << ' ' << wPan.t(i,1) << ' ' << wPan.t(i,2) << endl;
        cout << "Unit perpendicular vectors: " << wPan.p.rows() << 'X' << wPan.p.cols() << endl;
        for (int i = 0; i < wPan.nP; ++i)
            cout << i << ' ' << wPan.p(i,0) << ' ' << wPan.p(i,1) << ' ' << wPan.p(i,2) << endl;
    #endif
    cout << endl;
}


#include <iostream>
#include <sstream>
#include <fstream>

#include <layered_grid.h>
#include <config.h>
#include <support.h>

#include <boost/sort/sort.hpp>

LayeredGrid::LayeredGrid(const RectSteinerTree& rst)
    : terminal_points(rst.get_terminal_points())
{
    auto rst_points = rst.get_rst_points();

    for (const auto& point : rst_points)
        layered_points[point] = LayeredGridPoint(point);

    for (const auto& point : terminal_points)
        layered_points[point].enable_pins_connection();

    for (const auto& [begin_point, end_point] : rst)
    {
        if (is_same_horizontal(begin_point, end_point)) {
            const auto [source, target] = get_edges(begin_point.get_x(), end_point.get_x());
            add_segment(horizontal_segments_edge_array[begin_point.get_y()],
                        source,
                        target);

            layered_points[begin_point].enable_horizontal_connection();
            layered_points[end_point].enable_horizontal_connection();
        }
        else if (is_same_vertical(begin_point, end_point)) {
            const auto [source, target] = get_edges(begin_point.get_y(), end_point.get_y());
            add_segment(vertical_segments_edge_array[begin_point.get_x()],
                        source,
                        target);

            layered_points[begin_point].enable_vertical_connection();
            layered_points[end_point].enable_vertical_connection();
        }
        else
            add_segments_diagonal(begin_point, end_point);
    }

    create_segments(horizontal_segments_edge_array, create_horizontal_segment_points);
    create_segments(vertical_segments_edge_array, create_vertical_segment_points);
}

void LayeredGrid::add_segment(LayeredGrid::segment_edges_set_t& segment_edges_set,
                              coord_type source,
                              coord_type target)
{
    auto& [source_set, target_set] = segment_edges_set;
    const auto [can_prepend, can_append] = can_extend_segment(segment_edges_set, source, target);

    if (!can_prepend && !can_append)
    {
        source_set.emplace(source);
        target_set.emplace(target);
    }
    else
    {
        if (can_prepend)
        {
            source_set.erase(target);

            if (!can_append)
                source_set.emplace(source);
        }

        if (can_append)
        {
            target_set.erase(source);

            if (!can_prepend)
                target_set.emplace(target);
        }
    }
}

void LayeredGrid::add_segments_diagonal(const GridPoint& begin_point,
                                        const GridPoint& end_point)
{
    const auto [source_x, target_x] = get_edges(begin_point.get_x(), end_point.get_x());
    const auto [source_y, target_y] = get_edges(begin_point.get_y(), end_point.get_y());

    // horizontal-vertical pair
    const auto h_v_horizontal = std::make_tuple(begin_point.get_y(), source_x, target_x);
    const auto h_v_vertical   = std::make_tuple(end_point.get_x(), source_y, target_y);
    const auto h_v_corner_point = GridPoint(end_point.get_x(), begin_point.get_y());

    // vertical-horizontal pair
    const auto v_h_vertical   = std::make_tuple(begin_point.get_x(), source_y, target_y);
    const auto v_h_horizontal = std::make_tuple(end_point.get_y(), source_x, target_x);
    const auto v_h_corner_point = GridPoint(begin_point.get_x(), end_point.get_y());

    const auto h_v_extend_score = get_extend_score(h_v_horizontal, h_v_vertical);
    const auto v_h_extend_score = get_extend_score(v_h_horizontal, v_h_vertical);

    if (h_v_extend_score >= v_h_extend_score) {
        add_segment_horizontal(h_v_horizontal);
        add_segment_vertical(h_v_vertical);

        layered_points[begin_point].enable_horizontal_connection();
        layered_points[end_point].enable_vertical_connection();

        add_corner_point(h_v_corner_point);
    }
    else {
        add_segment_horizontal(v_h_horizontal);
        add_segment_vertical(v_h_vertical);

        layered_points[begin_point].enable_vertical_connection();
        layered_points[end_point].enable_horizontal_connection();

        add_corner_point(v_h_corner_point);
    }
}

LayeredGrid::extend_score_type LayeredGrid::get_extend_score(const LayeredGrid::segment_coord_t& horizontal_segment,
                                                             const LayeredGrid::segment_coord_t& vertical_segment)
{
    const auto [horizontal_y, source_x, target_x] = horizontal_segment;
    const auto [horizontal_can_prepend, horizontal_can_append] = can_extend_segment(horizontal_segments_edge_array[horizontal_y],
                                                                                source_x,
                                                                                target_x);

    const auto [vertical_x, source_y, target_y] = vertical_segment;
    const auto [vertical_can_prepend, vertical_can_append] = can_extend_segment(vertical_segments_edge_array[vertical_x],
                                                                                source_y,
                                                                                target_y);

    return static_cast<extend_score_type>(horizontal_can_prepend + horizontal_can_append +
                                          vertical_can_prepend + vertical_can_append);
}

void LayeredGrid::create_segments(const std::unordered_map<coord_type, LayeredGrid::segment_edges_set_t>& segments_edge_array,
                                  LayeredGrid::create_segment_points_func_t points_creation_func)
{
    for (const auto& [common_coord, edges_sets] : segments_edge_array)
    {
        const auto& [source_edges_set, target_edges_set] = edges_sets;

        std::vector<coord_type> source_edges_array(source_edges_set.begin(), source_edges_set.end());
        std::vector<coord_type> target_edges_array(target_edges_set.begin(), target_edges_set.end());

        boost::sort::block_indirect_sort(source_edges_array.begin(),
                                         source_edges_array.end(),
                                         config::ParallelConfig::get()->get_num_threads());

        boost::sort::block_indirect_sort(target_edges_array.begin(),
                                         target_edges_array.end(),
                                         config::ParallelConfig::get()->get_num_threads());
                  ;

        for (auto source_it = source_edges_array.begin(), target_it = target_edges_array.begin();
             source_it != source_edges_array.end();
             ++source_it, ++target_it)
        {
            segments.push_back(points_creation_func(common_coord, *source_it, *target_it));
        }
    }
}

void LayeredGrid::output_xml_file(const std::string& output_file) const
{
    std::ostringstream oss;

    oss << config::OUTPUT_XML_BEGINNING;

    for (const auto& terminal_point : terminal_points)
        oss << output_point(terminal_point.get_x(), terminal_point.get_y(), PointLayer::PINS);

    for (const auto& layered_point_pair : layered_points)
        oss << output_layered_point(layered_point_pair.second);

    for (const auto& segment : segments)
        oss << output_segment(segment);

    oss << config::OUTPUT_XML_ENDING;

    write_output_file(output_file, oss.str());
}

std::string LayeredGrid::output_segment(const LayeredGrid::segment_t& segment) const
{
    std::ostringstream oss;

    const auto& [source, target, layer] = segment;
    oss << "<segment x1=\"" << source.get_x() << "\" y1=\"" << source.get_y() << "\""
                   " x2=\"" << target.get_x() << "\" y2=\"" << target.get_y() << "\""
                " layer=\"" << get_layer_string(layer) << "\" />\n";

    return oss.str();
}

std::string LayeredGrid::output_point(coord_type x, coord_type y, PointLayer layer) const
{
    std::ostringstream oss;
    oss << "<point x=\"" << x << "\" y=\"" << y << "\" layer=\"" << get_layer_string(layer) << "\" />\n";
    return oss.str();
}

std::string LayeredGrid::output_layered_point(const LayeredGridPoint& point) const
{
    std::ostringstream oss;

    auto layers = point.get_layers();
    for (const auto& layer : layers)
        oss << output_point(point.get_x(), point.get_y(), layer);

    return oss.str();
}

void LayeredGrid::write_output_file(const std::string& output_file, const std::string& text) const
{
    std::ofstream out(output_file);

    if (out)
    {
        out << text;
        out.close();
    }
    else
        support::raise_error(support::ErrorType::BAD_OPEN_OUTPUT_FILE);
}


#include "QGAP.h"
#ifdef LINUX
#include "Eigen/Core"
#include "Eigen/LU"         // for the determinant
#include "SymEigsSolver.h"  // Also includes <MatOp/DenseSymMatProd.h>
#else
#include <Eigen/Core>       // inclded from project->propoerties->compiler->directories
#include <Eigen/LU>         // for the determinant
#include <SymEigsSolver.h>  // Also includes <MatOp/DenseSymMatProd.h>
#endif
#include <cfloat>
#include "HeuMagnify.h"
#include "HeuCHH.h"
#include "MTHG.h"

using Eigen::MatrixXd;
using namespace Spectra;

QuadraticGAP::QuadraticGAP()
{
   conf = new Config();
}

QuadraticGAP::~QuadraticGAP()
{
   //dtor
}

int QuadraticGAP::Qopt (void)
{
   // data which define the LP problem. setproblemdata() allocates space for the problem data.  */
   char     *probname = NULL;
   int      numcols;
   int      numrows;
   int      objsen;
   double   *obj = NULL;
   double   *rhs = NULL;
   char     *sense = NULL;
   int      *matbeg = NULL;
   int      *matcnt = NULL;
   int      *matind = NULL;
   double   *matval = NULL;
   double   *lb = NULL;
   double   *ub = NULL;
   int      *qmatbeg = NULL;
   int      *qmatcnt = NULL;
   int      *qmatind = NULL;
   double   *qmatval = NULL;
   char     *ctype = NULL;

   // optimization results
   int      solstat;
   double   objval;
   double   *x  = NULL;
   //double   *pi = NULL;
   double   *slack = NULL;
   //double   *dj = NULL;

   CPXENVptr     env = NULL;
   CPXLPptr      lp = NULL;
   int           status;
   int           i, j;
   int           cur_numrows, cur_numcols;

   vector < vector <double>> q(m, vector<double>(n)); // for debug

   optimality_target = conf->opt_target;

   // Initialize the CPLEX environment
   env = CPXopenCPLEX(&status);
   if (env == NULL)
   {  char  errmsg[CPXMESSAGEBUFSIZE];
      fprintf(stderr, "Could not open CPLEX environment.\n");
      CPXgeterrorstring(env, status, errmsg);
      fprintf(stderr, "%s", errmsg);
      goto TERMINATE;
   }

   // Turn on output to the screen
   status = CPXsetintparam(env, CPXPARAM_ScreenOutput, CPX_ON);
   if (status)
   {  fprintf(stderr, "Failure to turn on screen indicator, error %d.\n", status);
      goto TERMINATE;
   }

   // Fill in the data for the problem.
   status = setproblemdata(&probname, &numcols, &numrows, &objsen, &obj,
      &rhs, &sense, &matbeg, &matcnt, &matind,
      &matval, &lb, &ub, &ctype, &qmatbeg, &qmatcnt,
      &qmatind, &qmatval);

   if (status)
   {  fprintf(stderr, "Failed to build problem data arrays.\n");
      goto TERMINATE;
   }

   // Create the problem.
   lp = CPXcreateprob(env, &status, probname);
   if (lp == NULL)
   {  fprintf(stderr, "Failed to create problem.\n");
      goto TERMINATE;
   }

   // Now copy the LP part of the problem data into the lp
   status = CPXcopylp(env, lp, numcols, numrows, objsen, obj, rhs,
      sense, matbeg, matcnt, matind, matval, lb, ub, NULL);

   if (status)
   {  fprintf(stderr, "Failed to copy problem data.\n");
      goto TERMINATE;
   }

   status = CPXcopyquad(env, lp, qmatbeg, qmatcnt, qmatind, qmatval);
   if (status)
   {  fprintf(stderr, "Failed to copy quadratic matrix.\n");
      goto TERMINATE;
   }

   // Copy the ctype array, make problem integer
   if(optimality_target != 2)
   {  status = CPXcopyctype(env, lp, ctype);
      if (status)
      {  fprintf(stderr, "Failed to copy ctype\n");
         goto TERMINATE;
      }
   }

   // Write a copy of the problem to a file, if instance sufficiently small
   if(conf->isverbose && n*m < 100)
      status = CPXwriteprob(env, lp, "qgap.lp", NULL);
      if (status)
      {  fprintf(stderr, "Failed to write LP to disk.\n");
         goto TERMINATE;
      }

   if (optimality_target==4)     // magnifying glass heuristic
   {
      HeuCHH* HCHH = new HeuCHH(this);
      HCHH->goCHH(env,lp,conf->maxnodes, -1,solbest); // always QP opt
      delete HCHH;

      HeuMagnify* HMG;
      HMG = new HeuMagnify(this);
      int inner_optimality_target = 3; // 1 convex. 2 not available for MIP, 3 heu
      HMG->MagniGlass(env,lp,conf->maxnodes, inner_optimality_target,solbest); // always QP opt
      delete HMG;
      goto TERMINATE;
   }
   // 1: assumes that the model is convex and searches for a globally optimal solution.
   // 2: Searches for a solution that satisfies first-order optimality conditions, but is not necessarily globally optimal.
   // 3: Searches for a globally optimal solution to a nonconvex model; changes problem type to MIQP if necessary.
   cout << "Optimality target set to "<< optimality_target << endl;
   status = CPXsetintparam(env, CPXPARAM_OptimalityTarget, optimality_target);
   if (status)
   {  fprintf(stderr, "Failure to reset optimality target, error %d.\n", status);
      goto TERMINATE;
   }
   else
      switch (optimality_target)
      {
         case 1:  cout << "MIP solution, convex function" << endl;
            break;
         case 2:  cout << "Continuous solution, any function" << endl;
            break;
         case 3:  cout << "Trying heuristic MIP solution, any function" << endl;
            break;
         default:
            break;
      }

   // max cplex time: 60 seconds
   //status = CPXsetintparam(env, CPXPARAM_TimeLimit, 60);   // max cpu time, can't make this work
   status = CPXsetintparam(env, CPXPARAM_MIP_Limits_Nodes, conf->maxnodes);   // max num of nodes
   //status = CPXsetintparam(env,    CPXPARAM_DetTimeLimit, 20000);  // max cpu absolute time (ticks), can't make this work
   if (status)
   {  fprintf(stderr, "Failure to reset cpu max time, error %d.\n", status);
      goto TERMINATE;
   }

   // Optimize the problem and obtain solution.
   if(optimality_target > 1)
      status = CPXqpopt(env, lp); // in case of non convex function (opt target > 1)
   else
   {  status = CPXchgprobtype(env, lp, CPXPROB_MIQCP); // in case of convex functions
      status = CPXmipopt(env, lp);                     // in case of convex functions
   }
   if (status)
   {  fprintf(stderr, "Failed to optimize QP.\n");
      //goto TERMINATE;
   }

   cur_numrows = CPXgetnumrows(env, lp);
   cur_numcols = CPXgetnumcols(env, lp);

   int nn = CPXgetsolnpoolnumsolns(env, lp);
   if(nn==0)
   {  fprintf(stderr, "Failed to find feasible solutions.\n");
      goto TERMINATE;
   }
   double mincost = DBL_MAX;
   int minind = -1;
   for(i=0;i<nn;i++)
   {  status = CPXgetsolnpoolobjval (env, lp, i, &objval);
      cout << "Solution " << i << " cost " << std::fixed << objval << endl;
      if(objval<mincost)
      {  minind = i;
         mincost = objval;
      }
   }

   x     = (double *)malloc(cur_numcols * sizeof(double));
   slack = (double *)malloc(cur_numrows * sizeof(double));
   //dj = (double *)malloc(cur_numcols * sizeof(double));
   //pi = (double *)malloc(cur_numrows * sizeof(double));
   status = CPXgetsolnpoolx (env, lp, minind, x, 0, CPXgetnumcols(env, lp)-1);

   status = CPXsolution(env, lp, &solstat, &objval, x, NULL, slack, NULL);
   if (status)
   {  fprintf(stderr, "Failed to obtain solution.\n");
      goto TERMINATE;
   }

   status = checkfeas(x, objval);
   if (status)
   {  fprintf(stderr, "Solution infeasible.\n");
      cout << "Solution infeasible !!! status = " << status << endl;
      //goto TERMINATE;
   }
   else cout << "Solution checked, status " << status << " cost " << std::fixed << objval << endl;

   // Write the output to the screen.
   cout << "\nSolution status = " << solstat << endl;
   cout << "Solution value  = " << std::fixed << objval << endl;
   cout << "Solution:" << endl;
   for(i=0;i<cur_numcols;i++)
      cout << x[i] << " ";
   cout << endl;

TERMINATE:
   // Free up the problem as allocated by CPXcreateprob, if necessary
   if (lp != NULL) {
      status = CPXfreeprob(env, &lp);
      if (status) {
         fprintf(stderr, "CPXfreeprob failed, error code %d.\n", status);
      }
   }

   // Free up the CPLEX environment, if necessary
   if (env != NULL) {
      status = CPXcloseCPLEX(&env);
      if (status) {
         char  errmsg[CPXMESSAGEBUFSIZE];
         fprintf(stderr, "Could not close CPLEX environment.\n");
         CPXgeterrorstring(env, status, errmsg);
         fprintf(stderr, "%s", errmsg);
      }
   }

   // Free up the problem data arrays, if necessary.
   // free_and_null((char **)&probname);  // gives an error, don't know why
   free_and_null((char **)&obj);
   free_and_null((char **)&rhs);
   free_and_null((char **)&sense);
   free_and_null((char **)&matbeg);
   free_and_null((char **)&matcnt);
   free_and_null((char **)&matind);
   free_and_null((char **)&matval);
   free_and_null((char **)&lb);
   free_and_null((char **)&ub);
   free_and_null((char **)&ctype);
   free_and_null((char **)&qmatbeg);
   free_and_null((char **)&qmatcnt);
   free_and_null((char **)&qmatind);
   free_and_null((char **)&qmatval);

   free_and_null((char **)&x);
   free_and_null((char **)&slack);
   //free_and_null((char **)&dj);
   //free_and_null((char **)&pi);
   return (status);
}

int QuadraticGAP::setproblemdata(char **probname_p, int *numcols_p, int *numrows_p,
   int *objsen_p, double **obj_p, double **rhs_p, char **sense_p, int **matbeg_p, int **matcnt_p,
   int **matind_p, double **matval_p, double **lb_p, double **ub_p, char **ctype_p, int **qmatbeg_p, int **qmatcnt_p,
   int **qmatind_p, double **qmatval_p)
{
   int i,j,h,k,ij,hk,idRow,idCol;

   char     *zprobname = NULL;
   double   *zobj = NULL;
   double   *zrhs = NULL;
   char     *zsense = NULL;
   int      *zmatbeg = NULL;
   int      *zmatcnt = NULL;
   int      *zmatind = NULL;
   double   *zmatval = NULL;
   double   *zlb = NULL;
   double   *zzub = NULL;
   char     *zctype = NULL;
   int      *zqmatbeg = NULL;
   int      *zqmatcnt = NULL;
   int      *zqmatind = NULL;
   double   *zqmatval = NULL;
   int      status = 0;

   int numCols = n*m;
   int numRows = n+m;
   int numNZ = n*m+n*m;  // nonzeros in the linear coefficient matrix
   int numQNZ = n*n*m*m; // nonzeros in the quadratic coefficient matrix

   zprobname = (char *)malloc(16 * sizeof(char));
   zobj = (double *)malloc(numCols * sizeof(double));
   zrhs = (double *)malloc(numRows * sizeof(double));
   zsense = (char *)malloc(numRows * sizeof(char));
   zmatbeg = (int *)malloc(numCols * sizeof(int));
   zmatcnt = (int *)malloc(numCols * sizeof(int));
   zmatind = (int *)malloc(numNZ * sizeof(int));
   zmatval = (double *)malloc(numNZ * sizeof(double));
   zlb = (double *)malloc(numCols * sizeof(double));
   zzub = (double *)malloc(numCols * sizeof(double));
   zctype = (char *)malloc(numCols * sizeof(char));
   zqmatbeg = (int *)malloc(numCols * sizeof(int));
   zqmatcnt = (int *)malloc(numCols * sizeof(int));
   zqmatind = (int *)malloc(numQNZ * sizeof(int));
   zqmatval = (double *)malloc(numQNZ * sizeof(double));

   if (zprobname == NULL || zobj == NULL ||
      zrhs == NULL || zsense == NULL ||
      zmatbeg == NULL || zmatcnt == NULL ||
      zmatind == NULL || zmatval == NULL ||
      zlb == NULL || zzub == NULL ||
      zqmatbeg == NULL || zqmatcnt == NULL ||
      zqmatind == NULL || zqmatval == NULL) {
      status = 1;
      goto TERMINATE;
   }

   zprobname = (char*) name.c_str();

   // -------------------------- linear objective costs
   ij = 0;
   for(i=0;i<m;i++)
      for(j=0;j<n;j++)
      {
         zobj[ij] = cl[i][j];
         zlb[ij] = 0;
         zzub[ij] = 1;
         zctype[ij] = 'I';
         ij++;
      }

   // -------------------------- quadratic cost matrix
   ij = 0;
   numNZ = 0;
   for (i = 0; i<m; i++)
      for (j = 0; j<n; j++)
      {
         hk = 0;
         zqmatbeg[ij] = numNZ;
         for (h = 0; h<m; h++)
            for (k = 0; k<n; k++)
            {
               zqmatind[numNZ] = hk;
               if (cqf != NULL)
                  zqmatval[numNZ] = 2 * cqd[i][h] * cqf[j][k]; // d_ih f_jk input format MIND THE 2*
               else
                  zqmatval[numNZ] = 2 * cqd[ij][hk];           // c_ijhk input format    MIND THE 2*
               numNZ++;
               hk++;
            }
         zqmatcnt[ij] = numNZ - zqmatbeg[ij];
         ij++;
      }

   // -------------------------- find eigenvalues
   //double evalue = eigenValues(zqmatval,i*j);
   double evalue = eigenValues(zqmatval,m*n);
   cout << "Eigenvalue: " << evalue << endl;

   // -------------------------- constraints section
   idCol = 0;
   numNZ = 0;
   for (i = 0; i<m; i++)
      for (j = 0; j<n; j++)
      {
         zmatbeg[idCol] = numNZ;

         zmatind[numNZ] = j;           // Assignment constraint
         zmatval[numNZ] = 1.0;
         numNZ++;

         zmatind[numNZ] = n + i;       // Capacity constraint
         zmatval[numNZ] = req[i][j];
         numNZ++;

         zmatcnt[idCol] = numNZ - zmatbeg[idCol];
         idCol++;
      }

   // -------------------------- rhs
   for(j=0;j<n;j++)
   {  zsense[j] = 'E';
      zrhs[j]   = 1.0;
   }

   for(i=0;i<m;i++)
   {  zsense[n+i] = 'L';
      zrhs[n+i]   = cap[i];
   }

TERMINATE:
   if (status)
   {
      free_and_null((char **)&zprobname);
      free_and_null((char **)&zobj);
      free_and_null((char **)&zrhs);
      free_and_null((char **)&zsense);
      free_and_null((char **)&zmatbeg);
      free_and_null((char **)&zmatcnt);
      free_and_null((char **)&zmatind);
      free_and_null((char **)&zmatval);
      free_and_null((char **)&zlb);
      free_and_null((char **)&zzub);
      free_and_null((char **)&zctype);
      free_and_null((char **)&zqmatbeg);
      free_and_null((char **)&zqmatcnt);
      free_and_null((char **)&zqmatind);
      free_and_null((char **)&zqmatval);
   }
   else
   {
      *numcols_p = numCols;
      *numrows_p = numRows;
      *objsen_p = CPX_MIN;

      *probname_p = zprobname;
      *obj_p = zobj;
      *rhs_p = zrhs;
      *sense_p = zsense;
      *matbeg_p = zmatbeg;
      *matcnt_p = zmatcnt;
      *matind_p = zmatind;
      *matval_p = zmatval;
      *lb_p = zlb;
      *ub_p = zzub;
      *ctype_p = zctype;
      *qmatbeg_p = zqmatbeg;
      *qmatcnt_p = zqmatcnt;
      *qmatind_p = zqmatind;
      *qmatval_p = zqmatval;
   }
   return (status);

}  // END setproblemdata

double QuadraticGAP::eigenValues(double *qmatval, int n)
{  int i,j,k,sumMat;

   Eigen::VectorXd evalues;

   Eigen::MatrixXd mat = Eigen::MatrixXd(n, n);
   k = sumMat = 0;
   for(i=0;i<n;i++)
      for(j=0;j<n;j++)
      {  mat(i,j) = qmatval[k];
         sumMat += mat(i,j);
         k++;
      }

   cout << "Computing determinant" << endl;
   double det = mat.determinant();
   cout << "Determinant: " << det << endl;

   if (det >= DBL_MIN)
      cout << "NOT computing eigenvalues" << endl;
   else
   {
      cout << "Computing eigenvalues" << endl;
      Eigen::MatrixXd M = mat + mat.transpose();
      // Construct matrix operation object
      DenseSymMatProd<double> op(M);

      /*
      Construct eigen solver object, requesting the smallest / largest eigenvalues SMALLEST_MAGN / LARGEST_ALGE
      Parameters
      op_  Pointer to the matrix operation object, which should implement the matrix - vector multiplication operation of A: calculating Av for any vector v. Users could either create the object from the wrapper class such as DenseSymMatProd, or define their own that implements all the public member functions as in DenseSymMatProd.
      nev_ Number of eigenvalues requested. This should satisfy 1<=nev<=n-1, where n is the size of matrix.
      ncv_ Parameter that controls the convergence speed of the algorithm. Typically a larger ncv_ means faster convergence, but it may also result in greater memory use and more matrix operations in each iteration.This parameter must satisfy nev<ncv<=n, and is advised to take ncv>=2nev.
      */
      SymEigsSolver< DenseSymMatProd<double> > eigs(op, 1, n);

      //if (abs(det) > sumMat) goto lend;

      // Initialize and compute
      eigs.init();
      int nconv = eigs.compute(SortRule::SmallestAlge);
      // Retrieve results
      if (eigs.info() == CompInfo::Successful)
         evalues = eigs.eigenvalues();
      std::cout << "Smallest eigenvalues:" << fixed << evalues << std::endl;

      for (i = 0; i < n; i++)
         for (j = 0; j < n; j++)
            if (i == j)
               if (evalues[0] < 0 && optimality_target == 1)
                  qmatval[i*n + j] -= evalues[0];  // convexification
               else
                  qmatval[i*n + j] *= 2;  // all other coefficients will be doubled, CPLEX uses only upper triangular

      cout << "Eigenvalues completed" << endl;
   }
lend:
   return det;
}

// This simple routine frees up the pointer *ptr, and sets *ptr to NULL
void free_and_null(char **ptr)
{
   if (*ptr != NULL) {
      free(*ptr);
      *ptr = NULL;
   }
} // END free_and_null

// checks the feasibility of a given solution
int QuadraticGAP::checkfeas(double* x, double solcost)
{  int res = 0;  // solution ok
   int i, j;
   double cost = 0;
   double* capused = new double[m];
   vector<int> sol(n);
   bool isInteger = true;  // have got an integer solution

   for (i = 0; i<m; i++)
   {  capused[i] = 0;
      for(j=0;j<n;j++)
         if(abs(x[i*n+j]-trunc(x[i*n+j]) > EPS) )
            isInteger = false;
   }

   if(isInteger)
      for(j=0;j<n;j++)
      {  sol[j] = -1;
         for(i=0;i<m;i++)
            if(x[i*n+j] > EPS)
               if(sol[j] > EPS)
               {  res = 1;       // multiple assignment of some client
                  goto lend;
               }
               else
                  sol[j] = i;
      }

   // controllo assegnamenti
   if(isInteger)
      for (j = 0; j<n; j++)
         if (sol[j]<0 || sol[j] >= m)
         {  res = 2;       // client assignment to a non-server
            goto lend;
         }

   // controllo capacita'
   for (j = 0; j<n; j++)
   {  for(i=0;i<m;i++)
      {  capused[i] += x[i*n+j]*req[i][j];
         if (capused[i] > cap[i])
         {  res = 3;       // capacity exceeded
            goto lend;
         }
      }
   }
   delete capused;

   // check cost
   for(i=0;i<m;i++)
      for(j=0;j<n;j++)
      {  cost += cl[i][j]*x[i*n+j];

         for(int h=0;h<m;h++)
            for(int k=0;k<n;k++)
               cost += cqd[i][h]*cqf[j][k]*x[i*n+j]*x[h*n+k];
      }

   if (abs(solcost - cost) > EPS)
   {  res = 4;       // unaligned costs
      cout << "Solcost = " << solcost << " cost = " << cost << endl;
      goto lend;
   }

lend:
   return res;
}

// checks the feasibility of a given solution
int QuadraticGAP::checkfeas(int* sol, double solcost)
{  int res = 0;  // solution ok
   int i, j;
   double cost,costlin,costq;
   vector<double> capused(m);
   bool isInteger = true;  // have got an integer solution

   cost=costlin=costq = 0;
   // controllo assegnamenti
   if(isInteger)
      for (j = 0; j<n; j++)
         if (sol[j]<0 || sol[j] >= m)
         {  res = 2;       // client assignment to a non-server
            goto lend;
         }

   // controllo capacita'
   for (j = 0; j<n; j++)
   {  capused[sol[j]] += req[sol[j]][j];
      for(i=0;i<m;i++)
      if (capused[i] > cap[i])
      {  res = 3;       // capacity exceeded
         goto lend;
      }
   }

   // check cost
   for(j=0;j<n;j++)
   {  costlin += cl[sol[j]][j];

      for(int k=0;k<n;k++)
         costq += cqd[sol[j]][sol[k]]*cqf[j][k];
   }
   cost = costlin+costq;
   cout << "costlin = " << costlin << " costq = " << costq << " cost = " << cost << endl;

   if (abs(solcost - cost) > EPS)
   {  res = 4;       // unaligned costs
      cout << "Solcost = " << solcost << " cost = " << cost << endl;
      goto lend;
   }

lend:
   if(res==0)
      for(j=0;j<n;j++)
         cout << solbest[j] << " ";
   cout << " <== solbest" << endl;
   return res;
}

// Gilmore-Lawler-like costs
int QuadraticGAP::GLcosts()
{
   int i,j,h,k,z;
   MTHG* MT = new MTHG(this,z);
   MT->QGAP = this;
   vector<vector<int>> zlin;
   zlin.resize( m , vector<int>( n, 0 ) );
   zgl.resize(m, vector<int>(n, 0));

   for(i=0;i<m;i++)
      for(j=0;j<n;j++)
      {
         for (h = 0; h<m; h++)
            for (k = 0; k<n; k++)
               if(k!=j)
               {  zlin[h][k] = cqd[i][h] * cqf[j][k];
                  if(zlin[h][k] == 0)
                     zlin[h][k] = 1;   // well, it won't be a bound
               }
               else
                  zlin[h][k] = (h==i && k==j ? 1 : 1000000);
         zgl[i][j] = MT->run_mthg(zlin);     // WAtCHOUT! need to be integer costs
         zgl[i][j] += cl[i][j];
      }

   delete MT;
   return 0;
}

// saving best solution so far
void QuadraticGAP::saveZUB(int* sol, double solcost)
{
   int j,res;

   res = checkfeas(sol, solcost);
   if(res==0)
   {  zub = solcost;
      for(j=0;j<n;j++) solbest[j] = sol[j];
   }
   else
      cout << "[saveZUB] Detected one arror" << endl;
}

void QuadraticGAP::saveZUB(double* x, double solcost)
{
   int i,j,res;
   vector<int> sol(n);

   res = checkfeas(x, solcost);
   if(res==0)
   {
      for(j=0;j<n;j++)
      {  sol[j] = -1;
         for(i=0;i<m;i++)
            if(x[i*n+j] > EPS)
                sol[j] = i;
      }

      res = checkfeas(&sol[0], solcost);
      if(res==0)
      {  zub = solcost;
         for(j=0;j<n;j++) solbest[j] = sol[j];
      }
      else
         cout << "[saveZUB] Detected one arror" << endl;
   }
   else
      cout << "[saveZUB] Detected one arror" << endl;
}


// Copyright John Maddock 2006.

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

#include <boost/math/bindings/rr.hpp>
//#include <boost/math/tools/dual_precision.hpp>
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/tr1/random.hpp>

float extern_val;
// confuse the compilers optimiser, and force a truncation to float precision:
float truncate_to_float(float const * pf)
{
   extern_val = *pf;
   return *pf;
}

std::tr1::tuple<boost::math::ntl::RR, boost::math::ntl::RR> generate_data(boost::math::ntl::RR n, boost::math::ntl::RR phi)
{
   static std::tr1::mt19937 r;
   std::tr1::uniform_real<float> ui(0, 1);
   float k = ui(r);
   boost::math::ntl::RR kr(truncate_to_float(&k));
   boost::math::ntl::RR result = boost::math::ellint_3(kr, n, phi);
   return std::tr1::make_tuple(kr, result);
}

int test_main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   boost::math::ntl::RR::SetOutputPrecision(50);
   boost::math::ntl::RR::SetPrecision(1000);

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      if(0 == get_user_parameter_info(arg1, "n"))
         return 1;
      if(0 == get_user_parameter_info(arg2, "phi"))
         return 1;

      data.insert(&generate_data, arg1, arg2);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=ellint_pi3_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "ellint_pi3_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific;
   write_code(ofs, data, line.c_str());

   return 0;
}


#include <algorithm>
#include <boost/program_options.hpp>
#include <iostream>
#include <set>
#include <tuple>
#include <vector>

#include "numberplate_exprs.hpp"
namespace bpo = boost::program_options;

int main(int argc, char **argv) {
    bpo::options_description opt("option");
    opt.add_options()("help,h", "show this help")("nums,n", bpo::value<std::vector<int>>()->multitoken(),
                                                  "space separated numberplate,required")(
        "dest,d", bpo::value<int>()->default_value(10), "destination value")(
        "all_permutation,p", "try all possible permutation")("only-num,o", "output only the number of solutions found");

    bpo::variables_map varmap;
    bpo::store(bpo::parse_command_line(argc, argv, opt), varmap);
    bpo::notify(varmap);
    if (varmap.count("help")) {
        std::cout << opt << std::endl;
        std::exit(EXIT_SUCCESS);
    }
    if (!varmap.count("nums")) {
        std::cerr << "error: too few arguments" << std::endl;
        std::cerr << opt << std::endl;
        std::exit(EXIT_FAILURE);
    }
    auto np = varmap["nums"].as<std::vector<int>>();
    if (np.size() != 4) {
        std::cerr << "error: numberplate must be 4 digits" << std::endl;
        std::exit(EXIT_FAILURE);
    }
    auto destination = varmap["dest"].as<int>();
    std::set<std::string> ok_pretties;
    if (!varmap.count("all_permutation")) {
        for (const auto expr : numberplate::EXPRS) {
            if (expr(np[0], np[1], np[2], np[3]) == destination) {
                ok_pretties.insert(expr.pretty(np[0], np[1], np[2], np[3]));
            }
        }
    } else {
        std::vector<int> idxs_replaced = {0, 1, 2, 3};
        do {
            std::vector<int> np_replaced(4, 0);
            for (auto i = 0; i < 4; i++) {
                np_replaced[i] = np[idxs_replaced[i]];
            }
            for (const auto expr : numberplate::EXPRS) {
                if (expr(np_replaced[0], np_replaced[1], np_replaced[2], np_replaced[3]) == destination) {
                    ok_pretties.insert(expr.pretty(np_replaced[0], np_replaced[1], np_replaced[2], np_replaced[3]));
                }
            }
        } while (std::next_permutation(idxs_replaced.begin(), idxs_replaced.end()));
    }
    std::cout << ok_pretties.size() << " solutions found" << std::endl;
    if (!varmap.count("only-num")) {
        for (const auto ok_pretty : ok_pretties) {
            std::cout << ok_pretty << std::endl;
        }
    }
    return 0;
}


#ifndef FVMSCALAR1D_CFL_CONDITION_HPP
#define FVMSCALAR1D_CFL_CONDITION_HPP

#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>

/// Interface for computing CFL restricted timesteps.
class CFLCondition {
  public:
    virtual ~CFLCondition() = default;

    /// Compute the largest time-step satisfying the CFL condition.
    virtual double operator()(const Eigen::VectorXd &u) const = 0;
};

//// ANCSE_CUT_START_TEMPLATE
//----------------StandardCFLConditionDeclBegin----------------
/// Compute the CFL condition for generic models.
/** Compute the maximum eigenvalue given all the data, and compute the
 *  CFL restricted time-step accordingly.
 *
 *  Note: this requires looping over the entire grid, and computing a reduction,
 *        i.e. max.
 */
class StandardCFLCondition : public CFLCondition {
  public:
    StandardCFLCondition(const Grid &grid,
                         const Model &model,
                         double cfl_number);

    virtual double operator()(const Eigen::VectorXd &u) const override;

  private:
    Grid grid;
    Model model;
    double cfl_number;
};
//----------------StandardCFLConditionDeclEnd----------------
//// ANCSE_END_TEMPLATE

std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid, const Model &model, double cfl_number);

#endif // FVMSCALAR1D_CFL_CONDITION_HPP


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    using namespace literals;

    BOOST_HANA_CONSTANT_ASSERT(1234_c == llong<1234>);
    BOOST_HANA_CONSTANT_ASSERT(-1234_c == llong<-1234>);
    BOOST_HANA_CONSTANT_ASSERT(sum(list(1_c, 2_c, 3_c, 4_c)) == 10_c);
    //! [main]
}