// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace gn_sinu
    {

            static const double epsilon10 = 1e-10;
            static const int max_iter = 8;
            static const double loop_tol = 1e-7;

            template <typename T>
            struct par_gn_sinu
            {
                detail::en<T> en;
                T    m, n, C_x, C_y;
            };

            /* Ellipsoidal Sinusoidal only */

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_gn_sinu_ellipsoid
                : public base_t_fi<base_gn_sinu_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_gn_sinu<T> m_proj_parm;

                inline base_gn_sinu_ellipsoid(const Parameters& par)
                    : base_t_fi<base_gn_sinu_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T s, c;

                    xy_y = pj_mlfn(lp_lat, s = sin(lp_lat), c = cos(lp_lat), this->m_proj_parm.en);
                    xy_x = lp_lon * c / sqrt(1. - this->m_par.es * s * s);
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T s;

                    if ((s = fabs(lp_lat = pj_inv_mlfn(xy_y, this->m_par.es, this->m_proj_parm.en))) < half_pi) {
                        s = sin(lp_lat);
                        lp_lon = xy_x * sqrt(1. - this->m_par.es * s * s) / cos(lp_lat);
                    } else if ((s - epsilon10) < half_pi)
                        lp_lon = 0.;
                    else
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                }
                /* General spherical sinusoidals */

                static inline std::string get_name()
                {
                    return "gn_sinu_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_gn_sinu_spheroid
                : public base_t_fi<base_gn_sinu_spheroid<T, Parameters>, T, Parameters>
            {
                par_gn_sinu<T> m_proj_parm;

                inline base_gn_sinu_spheroid(const Parameters& par)
                    : base_t_fi<base_gn_sinu_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    if (this->m_proj_parm.m == 0.0)
                        lp_lat = this->m_proj_parm.n != 1. ? aasin(this->m_proj_parm.n * sin(lp_lat)): lp_lat;
                    else {
                        T k, V;
                        int i;

                        k = this->m_proj_parm.n * sin(lp_lat);
                        for (i = max_iter; i ; --i) {
                            lp_lat -= V = (this->m_proj_parm.m * lp_lat + sin(lp_lat) - k) /
                                (this->m_proj_parm.m + cos(lp_lat));
                            if (fabs(V) < loop_tol)
                                break;
                        }
                        if (!i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                    }
                    xy_x = this->m_proj_parm.C_x * lp_lon * (this->m_proj_parm.m + cos(lp_lat));
                    xy_y = this->m_proj_parm.C_y * lp_lat;
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    xy_y /= this->m_proj_parm.C_y;
                    lp_lat = (this->m_proj_parm.m != 0.0) ? aasin((this->m_proj_parm.m * xy_y + sin(xy_y)) / this->m_proj_parm.n) :
                        ( this->m_proj_parm.n != 1. ? aasin(sin(xy_y) / this->m_proj_parm.n) : xy_y );
                    lp_lon = xy_x / (this->m_proj_parm.C_x * (this->m_proj_parm.m + cos(xy_y)));
                }

                static inline std::string get_name()
                {
                    return "gn_sinu_spheroid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                par.es = 0;

                proj_parm.C_x = (proj_parm.C_y = sqrt((proj_parm.m + 1.) / proj_parm.n))/(proj_parm.m + 1.);
            }

            // General Sinusoidal Series
            template <typename Params, typename Parameters, typename T>
            inline void setup_gn_sinu(Params const& params, Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                if (pj_param_f<srs::spar::n>(params, "n", srs::dpar::n, proj_parm.n)
                 && pj_param_f<srs::spar::m>(params, "m", srs::dpar::m, proj_parm.m)) {
                    if (proj_parm.n <= 0 || proj_parm.m < 0)
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_m_or_n) );
                } else
                    BOOST_THROW_EXCEPTION( projection_exception(error_invalid_m_or_n) );

                setup(par, proj_parm);
            }

            // Sinusoidal (Sanson-Flamsteed)
            template <typename Parameters, typename T>
            inline void setup_sinu(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.en = pj_enfn<T>(par.es);

                if (par.es != 0.0) {
                    /* empty */
                } else {
                    proj_parm.n = 1.;
                    proj_parm.m = 0.;
                    setup(par, proj_parm);
                }
            }

            // Eckert VI
            template <typename Parameters, typename T>
            inline void setup_eck6(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.m = 1.;
                proj_parm.n = 2.570796326794896619231321691;
                setup(par, proj_parm);
            }

            // McBryde-Thomas Flat-Polar Sinusoidal
            template <typename Parameters, typename T>
            inline void setup_mbtfps(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.m = 0.5;
                proj_parm.n = 1.785398163397448309615660845;
                setup(par, proj_parm);
            }

    }} // namespace detail::gn_sinu
    #endif // doxygen

    /*!
        \brief General Sinusoidal Series projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Projection parameters
         - m (real)
         - n (real)
        \par Example
        \image html ex_gn_sinu.gif
    */
    template <typename T, typename Parameters>
    struct gn_sinu_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline gn_sinu_spheroid(Params const& params, Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_gn_sinu(params, this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Sinusoidal (Sanson-Flamsteed) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_sinu.gif
    */
    template <typename T, typename Parameters>
    struct sinu_ellipsoid : public detail::gn_sinu::base_gn_sinu_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline sinu_ellipsoid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_ellipsoid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_sinu(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Sinusoidal (Sanson-Flamsteed) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_sinu.gif
    */
    template <typename T, typename Parameters>
    struct sinu_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline sinu_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_sinu(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Eckert VI projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_eck6.gif
    */
    template <typename T, typename Parameters>
    struct eck6_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline eck6_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_eck6(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief McBryde-Thomas Flat-Polar Sinusoidal projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_mbtfps.gif
    */
    template <typename T, typename Parameters>
    struct mbtfps_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline mbtfps_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_mbtfps(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_gn_sinu, gn_sinu_spheroid, gn_sinu_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_sinu, sinu_spheroid, sinu_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_eck6, eck6_spheroid, eck6_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_mbtfps, mbtfps_spheroid, mbtfps_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(gn_sinu_entry, gn_sinu_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(sinu_entry, sinu_spheroid, sinu_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(eck6_entry, eck6_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(mbtfps_entry, mbtfps_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(gn_sinu_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(gn_sinu, gn_sinu_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(sinu, sinu_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(eck6, eck6_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(mbtfps, mbtfps_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP


// Copyright (c) 2017 Shota SUGIHARA
// Distributed under the MIT License.
#include "bubble_particles.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <Eigen/LU>

namespace my_mps{

BubbleParticles::BubbleParticles(const std::string& path, const tiny_mps::Condition& condition)
    : Particles(path, condition) {
  init_bubble_radius = cbrt((3 * condition.initial_void_fraction) / (4 * M_PI * condition.bubble_density * (1 - condition.initial_void_fraction)));
  average_pressure = Eigen::VectorXd::Zero(getSize());
  normal_vector = Eigen::Matrix3Xd::Zero(3, getSize());
  modified_pnd = Eigen::VectorXd::Zero(getSize());
  bubble_radius = Eigen::VectorXd::Constant(getSize(), init_bubble_radius);
  void_fraction = Eigen::VectorXd::Constant(getSize(), condition.initial_void_fraction);
  free_surface_type = Eigen::VectorXi::Zero(getSize());
  average_count = 0;

}

bool BubbleParticles::nextLoop(const std::string& path, tiny_mps::Timer& timer) {
  std::cout << std::endl;
  timer.limitCurrentDeltaTime(getMaxSpeed(), condition_);
  timer.printCompuationTime();
  timer.printTimeInfo();
  showParticlesInfo();
  std::cout << boost::format("Max velocity: %f") % getMaxSpeed() << std::endl;
  saveInterval(path, timer);
  if (checkNeedlessCalculation()) {
    std::cerr << "Error: All particles have become ghost." << std::endl;
    writeVtkFile(path + "err.vtk", (boost::format("Time: %s") % timer.getCurrentTime()).str());
    throw std::range_error("Error: All particles have become ghost.");
  }
  if (timer.isUnderMinDeltaTime()) {
    std::cerr << "Error: Delta time has become so small." << std::endl;
    writeVtkFile(path + "err.vtk", (boost::format("Time: %s") % timer.getCurrentTime()).str());
    throw std::range_error("Error: Delta time has become so small.");
  }
  if (!timer.hasNextLoop()) {
    std::cout << std::endl << "Total ";
    timer.printCompuationTime();
    std::cout << "Succeed in simulation." << std::endl;
    return false;
  }
  timer.update();
  temporary_velocity = velocity;
  temporary_position = position;
  return true;
}

bool BubbleParticles::saveInterval(const std::string& path, const tiny_mps::Timer& timer) const {
  if (!timer.isOutputTime()) return false;
  std::string output_index = (boost::format("%04d") % timer.getOutputCount()).str();
  writeVtkFile((boost::format(path + "output_%1%.vtk") % output_index).str(), (boost::format("Time: %s") % timer.getCurrentTime()).str());
  writeGridVtkFile((boost::format(path + "grid_%1%.vtk") % output_index).str(), (boost::format("Time: %s") % timer.getCurrentTime()).str());
  return true;
}

void BubbleParticles::writeVtkFile(const std::string& path, const std::string& title) const {
  int size = getSize();
  std::ofstream ofs(path);
  if(ofs.fail()) {
    std::cerr << "Error: in writeVtkFile() in particles.cpp." << std::endl;
    throw std::ios_base::failure("Error: in writeVtkFile() in particles.cpp.");
  }
  ofs << "# vtk DataFile Version 2.0" << std::endl;
  ofs << title << std::endl;
  ofs << "ASCII" << std::endl;
  ofs << "DATASET UNSTRUCTURED_GRID" << std::endl;
  ofs << std::endl;
  ofs << "POINTS " << size << " double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << position(0, i) << " " << position(1, i) << " " << position(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "CELLS " << size << " " << size * 2 << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << 1 << " " << i << std::endl;
  }
  ofs << std::endl;
  ofs << "CELL_TYPES " << size << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << 1 << std::endl;
  }
  ofs << std::endl;
  ofs << "POINT_DATA " << size << std::endl;
  ofs << "SCALARS Pressure double" << std::endl;
  ofs << "LOOKUP_TABLE Pressure" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << pressure(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "VECTORS Velocity double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << velocity(0, i) << " " << velocity(1, i) << " " << velocity(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS Type int" << std::endl;
  ofs << "LOOKUP_TABLE Type" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << particle_types(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS ParticleNumberDensity double" << std::endl;
  ofs << "LOOKUP_TABLE ParticleNumberDensity" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << particle_number_density(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS NeighborParticles int" << std::endl;
  ofs << "LOOKUP_TABLE NeighborParticles" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << neighbor_particles(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS BoundaryCondition int" << std::endl;
  ofs << "LOOKUP_TABLE BoundaryCondition" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << boundary_types(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "VECTORS CorrectionVelocity double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << correction_velocity(0, i) << " " << correction_velocity(1, i) << " " << correction_velocity(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS SourceTerm double" << std::endl;
  ofs << "LOOKUP_TABLE SourceTerm" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << source_term(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS VoxelsRatio double" << std::endl;
  ofs << "LOOKUP_TABLE VoxelsRatio" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << voxel_ratio(i) << std::endl;
  }

  // extended
  ofs << std::endl;
  ofs << "SCALARS AveragePressure double" << std::endl;
  ofs << "LOOKUP_TABLE AveragePressure" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << average_pressure(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "VECTORS NormalVector double" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << normal_vector(0, i) << " " << normal_vector(1, i) << " " << normal_vector(2, i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS BubbleRadius double" << std::endl;
  ofs << "LOOKUP_TABLE BubbleRadius" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << bubble_radius(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS VoidFraction double" << std::endl;
  ofs << "LOOKUP_TABLE VoidFraction" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << void_fraction(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS FreeSurfaceType int" << std::endl;
  ofs << "LOOKUP_TABLE FreeSurfaceType" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << free_surface_type(i) << std::endl;
  }
  ofs << std::endl;
  ofs << "SCALARS ModifiedParticleNumberDensity double" << std::endl;
  ofs << "LOOKUP_TABLE ModifiedParticleNumberDensity" << std::endl;
  for(int i = 0; i < size; ++i) {
    ofs << modified_pnd(i) << std::endl;
  }

  std::cout << "Succeed in writing vtk file: " << path << std::endl;
}

void BubbleParticles::writeGridVtkFile(const std::string& path, const std::string& title) const {
  std::ofstream ofs(path);
  if(ofs.fail()) {
    std::cerr << "Error: in writeGridVtkFile() in particles.cpp." << std::endl;
    throw std::ios_base::failure("Error: in writeGridVtkFile() in particles.cpp.");
  }
  ofs << "# vtk DataFile Version 2.0" << std::endl;
  ofs << title << std::endl;
  ofs << "ASCII" << std::endl;
  ofs << "DATASET STRUCTURED_POINTS" << std::endl;
  ofs << "DIMENSIONS " << grid_w << " " << grid_h << " 1" << std::endl;
  ofs << "ORIGIN " << grid_min_pos(0) << " " << grid_min_pos(1) <<  " 0.0" << std::endl;
  ofs << "SPACING " << condition_.average_distance << " " << condition_.average_distance << " " << condition_.average_distance << std::endl;
  ofs << "POINT_DATA " << grid_w * grid_h << std::endl;
  ofs << "SCALARS Pressure double" << std::endl;
  ofs << "LOOKUP_TABLE Pressure" << std::endl;
  for(int i = 0; i < grid_w * grid_h; ++i) {
    ofs << average_grid[i] << std::endl;
  }
  ofs << std::endl;

  std::cout << "Succeed in writing grid vtk file: " << path << std::endl;
}

void BubbleParticles::extendStorage(int extra_size) {
  int size = getSize();
  Particles::extendStorage(extra_size);
  average_pressure.conservativeResize(size + extra_size);
  average_pressure.segment(size, extra_size).setZero();
  normal_vector.conservativeResize(3, size + extra_size);
  normal_vector.block(0, size, 3, extra_size).setZero();
  modified_pnd.conservativeResize(size + extra_size);
  modified_pnd.segment(size, extra_size).setZero();
  bubble_radius.conservativeResize(size + extra_size);
  bubble_radius.segment(size, extra_size).setZero();
  void_fraction.conservativeResize(size + extra_size);
  void_fraction.segment(size, extra_size).setZero();
  free_surface_type.conservativeResize(size + extra_size);
  free_surface_type.segment(size, extra_size).setZero();
}

void BubbleParticles::setGhostParticle(int index) {
  Particles::setGhostParticle(index);
  average_pressure(index) = 0.0;
  normal_vector.col(index).setZero();
  modified_pnd(index) = 0.0;
  bubble_radius(index) = 0.0;
  void_fraction(index) = condition_.initial_void_fraction;
  free_surface_type(index) = SurfaceLayer::OTHERS;
}

void BubbleParticles::checkSurface(){
  // First step.
  using namespace tiny_mps;
  for(int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (particle_types(i_particle) == ParticleType::NORMAL || particle_types(i_particle) == ParticleType::WALL
        || particle_types(i_particle) == ParticleType::INFLOW) {
      if (particle_number_density(i_particle) < condition_.surface_threshold_pnd * initial_particle_number_density
              && neighbor_particles(i_particle) < condition_.surface_threshold_number * initial_neighbor_particles) {
              // && temporary_position(1, i) < shift) {
        boundary_types(i_particle) = BoundaryType::SURFACE;
        free_surface_type(i_particle) = SurfaceLayer::OUTER_SURFACE;
      } else {
        boundary_types(i_particle) = BoundaryType::INNER;
        free_surface_type(i_particle) = SurfaceLayer::INNER;
      }
    } else {
      boundary_types(i_particle) = BoundaryType::OTHERS;
      free_surface_type(i_particle) = SurfaceLayer::OTHERS;
    }
  }
  // Second step.
  Grid grid(condition_.pnd_weight_radius, temporary_position, particle_types.array() != ParticleType::GHOST, dimension);
  const double root2 = std::sqrt(2);
  normal_vector.setZero();
  for(int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if(boundary_types(i_particle) == BoundaryType::SURFACE) {
      Grid::Neighbors neighbors;
      grid.getNeighbors(i_particle, neighbors);
      if (neighbors.empty()) continue;
      for (int j_particle : neighbors) {
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        normal_vector.col(i_particle) += r_ij.normalized() * weightForParticleNumberDensity(r_ij);
      }
      normal_vector.col(i_particle) /= particle_number_density(i_particle);
      for (int j_particle : neighbors) {
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        if (r_ij.norm() >= root2 * condition_.average_distance
            && (temporary_position.col(i_particle) + condition_.average_distance * normal_vector.col(i_particle).normalized() - temporary_position.col(j_particle)).norm() < condition_.average_distance) {
          boundary_types(i_particle) = BoundaryType::INNER;
          free_surface_type(i_particle) = SurfaceLayer::INNER;
          break;
        }
        if (r_ij.norm() < root2 * condition_.average_distance
            && r_ij.normalized().dot(normal_vector.col(i_particle).normalized()) > 1.0 / root2) {
          boundary_types(i_particle) = BoundaryType::INNER;
          free_surface_type(i_particle) = SurfaceLayer::INNER;
          break;
        }
      }
    }
  }
  // Third step.
  Grid judge_inner_surface(condition_.average_distance * condition_.secondary_surface_eta, temporary_position, particle_types.array() != ParticleType::GHOST, dimension);
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      Grid::Neighbors neighbors;
      judge_inner_surface.getNeighbors(i_particle, neighbors);
      if (neighbors.empty()) continue;
      for (int j_particle : neighbors) {
        if (boundary_types(j_particle) == BoundaryType::INNER && free_surface_type(j_particle) == SurfaceLayer::INNER)
          free_surface_type(j_particle) = SurfaceLayer::INNER_SURFACE;
      }
    }
  }
}

void BubbleParticles::checkSurface2(){
  // First step.
  using namespace tiny_mps;
  for(int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (particle_types(i_particle) == ParticleType::NORMAL || particle_types(i_particle) == ParticleType::WALL
        || particle_types(i_particle) == ParticleType::INFLOW) {
      if (particle_number_density(i_particle) < condition_.surface_threshold_pnd * initial_particle_number_density
              && neighbor_particles(i_particle) < condition_.surface_threshold_number * initial_neighbor_particles
              && temporary_position(1, i_particle) < condition_.pnd_weight_radius) {
        boundary_types(i_particle) = BoundaryType::SURFACE;
        free_surface_type(i_particle) = SurfaceLayer::OUTER_SURFACE;
      } else {
        boundary_types(i_particle) = BoundaryType::INNER;
        free_surface_type(i_particle) = SurfaceLayer::INNER;
      }
    } else {
      boundary_types(i_particle) = BoundaryType::OTHERS;
      free_surface_type(i_particle) = SurfaceLayer::OTHERS;
    }
  }
  // Second step.
  Grid grid(condition_.pnd_weight_radius, temporary_position, particle_types.array() != ParticleType::GHOST, dimension);
  normal_vector.setZero();
  for(int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if(boundary_types(i_particle) == BoundaryType::SURFACE) {
      Grid::Neighbors neighbors;
      grid.getNeighbors(i_particle, neighbors);
      if (neighbors.empty()) continue;
      for (int j_particle : neighbors) {
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        normal_vector.col(i_particle) += r_ij.normalized() * weightForParticleNumberDensity(r_ij);
      }
      normal_vector.col(i_particle) /= particle_number_density(i_particle);
    }
  }
  // Third step.
  Grid judge_inner_surface(condition_.average_distance * condition_.secondary_surface_eta, temporary_position, particle_types.array() != ParticleType::GHOST, dimension);
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      Grid::Neighbors neighbors;
      judge_inner_surface.getNeighbors(i_particle, neighbors);
      if (neighbors.empty()) continue;
      for (int j_particle : neighbors) {
        if (boundary_types(j_particle) == BoundaryType::INNER && free_surface_type(j_particle) == SurfaceLayer::INNER)
          free_surface_type(j_particle) = SurfaceLayer::INNER_SURFACE;
      }
    }
  }
}

void BubbleParticles::calculateBubbles() {
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (particle_types(i_particle) == tiny_mps::ParticleType::NORMAL) {
      double del_p = (condition_.vapor_pressure - condition_.head_pressure) - pressure(i_particle);
      // double del_p = (condition_.vapor_pressure - condition_.head_pressure) - average_pressure(i_particle);
      if (del_p > 0) bubble_radius(i_particle) += sqrt(2 * abs(del_p) / (3 * condition_.mass_density));
      else bubble_radius(i_particle) -= sqrt(2 * abs(del_p) / (3 * condition_.mass_density));
      if (bubble_radius(i_particle) > condition_.average_distance) bubble_radius(i_particle) = condition_.average_distance;
      if (bubble_radius(i_particle) < 0) bubble_radius(i_particle) = 0;

      double bubble_vol = 4 * M_PI * condition_.bubble_density * bubble_radius(i_particle) * bubble_radius(i_particle) * bubble_radius(i_particle) / 3;
      void_fraction(i_particle) = bubble_vol / (1 + bubble_vol);
      if (void_fraction(i_particle) < condition_.min_void_fraction) void_fraction(i_particle) = condition_.min_void_fraction;
      if (void_fraction(i_particle) > 0.5) void_fraction(i_particle) = 0.5;
    }
  }
}

void BubbleParticles::calculateBubblesFromAveragePressure() {
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (particle_types(i_particle) == tiny_mps::ParticleType::NORMAL) {
      int ix = std::floor((temporary_position(0, i_particle) - grid_min_pos(0) + condition_.average_distance / 2.0) / condition_.average_distance);
      int iy = std::floor((temporary_position(1, i_particle) - grid_min_pos(1) + condition_.average_distance / 2.0) / condition_.average_distance);
      if (ix < 0 || ix >= grid_w) continue;
      if (iy < 0 || iy >= grid_h) continue;
      // double del_p = (condition_.vapor_pressure - condition_.head_pressure) - pressure(i_particle);
      double del_p = (condition_.vapor_pressure - condition_.head_pressure) - average_grid[ix + iy * grid_w];
      if (del_p > 0) bubble_radius(i_particle) += sqrt(2 * abs(del_p) / (3 * condition_.mass_density));
      else bubble_radius(i_particle) -= sqrt(2 * abs(del_p) / (3 * condition_.mass_density));
      if (bubble_radius(i_particle) > condition_.average_distance) bubble_radius(i_particle) = condition_.average_distance;
      if (bubble_radius(i_particle) < 0) bubble_radius(i_particle) = 0;

      double bubble_vol = 4 * M_PI * condition_.bubble_density * bubble_radius(i_particle) * bubble_radius(i_particle) * bubble_radius(i_particle) / 3;
      void_fraction(i_particle) = bubble_vol / (1 + bubble_vol);
      if (void_fraction(i_particle) < condition_.min_void_fraction) void_fraction(i_particle) = condition_.min_void_fraction;
      if (void_fraction(i_particle) > 0.5) void_fraction(i_particle) = 0.5;
    }
  }
}

void BubbleParticles::calculateAveragePressure() {
  using namespace tiny_mps;
  Grid grid(condition_.pnd_weight_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      average_pressure(i_particle) = 0.0;
      continue;
    }
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    if (neighbors.empty()) {
      average_pressure(i_particle) = pressure(i_particle);
      continue;
    }
    double numerator = pressure(i_particle) * weightPoly6Kernel(0, condition_.pnd_weight_radius);
    double denominator = weightPoly6Kernel(0, condition_.pnd_weight_radius);
    for (int j_particle : neighbors) {
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      numerator += pressure(j_particle) * weightPoly6Kernel(r_ij.norm(), condition_.pnd_weight_radius);
      denominator += weightPoly6Kernel(r_ij.norm(), condition_.pnd_weight_radius);
    }
    average_pressure(i_particle) = numerator / denominator;
  }
}

inline double BubbleParticles::weightPoly6Kernel(double r, double h) {
  if (r > h) return 0;
  if (condition_.dimension == 2) {
    return 4 * std::pow(h * h - r * r, 3) / (M_PI * std::pow(h, 8));
  } else {
    return 315 * std::pow(h * h - r * r, 3) / (64 * M_PI * std::pow(h, 8));
  }
}

void BubbleParticles::calculateModifiedParticleNumberDensity() {
  using namespace tiny_mps;
  Grid grid(condition_.average_distance * 1.05, temporary_position, particle_types.array() != ParticleType::GHOST, condition_.dimension);
  Eigen::Vector3d l0_vec(condition_.average_distance, 0.0, 0.0);
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (particle_types(i_particle) == ParticleType::GHOST) {
      modified_pnd(i_particle) = 0.0;
      continue;
    }
    double n_hat = initial_particle_number_density;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    if (neighbors.empty()) continue;
    for (int j_particle : neighbors) {
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      n_hat += weightForParticleNumberDensity(r_ij) - weightForParticleNumberDensity(l0_vec);
    }
    modified_pnd(i_particle) = std::max(particle_number_density(i_particle), n_hat);
  }
}

void BubbleParticles::solvePressurePoisson(const tiny_mps::Timer& timer) {
  using namespace tiny_mps;
  Grid grid(condition_.laplacian_pressure_weight_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  using T = Eigen::Triplet<double>;
  double lap_r = grid.getGridWidth();
  int n_size = (int)(std::pow(lap_r * 2, dimension));
  double delta_time = timer.getCurrentDeltaTime();
  Eigen::SparseMatrix<double> p_mat(size, size);
  source_term.setZero();
  std::vector<T> coeffs(size * n_size);
  std::vector<int> neighbors(n_size * 2);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    } else if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    }
    grid.getNeighbors(i_particle, neighbors);
    double sum = 0.0;
    double div_vel = 0.0;
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      double mat_ij = weightForLaplacianPressure(r_ij) * 2 * dimension
              / (laplacian_lambda_pressure * initial_particle_number_density);
      sum -= mat_ij;
      div_vel += (temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
      if (boundary_types(j_particle) == BoundaryType::INNER) {
        coeffs.push_back(T(i_particle, j_particle, mat_ij));
      }
    }
    sum -= condition_.weak_compressibility * condition_.mass_density / (delta_time * delta_time);
    coeffs.push_back(T(i_particle, i_particle, sum));
    double initial_pnd_i = initial_particle_number_density * (1 - void_fraction(i_particle));
    source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
                - (particle_number_density(i_particle) - initial_pnd_i)
                * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
  }
  p_mat.setFromTriplets(coeffs.begin(), coeffs.end()); // Finished setup matrix
  solveConjugateGradient(p_mat);
}

void BubbleParticles::solvePressurePoissonDuan(const tiny_mps::Timer& timer) {
  using namespace tiny_mps;
  Grid grid(condition_.laplacian_pressure_weight_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  using T = Eigen::Triplet<double>;
  double lap_r = grid.getGridWidth();
  int n_size = (int)(std::pow(lap_r * 2, dimension));
  double delta_time = timer.getCurrentDeltaTime();
  Eigen::SparseMatrix<double> p_mat(size, size);
  source_term.setZero();
  std::vector<T> coeffs(size * n_size);
  std::vector<int> neighbors(n_size * 2);
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    } else if (boundary_types(i_particle) == BoundaryType::SURFACE) {
      coeffs.push_back(T(i_particle, i_particle, 1.0));
      continue;
    }
    grid.getNeighbors(i_particle, neighbors);
    double sum = 0.0;
    double div_vel = 0.0;
    for (int j_particle : neighbors) {
      if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
      Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
      double mat_ij = weightForLaplacianPressure(r_ij) * 2 * dimension
              / (laplacian_lambda_pressure * initial_particle_number_density);
      sum -= mat_ij;
      div_vel += (temporary_velocity.col(j_particle) - temporary_velocity.col(i_particle)).dot(r_ij)
              * weightForLaplacianPressure(r_ij) * condition_.dimension / (r_ij.squaredNorm() * initial_particle_number_density);
      if (boundary_types(j_particle) == BoundaryType::INNER) {
        coeffs.push_back(T(i_particle, j_particle, mat_ij));
      }
    }
    sum -= condition_.weak_compressibility * condition_.mass_density / (delta_time * delta_time);
    if (free_surface_type(i_particle) == SurfaceLayer::INNER_SURFACE) {
      sum -= (modified_pnd(i_particle) - particle_number_density(i_particle)) * 2 * dimension / (laplacian_lambda_pressure * initial_particle_number_density);
      coeffs.push_back(T(i_particle, i_particle, sum));
      double initial_pnd_i = initial_particle_number_density * (1 - void_fraction(i_particle));
      source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
                - (modified_pnd(i_particle) - initial_pnd_i)
                * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
      // source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
      //           - (modified_pnd(i_particle) - initial_particle_number_density)
      //           * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
    } else {
      coeffs.push_back(T(i_particle, i_particle, sum));
      // double initial_pnd_i = initial_particle_number_density * (1 - void_fraction(i_particle));
      source_term(i_particle) = div_vel * condition_.mass_density * condition_.relaxation_coefficient_vel_div / delta_time
                - (particle_number_density(i_particle) - initial_particle_number_density)
                * condition_.relaxation_coefficient_pnd * condition_.mass_density / (delta_time * delta_time * initial_particle_number_density);
    }
  }
  p_mat.setFromTriplets(coeffs.begin(), coeffs.end()); // Finished setup matrix
  solveConjugateGradient(p_mat);
}

void BubbleParticles::correctVelocityDuan(const tiny_mps::Timer& timer) {
  using namespace tiny_mps;
  Grid grid(condition_.gradient_radius, temporary_position, boundary_types.array() != BoundaryType::OTHERS, condition_.dimension);
  correction_velocity.setZero();
  int tensor_count = 0;
  int not_tensor_count = 0;
  for (int i_particle = 0; i_particle < size; ++i_particle) {
    if (particle_types(i_particle) != ParticleType::NORMAL) continue;
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    Grid::Neighbors neighbors;
    grid.getNeighbors(i_particle, neighbors);
    Eigen::Matrix3d tensor = Eigen::Matrix3d::Zero();
    Eigen::Vector3d tmp_vel(0.0, 0.0, 0.0);
    if (free_surface_type(i_particle) == SurfaceLayer::INNER_SURFACE) {
      for (int j_particle : neighbors) {
        if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        tmp_vel += r_ij * (pressure(j_particle) + pressure(i_particle)) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
      }
      if (dimension == 2) tmp_vel(2) = 0;
      correction_velocity.col(i_particle) -= tmp_vel * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
    } else {
      double p_min = pressure(i_particle);
      double p_max = pressure(i_particle);
      for (int j_particle : neighbors) {
        if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
        p_min = std::min(pressure(j_particle), p_min);
        p_max = std::max(pressure(j_particle), p_max);
      }
      for (int j_particle : neighbors) {
        if (boundary_types(j_particle) == BoundaryType::OTHERS) continue;
        Eigen::Vector3d r_ij = temporary_position.col(j_particle) - temporary_position.col(i_particle);
        Eigen::Vector3d n_ij = r_ij.normalized();
        Eigen::Matrix3d tmp_tensor = Eigen::Matrix3d::Zero();
        tmp_tensor << n_ij(0) * n_ij(0), n_ij(0) * n_ij(1), n_ij(0) * n_ij(2),
                      n_ij(1) * n_ij(0), n_ij(1) * n_ij(1), n_ij(1) * n_ij(2),
                      n_ij(2) * n_ij(0), n_ij(2) * n_ij(1), n_ij(2) * n_ij(2);
        tensor += tmp_tensor * weightForGradientPressure(r_ij) / initial_particle_number_density;
        double xi = 0.2 + 2 * normal_vector.col(j_particle).norm();
        tmp_vel += r_ij * (pressure(j_particle) - pressure(i_particle) + xi * (p_max - p_min)) * weightForGradientPressure(r_ij) / r_ij.squaredNorm();
      }
      if (dimension == 2) {
        tmp_vel(2) = 0;
        tensor(2, 2) = 1.0;
      }
      if (tensor.determinant() > 1.0e-10) {
        correction_velocity.col(i_particle) -= tensor.inverse() * tmp_vel * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
        ++tensor_count;
      } else {
        correction_velocity.col(i_particle) -= tmp_vel * dimension * timer.getCurrentDeltaTime() / (initial_particle_number_density * condition_.mass_density);
        ++not_tensor_count;
      }
    }
  }
  std::cout << "Tensor: " << tensor_count << ", Not Tensor: " << not_tensor_count << std::endl;
  temporary_velocity += correction_velocity;
}

void BubbleParticles::initAverageGrid(const Eigen::Vector3d& min_pos, const Eigen::Vector3d& max_pos) {
  Eigen::Vector3d r_ij = max_pos - min_pos;
  grid_min_pos = min_pos;
  grid_max_pos = max_pos;
  grid_w = r_ij(0) / condition_.average_distance + 1;
  grid_h = r_ij(1) / condition_.average_distance + 1;
  average_grid.reserve(grid_w * grid_h);
  for (int i = 0; i < grid_w * grid_h; i++) {
    average_grid[i] = 0;
  }
}

void BubbleParticles::updateAverageGrid(double start_time, const tiny_mps::Timer& timer) {
  using namespace tiny_mps;
  std::cout << "average: " << average_count  << ", " << timer.getLoopCount() << std::endl;
  if (start_time > timer.getCurrentTime()) {
    average_count = timer.getLoopCount();
    return;
  }
  std::vector<int> number(grid_w * grid_h);
  std::vector<double> tmp_average(grid_w * grid_h);
  for (int i_grid = 0; i_grid < grid_w * grid_h; ++i_grid) {
    tmp_average[i_grid] = 0.0;
    number[i_grid] = 0.0;
  }
  for (int i_particle = 0; i_particle < getSize(); ++i_particle) {
    if (boundary_types(i_particle) == BoundaryType::OTHERS) continue;
    int ix = std::floor((temporary_position(0, i_particle) - grid_min_pos(0) + condition_.average_distance / 2.0) / condition_.average_distance);
    int iy = std::floor((temporary_position(1, i_particle) - grid_min_pos(1) + condition_.average_distance / 2.0) / condition_.average_distance);
    if (ix < 0 || ix >= grid_w) continue;
    if (iy < 0 || iy >= grid_h) continue;
    tmp_average[ix + iy * grid_w] += pressure(i_particle);
    number[ix + iy * grid_w]++;
  }
  int n = timer.getLoopCount() - average_count;
  for (int i_grid = 0; i_grid < grid_w * grid_h; ++i_grid) {
    if (number[i_grid] > 0) tmp_average[i_grid] /= number[i_grid];
    average_grid[i_grid] = (average_grid[i_grid] * n + tmp_average[i_grid])/ (n + 1);
  }

}

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOOKUP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOOKUP_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-swar
    Function object implementing lookup capabilities

    shuffles the elements of x using indices in n

    @par Semantic:

    For every parameters of types respectively T, N:

    @code
    T r = lookup(x,n);
    @endcode

    is similar to:

    @code
    T r;
    for(size_t i=0; i < boost::simd::meta::cardinal_of<T0>::value; i++)
      r[i] = x[n[i]];
    @endcode

  **/
  Value lookup(Value const & v0, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/lookup.hpp>
#include <boost/simd/function/simd/lookup.hpp>

#endif


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceEdgeCases
#include <algorithm>
#include <boost/multi_array.hpp>
#include <boost/program_options.hpp>
#include <boost/test/unit_test.hpp>
#include <functional>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Util.hpp>
#include <popops/Reduce.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace popops;
using namespace poplibs_test::util;
using namespace poplibs_support;

const OptionFlags options;

struct TestCase {
  std::vector<std::size_t> inShape;
  std::vector<std::size_t> dims;
  std::vector<std::size_t> outShape;
};

std::ostream &operator<<(std::ostream &os, const std::vector<size_t> &test) {
  os << '[';
  StringRef sep = "";
  for (size_t item : test) {
    os << sep << item;
    sep = ", ";
  }
  return os << ']';
}

std::ostream &operator<<(std::ostream &os, const TestCase &test) {
  return os << "TestCase{inShape=" << test.inShape << ", dims=" << test.dims
            << ", outShape=" << test.outShape << "}";
}

// Call popops::reduce or popops::reduceMany depending on `useReduceMany` with
// a single input tensor and expect a single output tensor.
static Tensor reduceWrapper(bool useReduceMany, Graph &graph, const Tensor &in,
                            const std::vector<size_t> &dims, Sequence &prog) {
  if (!useReduceMany)
    return popops::reduce(graph, in, FLOAT, dims, popops::Operation::ADD, prog);
  else {
    std::vector<Tensor> outs;
    SingleReduceOp op = {in, dims, popops::Operation::ADD, FLOAT};
    popops::reduceMany(graph, {std::move(op)}, outs, prog);
    BOOST_TEST(outs.size() == 1);
    return outs[0];
  }
}

BOOST_AUTO_TEST_CASE(Reduce_Nop_ADD_float) {
  // Tests for nop reductions, where the reduced dimension is 1, or
  // any of the input dimensions are 0.
  std::vector<TestCase> testCases = {
      TestCase{{2, 1, 2, 3}, {1, 2}, {2, 3}},
      TestCase{{2, 3, 4, 0}, {3}, {2, 3, 4}},
      TestCase{{2, 3, 4, 0}, {0}, {3, 4, 0}},
      TestCase{{1, 1, 1}, {1}, {1, 1}},
      TestCase{{1, 1, 1, 0}, {0, 1}, {1, 0}},
      TestCase{{0, 1, 2}, {}, {0, 1, 2}},
      TestCase{{0, 1, 2, 3}, {3}, {0, 1, 2}},
      TestCase{{2, 2}, {0, 1}, {}},
  };

  auto device = createTestDevice(TEST_TARGET, 1, 64);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  Sequence prog;
  for (const auto &[inShape, dims, outShape] : testCases) {
    auto in = graph.addVariable(FLOAT, inShape, "in");
    poputil::mapTensorLinearly(graph, in);
    for (bool useReduceMany : {false, true}) {
      Tensor out = reduceWrapper(useReduceMany, graph, in, dims, prog);
      BOOST_TEST(out.shape() == outShape);
    }
  }
}

BOOST_AUTO_TEST_CASE(ReduceCheckProgsAreOptimised) {
  // Any reduction that only removes dimensions of size 1 will leave the number
  // of output elements the same and so should be optimised to a simpler
  // expression such as a copy/cast or map expression. Unless the compute set
  // overload of reduce is used.
  const std::array<TestCase, 2> tests = {
      TestCase{{1}, {0}, {}},
      TestCase{{2, 2, 1}, {2}, {2, 2}},
  };

  auto device = createTestDevice(TEST_TARGET, 1, 64);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  for (const auto &test : tests) {
    for (bool useReduceMany : {false, true}) {
      std::string reduceName = (useReduceMany ? "reduceMany" : "reduce");
      Sequence prog({}, DebugContext{reduceName + "CheckProgsAreOptimised"});
      Tensor in = graph.addVariable(FLOAT, test.inShape,
                                    VariableMappingMethod::LINEAR, "in");
      Tensor out = reduceWrapper(useReduceMany, graph, in, test.dims, prog);
      BOOST_TEST(out.shape() == test.outShape);
      // This is quicker and simpler than compiling the graph and iterating
      // through the execution profile. However this is a terrible test
      // because it will still pass if there's a copy and a bunch of junk.
      // TODO: deserialize the JSON and check the results properly.
      std::stringstream s;
      dumpProgram(graph, prog, s);
      auto progJson = s.str();
      BOOST_TEST_MESSAGE(test << " with " << reduceName
                              << " => progJson=" << progJson);
      BOOST_TEST(progJson.find("Copy") != std::string::npos);
      BOOST_TEST(progJson.find("Execute") == std::string::npos);
    }
  }
}

BOOST_AUTO_TEST_CASE(ReduceCheckNoProgsNeeded) {
  // When the output tensor is empty no reduction needs to happen.
  const std::array<TestCase, 2> tests = {
      TestCase{{2, 0, 1}, {2}, {2, 0}},
      TestCase{{0, 1, 2}, {}, {0, 1, 2}},
  };

  auto device = createTestDevice(TEST_TARGET, 1, 64);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  for (const auto &test : tests) {
    for (bool useReduceMany : {false, true}) {
      std::string reduceName = (useReduceMany ? "reduceMany" : "reduce");
      Sequence prog({}, DebugContext{reduceName + "CheckNoProgsNeeded"});
      Tensor in = graph.addVariable(FLOAT, test.inShape,
                                    VariableMappingMethod::LINEAR, "in");
      Tensor out = reduceWrapper(useReduceMany, graph, in, test.dims, prog);
      BOOST_TEST(out.shape() == std::vector<size_t>(test.outShape));
      BOOST_TEST(out.numElements() == 0);
      std::stringstream s;
      dumpProgram(graph, prog, s);
      auto progJson = s.str();
      BOOST_TEST_MESSAGE(test << " with " << reduceName
                              << " => progJson=" << progJson);
      BOOST_TEST(progJson.find("Copy") == std::string::npos);
      BOOST_TEST(progJson.find("Execute") == std::string::npos);
    }
  }
}

BOOST_AUTO_TEST_CASE(ReduceCheckOutputFromEmptyInputIsFilled) {
  // When the inputs are empty but the output tensor is not empty the
  // output tensor needs to be created and filled with starting values
  // using an Execute program.
  const std::array<TestCase, 2> tests = {
      TestCase{{0}, {0}, {}},
      TestCase{{2, 0, 1}, {1}, {2, 1}},
  };

  auto device = createTestDevice(TEST_TARGET, 1, 64);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  for (const auto &test : tests) {
    for (bool useReduceMany : {false, true}) {
      std::string reduceName = (useReduceMany ? "reduceMany" : "reduce");
      Sequence prog(
          {}, DebugContext{reduceName + "CheckOutputFromEmptyInputIsFilled"});
      Tensor in = graph.addVariable(FLOAT, test.inShape,
                                    VariableMappingMethod::LINEAR, "in");
      Tensor out = reduceWrapper(useReduceMany, graph, in, test.dims, prog);
      BOOST_TEST(out.shape() == std::vector<size_t>(test.outShape));
      BOOST_TEST(out.numElements() != 0);
      std::stringstream s;
      dumpProgram(graph, prog, s);
      auto progJson = s.str();
      BOOST_TEST_MESSAGE(test << " with " << reduceName
                              << " => progJson=" << progJson);
      BOOST_TEST(progJson.find("Copy") == std::string::npos);
      BOOST_TEST(progJson.find("Execute") != std::string::npos);
    }
  }
}

BOOST_AUTO_TEST_CASE(ReduceIntermediatePrec) {
  // Test that we can accumulate in higher precision by adding lots of small
  // values to a large value such that if it were done with half precision
  // accumulation all the smaller terms would be lost.
  auto tdevice = createTestDevice(TEST_TARGET);
  const auto &target = tdevice.getTarget();
  Graph graph(target);

  popops::addCodelets(graph);

  const auto N = 100;
  Tensor input = graph.addVariable(HALF, {N});
  poputil::mapTensorLinearly(graph, input);

  Sequence prog;

  auto out = reduce(graph, input, {0}, popops::Operation::ADD, prog);

  std::vector<float> hInput(N);
  hInput[0] = 8192;
  for (unsigned i = 1; i < N; ++i)
    hInput[i] = 1;

  graph.setInitialValue(input, poplar::ArrayRef<float>(hInput));
  graph.createHostRead("out", out);

  Engine engine(graph, prog, options);
  tdevice.bind([&](const Device &device) {
    engine.load(device);
    engine.run(0);

    std::vector<char> hVal(target.getTypeSize(HALF));
    float val;

    engine.readTensor("out", hVal.data(), hVal.data() + hVal.size());

    copyDeviceHalfToFloat(target, hVal.data(), &val, 1);

    // In the half precision range > 8192 the representation will round to
    // multiples of 8
    BOOST_CHECK_EQUAL(val, 8192 + ((N - 1) / 8) * 8);
  });
}

BOOST_AUTO_TEST_CASE(Reduce_Huge_ADD_float) {
  auto device = createTestDevice(TEST_TARGET);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  // create a huge amount of partials and map them all to the same tile to blow
  // the vector list 12-bit count limit.
  auto in = graph.addVariable(HALF, {{3, 10500, 3}}, "in");
  graph.setTileMapping(in, 0);

  Sequence prog;
  popops::reduce(graph, in, HALF, {1}, popops::Operation::ADD, prog);

  // we expect this to throw an out of memory exception but NOT an exception
  // complaining about the number of partials.
  try {
    Engine e(graph, prog, {{"debug.allowOutOfMemory", "true"}});
  } catch (const poplar::graph_memory_allocation_error &) {
  };
}

BOOST_AUTO_TEST_CASE(Avoid_subword_mapping_single_element_per_tile) {
  const unsigned numOutputs = 4;
  auto device = createTestDevice(TEST_TARGET, 1, numOutputs);
  Graph graph(device.getTarget());
  popops::addCodelets(graph);

  auto inHalf = graph.addVariable(HALF, {{numOutputs, 100}}, "inH");
  auto inFloat = graph.addVariable(HALF, {{numOutputs, 100}}, "inF");

  for (unsigned i = 0; i != numOutputs; ++i) {
    graph.setTileMapping(inHalf[i], i);
    graph.setTileMapping(inFloat[i], i);
  }

  Sequence prog;
  auto outHalf =
      popops::reduce(graph, inHalf, HALF, {1}, popops::Operation::ADD, prog);
  auto outFloat =
      popops::reduce(graph, inHalf, FLOAT, {1}, popops::Operation::ADD, prog);

  // check over how many tiles the output is mapped
  auto tMap = graph.getTileMapping(outHalf);
  auto tilesContainingOutHalf =
      std::accumulate(tMap.begin(), tMap.end(), 0U,
                      [](unsigned num, const std::vector<Interval> &mapping) {
                        return num + !mapping.empty();
                      });

  tMap = graph.getTileMapping(outFloat);
  auto tilesContainingOutFloat =
      std::accumulate(tMap.begin(), tMap.end(), 0U,
                      [](unsigned num, const std::vector<Interval> &mapping) {
                        return num + !mapping.empty();
                      });

  // The output should be remapped
  BOOST_CHECK_NE(tilesContainingOutHalf, numOutputs);
  // The output should not be remapped
  BOOST_CHECK_EQUAL(tilesContainingOutFloat, numOutputs);
}


#include <stdexcept>
#include <boost\test\unit_test.hpp>
#include <CL/cl.hpp>
#include "OpenCLWorkBench.h"
#include <boost/cstdint.hpp>

using namespace std;
using namespace OpenCLWorkBench;

BOOST_AUTO_TEST_SUITE( TestPitchComparison )

BOOST_AUTO_TEST_CASE( TestPitchComparison00 )
{
	Workbench task;

	try {
		task.ConnectDevice( CL_DEVICE_TYPE_GPU )
			.BuildContext()
			.BuildProgram( "KernelGPU_Inspection.cl" );

		cl::Kernel kernelPitch0( task.GetProgram(), "pitch_comprison_0" );

		int width = 12*1024, height = 512;
		int left = 10, top =0, right = width-10, bottom = height-1;
		int pitch = 10;

		vector<uint8_t> src( width*height, 0 );
		src[ 10 ] = 50;
		src[ 20 ] = 150;
		src[ 30 ] = 50;
		src[ 10 + (height-1)*width ] = 50;
		src[ 20 + (height-1)*width ] = 150;
		src[ 30 + (height-1)*width ] = 50;

		vector<uint8_t> dst( width*height, 0 );

		cl::CommandQueue queue( task.GetContext(), task.GetConnectedDevices()[0] );

		cl::Buffer devSrc( task.GetContext(), CL_MEM_READ_ONLY, width*height );
		cl::Buffer devDst( task.GetContext(), CL_MEM_WRITE_ONLY, width*height );

		cl::UserEvent kickOff( task.GetContext() );
		cl::Event e1, e2, e3;

		vector<cl::Event> vKickOff; vKickOff.push_back( kickOff );
		queue.enqueueWriteBuffer( devSrc, CL_FALSE, 0, width*height, src.data(), &vKickOff, &e1 );

		kernelPitch0.setArg( 0, devSrc );
		kernelPitch0.setArg( 1, devDst );
		kernelPitch0.setArg( 2, width );
		kernelPitch0.setArg( 3, height );
		kernelPitch0.setArg( 4, left );
		kernelPitch0.setArg( 5, top );
		kernelPitch0.setArg( 6, right );
		kernelPitch0.setArg( 7, bottom );
		kernelPitch0.setArg( 8, pitch );

		const int LOCAL_WIDTH = 64, LOCAL_HEIGHT = 4;
		cl::NDRange localws( LOCAL_WIDTH, LOCAL_HEIGHT );
		cl::NDRange globalws(
			LOCAL_WIDTH * (int)ceil( (double)(right-left+1) / (double)LOCAL_WIDTH ),
			LOCAL_HEIGHT * (int)ceil( (double)(bottom-top+1) / (double)LOCAL_HEIGHT )
		);

		vector<cl::Event> ve1; ve1.push_back( e1 );
		queue.enqueueNDRangeKernel( kernelPitch0, cl::NullRange, globalws, localws, &ve1, &e2 );

		vector<cl::Event> ve2; ve2.push_back( e2 );
		queue.enqueueReadBuffer( devDst, CL_FALSE, 0, width*height, dst.data(), &ve2, &e3  );

		kickOff.setStatus( CL_COMPLETE );
		e3.wait();

		BOOST_CHECK_EQUAL( dst[ 10 ], 50 );
		BOOST_CHECK_EQUAL( dst[ 20 ], 200 );
		BOOST_CHECK_EQUAL( dst[ 30 ], 50 );
		BOOST_CHECK_EQUAL( dst[ 10 + (height-1)*width ], 50 );
		BOOST_CHECK_EQUAL( dst[ 20 + (height-1)*width ], 200 );
		BOOST_CHECK_EQUAL( dst[ 30 + (height-1)*width ], 50 );
	}
	catch( cl::Error e ) {
		cout << e.what() << ": Error Code " << e.err() << endl;
	}
	catch( WorkbenchException e )
	{
		cout << e.what() << endl;
		cout << task.GetBuildLog() << endl;
	}
}

BOOST_AUTO_TEST_CASE( TestPitchComparison01 )
{
	Workbench task;

	try {
		task.ConnectDevice( CL_DEVICE_TYPE_CPU )
			.BuildContext()
			.BuildProgram( "KernelCPU_Inspection.cl" );

		cl::Kernel kernelPitch0( task.GetProgram(), "pitch_comparion_0" );

		int width = 12*1024, height = 512;
		int left = 10, top =0, right = width-10, bottom = height-1;
		float pitch = 9.5;

		cl::CommandQueue queue( task.GetContext(), task.GetConnectedDevices()[0] );

		cl::Buffer devSrc( task.GetContext(), CL_MEM_READ_ONLY, width*height );
		cl::Buffer devDst( task.GetContext(), CL_MEM_WRITE_ONLY, width*height );

		uint8_t* src = (uint8_t*)queue.enqueueMapBuffer( devSrc, CL_TRUE, CL_MAP_WRITE, 0, width*height );
		src[ 10 ] = 50;  src[ 11 ] = 100;
		src[ 20 ] = 125;
		src[ 29 ] = 50;	 src[ 30 ] = 100;
		queue.enqueueUnmapMemObject( devSrc, src );

		int integerPitch = (int)(pitch);
		short toFloor = (short)( ((pitch - integerPitch) * 0x7F)+0.5f);
		short toCeiling = (short)( 0x7F - toFloor );
		int cpuCount = 4;
		int blockWidth = (int)ceil( (double)(right - left + 1 ) / cpuCount );

		kernelPitch0.setArg( 0, devSrc );
		kernelPitch0.setArg( 1, devDst );
		kernelPitch0.setArg( 2, width );
		kernelPitch0.setArg( 3, height );
		kernelPitch0.setArg( 4, left );
		kernelPitch0.setArg( 5, top );
		kernelPitch0.setArg( 6, right );
		kernelPitch0.setArg( 7, bottom );
		kernelPitch0.setArg( 8, blockWidth );
		kernelPitch0.setArg( 9, integerPitch );
		kernelPitch0.setArg( 10, toCeiling );
		kernelPitch0.setArg( 11, toFloor );

		cl::NDRange globalws( cpuCount );
		cl::NDRange localws( 1 );

		cl::Event ev = cl::Event();
		queue.enqueueNDRangeKernel( kernelPitch0, cl::NullRange, globalws, localws, NULL, &ev );
		queue.finish();

		uint8_t* dst = (uint8_t*)queue.enqueueMapBuffer( devDst, CL_TRUE, CL_MAP_READ, 0, width*height );
		BOOST_CHECK_EQUAL( dst[ 20 ], 50 );
		queue.enqueueUnmapMemObject( devDst, dst );

	}
	catch( cl::Error e ) {
		cout << e.what() << ": Error Code " << e.err() << endl;
		cout << task.GetBuildLog() << endl;
	}
	catch( WorkbenchException e )
	{
		cout << e.what() << endl;
		cout << task.GetBuildLog() << endl;
	}
}

BOOST_AUTO_TEST_CASE( TestPitchThreshold00 )
{
	Workbench task;

	try {
		task.ConnectDevice( CL_DEVICE_TYPE_GPU )
			.BuildContext()
			.BuildProgram( "KernelGPU_Inspection.cl" );

		cl::Kernel kernelPitch0( task.GetProgram(), "pitch_threshold_0" );

		int width = 12*1024, height = 512;
		int left = 10, top =0, right = width-10, bottom = height-1;
		float pitch = 10.0;

		vector<uint8_t> src( width*height, 0 );
		src[ 10 ] = 50;
		src[ 20 ] = 150;
		src[ 30 ] = 50;
		src[ 10 + (height-1)*width ] = 50;
		src[ 20 + (height-1)*width ] = 150;
		src[ 30 + (height-1)*width ] = 50;

		vector<uint8_t> dst( width*height/8, 0 );

		cl::CommandQueue queue( task.GetContext(), task.GetConnectedDevices()[0] );

		cl::Buffer devSrc( task.GetContext(), CL_MEM_READ_ONLY, width*height );
		cl::Buffer devDst( task.GetContext(), CL_MEM_WRITE_ONLY, width*height/8 );

		cl::UserEvent kickOff( task.GetContext() );
		cl::Event e1, e2, e3;

		vector<cl::Event> vKickOff; vKickOff.push_back( kickOff );
		queue.enqueueWriteBuffer( devSrc, CL_FALSE, 0, width*height, src.data(), &vKickOff, &e1 );

		const int LOCAL_WIDTH = 64, LOCAL_HEIGHT = 4;
		cl::NDRange localws( LOCAL_WIDTH, LOCAL_HEIGHT );
		cl::NDRange globalws(
			LOCAL_WIDTH * (int)ceil( (double)(right-left+1) / (double)LOCAL_WIDTH ),
			LOCAL_HEIGHT * (int)ceil( (double)(bottom-top+1) / (double)LOCAL_HEIGHT )
		);

		int integerPitch = (int)(pitch);
		float toCeiling = pitch - (int)pitch;
		float toFloor = 1.0f - toCeiling;
		uint8_t threshold = 10;

		kernelPitch0.setArg( 0, devSrc );
		kernelPitch0.setArg( 1, devDst );
		kernelPitch0.setArg( 2, width );
		kernelPitch0.setArg( 3, height );
		kernelPitch0.setArg( 4, left );
		kernelPitch0.setArg( 5, top );
		kernelPitch0.setArg( 6, right );
		kernelPitch0.setArg( 7, bottom );
		kernelPitch0.setArg( 8, integerPitch );
		kernelPitch0.setArg( 9, toCeiling );
		kernelPitch0.setArg( 10, toFloor );
		kernelPitch0.setArg( 11, LOCAL_WIDTH*LOCAL_HEIGHT, NULL );
		kernelPitch0.setArg( 12, threshold );

		vector<cl::Event> ve1; ve1.push_back( e1 );
		queue.enqueueNDRangeKernel( kernelPitch0, cl::NullRange, globalws, localws, &ve1, &e2 );

		vector<cl::Event> ve2; ve2.push_back( e2 );
		queue.enqueueReadBuffer( devDst, CL_FALSE, 0, width*height/8, dst.data(), &ve2, &e3  );

		kickOff.setStatus( CL_COMPLETE );
		e3.wait();

	}
	catch( cl::Error e ) {
		cout << e.what() << ": Error Code " << e.err() << endl;
	}
	catch( WorkbenchException e )
	{
		cout << e.what() << endl;
		cout << task.GetBuildLog() << endl;
	}
}

void FillUpPitchThresholdArg(
	cl::Kernel& kernel, cl::Buffer& src, cl::Buffer& dst,
	int width, int height,
	int left, int top, int right, int bottom,
	float pitch, uint8_t threshold, int workspace_size )
{
	int integerPitch = (int)(pitch);
	float toCeiling = pitch - (int)pitch;
	float toFloor = 1.0f - toCeiling;

	kernel.setArg( 0, src );
	kernel.setArg( 1, dst );
	kernel.setArg( 2, width );
	kernel.setArg( 3, height );
	kernel.setArg( 4, left );
	kernel.setArg( 5, top );
	kernel.setArg( 6, right );
	kernel.setArg( 7, bottom );
	kernel.setArg( 8, integerPitch );
	kernel.setArg( 9, toCeiling );
	kernel.setArg( 10, toFloor );
	kernel.setArg( 11, workspace_size, NULL );
	kernel.setArg( 12, threshold );
}

BOOST_AUTO_TEST_CASE( TestPitchThreshold01 )
{
	Workbench task;

	try {
		task.ConnectDevice( CL_DEVICE_TYPE_GPU )
			.BuildContext()
			.BuildProgram( "KernelGPU_Inspection.cl" );

		cl::Kernel kernelPitch0( task.GetProgram() , "pitch_threshold_0" );
		cl::Kernel kernelPitch1( task.GetProgram() , "pitch_threshold_0" );

		int width = 12*1024, height = 512;
		int left = 10, top =0, right = width-10, bottom = height-1;
		float pitch = 10.0;

		vector<uint8_t> src1( width*height, 0 ), src2( width*height, 0 );
		vector<uint8_t> dst1( width*height/8, 0 ), dst2( width*height/8, 0 );

		cl::CommandQueue queue( task.GetContext(), task.GetConnectedDevices()[0] );
		cl::CommandQueue queue2( task.GetContext(), task.GetConnectedDevices()[0] );

		cl::Buffer devSrc1( task.GetContext(), CL_MEM_READ_ONLY, width*height ), devDst1( task.GetContext(), CL_MEM_WRITE_ONLY, width*height/8 );
		cl::Buffer devSrc2( task.GetContext(), CL_MEM_READ_ONLY, width*height ), devDst2( task.GetContext(), CL_MEM_WRITE_ONLY, width*height/8 );

		const int LOCAL_WIDTH = 64, LOCAL_HEIGHT = 4;
		cl::NDRange localws( LOCAL_WIDTH, LOCAL_HEIGHT );
		cl::NDRange globalws(
			LOCAL_WIDTH * (int)ceil( (double)(right-left+1) / (double)LOCAL_WIDTH ),
			LOCAL_HEIGHT * (int)ceil( (double)(bottom-top+1) / (double)LOCAL_HEIGHT )
		);

		FillUpPitchThresholdArg( kernelPitch0, devSrc1, devDst1, width, height, left, top, right, bottom, pitch, 10, LOCAL_WIDTH*LOCAL_HEIGHT );
		FillUpPitchThresholdArg( kernelPitch1, devSrc2, devDst2, width, height, left, top, right, bottom, pitch, 10, LOCAL_WIDTH*LOCAL_HEIGHT );

		cl::UserEvent kickOff( task.GetContext() );
		cl::Event e0, e1, e2;
		cl::Event f0, f1, f2;

		vector<cl::Event> vKickOff; vKickOff.push_back( kickOff );
		queue.enqueueWriteBuffer( devSrc1, CL_FALSE, 0, width*height, src1.data(), &vKickOff, &e0 );

		vector<cl::Event> fve0; fve0.push_back( e0 ); fve0.push_back( kickOff );
		queue2.enqueueWriteBuffer( devSrc2, CL_FALSE, 0, width*height, src2.data(), &fve0, &f0 );

		vector<cl::Event> ve0; ve0.push_back( e0 );
		queue.enqueueNDRangeKernel( kernelPitch0, cl::NullRange, globalws, localws, &ve0, &e1 );

		vector<cl::Event> ve1; ve1.push_back( e1 );
		queue.enqueueReadBuffer( devDst1, CL_FALSE, 0, width*height/8, dst1.data(), &ve1, &e2  );

/*
		vector<cl::Event> vf0; vf0.push_back( f0 ); vf0.push_back( e1 );
		queue.enqueueNDRangeKernel( kernelPitch1, cl::NullRange, globalws, localws, &vf0, &f1 );

		vector<cl::Event> vf1; vf1.push_back( f1 ); vf1.push_back( e2 );
		queue.enqueueReadBuffer( devDst2, CL_FALSE, 0, width*height/8, dst2.data(), &vf1, &f2  );

		kickOff.setStatus( CL_COMPLETE );
		f2.wait();
*/
		kickOff.setStatus( CL_COMPLETE );
		e2.wait();

	}
	catch( cl::Error e ) {
		cout << e.what() << ": Error Code " << e.err() << endl;
	}
	catch( WorkbenchException e )
	{
		cout << e.what() << endl;
		cout << task.GetBuildLog() << endl;
	}
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

#include <boost/graph/use_mpi.hpp>
#include <boost/property_map/parallel/distributed_property_map.hpp>
#include <boost/property_map/parallel/global_index_map.hpp>
#include <boost/graph/distributed/page_rank.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random.hpp>
#include <vector>
#include <iostream>
#include <stdlib.h>

#ifdef CSR
#include <boost/graph/distributed/compressed_sparse_row_graph.hpp>
#endif

using namespace boost;

bool close_to(double x, double y)
{
  double diff = x - y;
  if (diff < 0) diff = -diff;
  double base = (y == 0? x : y);
  if (base != 0) return diff / base < 0.01;
  else return true;
}

// Make convenient labels for the vertices
void test_distributed_page_rank(amplusplus::transport trans, int n, double p, int iterations,
				int nthreads, bool verify, int seed)
{
  assert(nthreads == 1); // PageRank only works single threaded for now

#ifndef CSR
  typedef adjacency_list<vecS,
                         distributedS<vecS>,
                         directedS> Graph;
#else
  typedef compressed_sparse_row_graph<directedS, no_property, no_property, no_property,
                                      distributedS<unsigned long> > Graph;
#endif

  typedef graph_traits<Graph>::vertex_descriptor vertex_descriptor;

  minstd_rand gen;

  gen.seed(seed);

  Graph g(erdos_renyi_iterator<minstd_rand, Graph>(gen, n, p/2),
	  erdos_renyi_iterator<minstd_rand, Graph>(),
	  n, trans);

  // Setup RankMaps
  std::vector<double> rank(num_vertices(g)), other_rank(num_vertices(g));

  typedef property_map<Graph, vertex_index_t>::type VertexIndexMap;
  typedef property_map<Graph::base_type, vertex_index_t>::type LocalVertexIndexMap;
  typedef iterator_property_map<std::vector<double>::iterator, LocalVertexIndexMap> LocalRankMap;
  typedef property_map<Graph, vertex_global_t>::const_type VertexGlobalMap;
  typedef boost::parallel::distributed_property_map<VertexGlobalMap, LocalRankMap> RankMap;

  trans.set_nthreads(nthreads);
  boost::graph::distributed::page_rank<Graph, RankMap, boost::graph::n_iterations>
    PR(g,
       make_iterator_property_map(rank.begin(), get(vertex_index, g)),
       make_iterator_property_map(other_rank.begin(), get(vertex_index, g)),
       boost::graph::n_iterations(iterations), 0.85, n);

  boost::scoped_array<boost::thread> threads(new boost::thread[nthreads]);
  for (int i = 0 ; i < nthreads ; ++i) {
    boost::thread thr(boost::ref(PR), i);
    threads[i].swap(thr);
  }

  for (int i = 0 ; i < nthreads ; ++i)
    threads[i].join();

  // Back to one thread
  trans.set_nthreads(1);

  if (trans.rank() == 0)
    std::cout << "Distributed PageRank complete (" << nthreads << " threads)\n";

  if ( verify ) {
    // Check against the sequential version

    typedef adjacency_list<listS,
      vecS,
      directedS,
      // Vertex properties
      no_property,
      // Edge properties
      no_property > Graph2;

    gen.seed(seed);

    Graph2 g2(erdos_renyi_iterator<minstd_rand, Graph>(gen, n, p/2),
	      erdos_renyi_iterator<minstd_rand, Graph>(),
	      n);

    std::vector<double> rank2(num_vertices(g2));

    page_rank(g2,
	      make_iterator_property_map(rank2.begin(), get(vertex_index, g2)),
	      boost::graph::n_iterations(iterations), 0.85, n);

    if (trans.rank() == 0)
      std::cout << "Sequential PageRank complete\n";

    VertexIndexMap v_index(get(vertex_index, g));

    boost::parallel::global_index_map<VertexIndexMap, VertexGlobalMap>
      global_index(trans, num_vertices(g), v_index, get(vertex_global, g));

    BGL_FORALL_VERTICES(v, g, Graph) {
      if (!close_to(rank[get(v_index, v)], rank2[get(global_index, v)]))
	std::cout << "distributed rank = " << rank[get(v_index, v)] << "  sequential rank = "
	          << rank2[get(global_index, v)] << std::endl;

      BOOST_CHECK(close_to(rank[get(v_index, v)], rank2[get(global_index, v)]));
    }
  }
}

int test_main(int argc, char* argv[])
{
  // AM++ initialization
  amplusplus::environment env = amplusplus::mpi_environment(argc, argv, true);
  amplusplus::transport trans = env.create_transport();

  int n = 1000;
  double p = 0.01;
  int iterations = 20;

  if (argc < 4)
    test_distributed_page_rank(trans, n, p, iterations, 1, true, 123);
//     test_distributed_page_rank(trans, n, p, iterations, 2, true, 123);
  else
    /* n, p, iterations threads seed verify */
    test_distributed_page_rank(trans, atoi(argv[1]), atof(argv[2]), atoi(argv[3]),
			       argc == 4 ? 1 : atoi(argv[4]),
			       argc == 6 ? true : argv[6] == std::string("true"),
			       argc == 5 ? 123 : atoi(argv[5]));

  return 0;
}


#define BOOST_FILESYSTEM_VERSION 3

#include <iostream>
#include <fstream>
#include <vector>
#include "huffman_tree.hpp"
#include <boost/filesystem.hpp>
#include "constants.hpp"

long COUNTS_SIZE;
unsigned int VAL_SIZE;
unsigned long bytecount_ending;

std::vector<uint64_t> count(std::ifstream &file){

    std::vector<uint64_t> counts;
    counts.reserve(BYTE_SIZE);

    char buffer[2050]; // worst case size
    int index = 0;

    file.read(buffer, 2050);

    bytecount_ending = static_cast<unsigned long>(static_cast<unsigned char>(buffer[index++]));

    VAL_SIZE = static_cast<unsigned int>(static_cast<unsigned char>(buffer[index++]));

    if (verbose) std::cout << "read " << file.gcount() << " bytes" << std::endl;

    auto *ubuffer = reinterpret_cast<unsigned char *>(buffer);

    for (int counter = 0; counter < BYTE_SIZE; counter++){
        uint64_t next = 0;
        for(unsigned int i = 0; i < VAL_SIZE; i++){
            next |=  static_cast<uint64_t>(ubuffer[index++]) << (i*8);
        }

        counts.emplace_back(next);
    }
    COUNTS_SIZE = 2 + VAL_SIZE*BYTE_SIZE;

    return counts;
}

void huffman_decode(const huffman_tree &huffman, std::ifstream &file_in, std::ofstream &file_out){

    char buffer_write[BLOCK_SIZE];
    int write_index = 0;

    char buffer_bool = 0;

    huffman_tree::tree *position = huffman.root;

    file_in.seekg(COUNTS_SIZE);
    file_in.clear();

    while(file_in.get(buffer_bool)) {
        /*if (file_in.eof() and i == file_in.gcount()){ // todo make this work :&
            max_bit_index -= static_cast<unsigned char>(bits_to_skip);
        }*/
        for(signed int i = 0; i < 8; ++i){
            bool direction = static_cast<bool>((buffer_bool >> i) & 1);
            if ( direction == huffman_tree::LEFT){
                position = position->left;
            } else if (direction == huffman_tree::RIGHT){
                position = position->right;
            }

            if (position->character != nullptr){
                buffer_write[write_index++] = *(position->character);
                position = huffman.root;

                if (write_index == BLOCK_SIZE) {

                    while (file_in.eof() and bytecount_ending != (( (long)file_out.tellp() + write_index ) & 0xFF) ){
                        write_index--;
                        // hack to fix bug caused by a byte having garbage data at end
                        // should ideally be fixed by saving the amount of bits that do not count
                        // and not parsing them, but can't get it to work :/
                    }
                    file_out.write(buffer_write, BLOCK_SIZE);
                    if (verbose) std::cout << "wrote " << BLOCK_SIZE << " bytes" << std::endl;
                    write_index = 0;
                }
            }

        }
    }

    while (bytecount_ending != (( (unsigned int)file_out.tellp() + write_index ) & 0xFF) ){
        std::cerr << write_index << '\r';
        write_index--; // hack to fix bug, see above
    }
    file_out.write(buffer_write, write_index);
    if (verbose) std::cout << "wrote " << write_index << " bytes" << std::endl;
}

int main(int argc, char *argv[]) {
    auto filename_in = test_huff / boost::filesystem::path("opg12.pdf.huff");
    if(argc > 1) filename_in = argv[1];

    auto file_part = filename_in.stem();
    auto extension = filename_in.extension();

    auto filename_out = test_out / filename_in;
    if(argc > 2) filename_out = argv[2];
    if (boost::filesystem::is_directory(filename_out)){
        filename_out /= file_part;
    }

    std::ifstream file_in;
    std::ofstream file_out;

    file_in.open(filename_in.string()  , std::ios::in  | std::ios::binary);
    file_out.open(filename_out.string(), std::ios::out | std::ios::binary);

    std::vector<uint64_t> counts = count(file_in);

    auto huff = huffman_tree(counts);

    huffman_decode(huff, file_in, file_out);

    file_in.close();
    file_out.close();
    return EXIT_SUCCESS;
}


#include <boost/format.hpp>
#include "../../include/dbcppp/Network2Functions.h"

using namespace dbcppp;

bool bit_is_inbetween(const ISignal& sig, std::size_t i_bit, uint64_t switch_value = -1)
{
    if (switch_value != -1 &&
        sig.MultiplexerIndicator() == ISignal::EMultiplexer::MuxValue &&
        sig.MultiplexerSwitchValue() != switch_value)
    {
        return false;
    }
    switch (sig.ByteOrder())
    {
    case ISignal::EByteOrder::LittleEndian:
    {
        std::size_t start = sig.StartBit();
        std::size_t end = sig.StartBit() + sig.BitSize();
        return start <= i_bit && end > i_bit;
    }
    case ISignal::EByteOrder::BigEndian:
    {
        std::size_t start = sig.StartBit();
        std::size_t n = sig.BitSize();
        while (n)
        {
            if (start == i_bit)
            {
                return true;
            }
            if (start % 8 == 0)
            {
                start += 15;
            }
            else
            {
                start--;
            }
            n--;
        }
        return true;
    }
    }
    return false;
}
bool is_start_bit(const ISignal& sig, std::size_t i_bit)
{
    switch (sig.ByteOrder())
    {
    case ISignal::EByteOrder::LittleEndian:
        return sig.StartBit() == i_bit;
    case ISignal::EByteOrder::BigEndian:
        break;
    }
    return false;
}
bool is_end_bit(const ISignal& sig, std::size_t i_bit)
{
    switch (sig.ByteOrder())
    {
    case ISignal::EByteOrder::LittleEndian:
        return sig.StartBit() + sig.BitSize() - 1 == i_bit;
    case ISignal::EByteOrder::BigEndian:
        break;
    }
    return false;
}
const ISignal* get_mux_signal(const IMessage& msg)
{
    auto beg = msg.Signals().begin();
    auto end = msg.Signals().end();
    auto iter = std::find_if(beg, end,
        [&](const ISignal& sig)
        {
            return sig.MultiplexerIndicator() == ISignal::EMultiplexer::MuxSwitch;
        });
    return iter != end ? &*iter : nullptr;
}
std::set<uint64_t> get_mux_values(const IMessage& msg)
{
    std::set<uint64_t> mux_values;
    for (const ISignal& sig : msg.Signals())
    {
        if (sig.MultiplexerIndicator() == ISignal::EMultiplexer::MuxValue)
        {
            mux_values.insert(sig.MultiplexerSwitchValue());
        }
    }
    return mux_values;
}

DBCPPP_API std::ostream& dbcppp::Network2Human::operator<<(std::ostream& os, const IMessage& msg)
{
    os << boost::format("  %-12s%s\n") % "Name:" % msg.Name();
    os << boost::format("  %-12s0x%X\n") % "ID:" % msg.Id();
    os << boost::format("  %-12s0x%X\n") % "Length:" % msg.MessageSize();
    os << boost::format("  %-12s%s\n") % "Sender:" % msg.Transmitter();
    os << boost::format("  Layout:\n");

    auto mux_values = get_mux_values(msg);
    const ISignal* mux_sig = get_mux_signal(msg);

    std::vector<const ISignal*> sigs;
    for (const ISignal& sig : msg.Signals())
    {
        sigs.push_back(&sig);
    }
    std::size_t num_lines = 3 + msg.MessageSize() * 2 + sigs.size();
    std::size_t i = 0;
    std::size_t i_bit = 7;
    int64_t i_byte_str = int64_t(num_lines) / 2 + 2 - int64_t(num_lines);
    auto print_ident =
        [&]()
        {
            os << boost::format("         ");
        };
    auto print_bar =
        [&]()
        {
            print_ident();
            os << boost::format("  +---+---+---+---+---+---+---+---+\n");
        };
    auto print_signal_header =
        [&]()
        {
            print_ident();
            os << boost::format("                 Bit\n");
            print_ident();
            os << boost::format("    7   6   5   4   3   2   1   0\n");
        };
    bool print_last_line = false;
    auto print_signal =
        [&](uint64_t mux_value = -1)
        {
            print_bar();
            print_ident();
            os << boost::format("%d |") % (i_bit / 8);
            auto find_cur_sig =
                [&]()
                {
                    auto beg = msg.Signals().begin();
                    auto end = msg.Signals().end();
                    auto iter = std::find_if(beg, end, [&](const ISignal& sig) { return bit_is_inbetween(sig, i_bit, mux_value); });
                    return iter != end ? &*iter : nullptr;
                };
            int64_t depth = 0;
            for (std::size_t i = 0; i < 8; i++)
            {
                const auto* sig = find_cur_sig();
                if (sig)
                {
                    if (is_start_bit(*sig, i_bit) && is_end_bit(*sig, i_bit))
                    {
                        os << boost::format("<-x|");
                        depth++;
                    }
                    else if (is_start_bit(*sig, i_bit))
                    {
                        os << boost::format("--x|");
                        depth++;
                    }
                    else if (is_end_bit(*sig, i_bit))
                    {
                        os << boost::format("<--");
                        if (i_bit % 8 == 0)
                        {
                            os << boost::format("|");
                        }
                        else
                        {
                            os << boost::format("-");
                        }
                    }
                    else
                    {
                        os << boost::format("---");
                        if (i_bit % 8 == 0)
                        {
                            os << boost::format("|");
                        }
                        else
                        {
                            os << boost::format("-");
                        }
                    }
                }
                else
                {
                    os << boost::format("   |");
                }
                i_bit--;
            }
            os << boost::format("\n");
            if (depth)
            {
                print_last_line = false;
                print_ident();
                os << boost::format("  +---+---+---+---+---+---+---+---+\n");
            }
            else
            {
                print_last_line = true;
            }
            i_bit += 8;
            if (depth)
            {
                print_ident();
                os << boost::format("   ");
                std::size_t n_sigs = 1;
                for (; 0 < depth; depth--)
                {
                    n_sigs = 0;
                    for (std::size_t i = 0; i < 8; i++)
                    {
                        const auto* sig = find_cur_sig();
                        if (sig)
                        {
                            while (i_bit > sig->StartBit() && i < 8)
                            {
                                os << boost::format("    ");
                                i_bit--;
                                i++;
                            }
                            if (i == 8)
                            {
                                continue;
                            }
                            if (depth - 1 == n_sigs)
                            {
                                os << boost::format(" +-- %s") % sig->Name();
                            }
                            else if (n_sigs < std::size_t(depth - 1))
                            {
                                os << boost::format(" |  ");
                            }
                            n_sigs++;
                        }
                        else
                        {
                            os << boost::format("    ");
                        }
                        i_bit--;
                    }
                    i_bit += 8;
                    os << boost::format("\n");
                    if (depth != 1)
                    {
                        print_ident();
                        os << boost::format("   ");
                    }
                }
            }
            i_bit += 8;
        };
    auto print_mux_header =
        [&](uint64_t mux_value)
        {
            print_ident();
            os << boost::format("               %s: %d\n") % mux_sig->Name()  % mux_value;
        };

    if (mux_values.size() == 0)
    {
        print_signal_header();
        i_bit = 7;
        while (i_bit < msg.MessageSize() * 8)
        {
            print_signal();
        }
        if (print_last_line)
        {
            print_ident();
            os << boost::format("  +---+---+---+---+---+---+---+---+\n");
        }
    }
    else
    {
        for (uint64_t mux_value : mux_values)
        {
            print_mux_header(mux_value);
            print_signal_header();
            i_bit = 7;
            while (i_bit < msg.MessageSize() * 8)
            {
                print_signal(mux_value);
            }
            if (print_last_line)
            {
                print_ident();
                os << boost::format("  +---+---+---+---+---+---+---+---+\n");
            }
        }
    }
    os << boost::format("\n");
    auto print_signal_tree =
        [&]()
        {
            os << boost::format("  Signal tree:\n\n");
            os << boost::format("    -- {root}\n");
            if (mux_sig)
            {
                os << boost::format("       +-- %s\n") % mux_sig->Name();
                std::size_t i_mux = mux_values.size();
                for (uint64_t mux_value : mux_values)
                {
                    os << boost::format("       |   +--%d\n") % mux_value;
                    for (const ISignal& sig : msg.Signals())
                    {
                        if (sig.MultiplexerIndicator() == ISignal::EMultiplexer::MuxValue &&
                            sig.MultiplexerSwitchValue() == mux_value)
                        {
                            if (i_mux > 1)
                            {
                                os << boost::format("       |   |  +-- %s\n") % sig.Name();
                            }
                            else
                            {
                                os << boost::format("       |      +-- %s\n") % sig.Name();
                            }
                        }
                    }
                    i_mux--;
                }
            }
            for (const ISignal& sig : msg.Signals())
            {
                if (sig.MultiplexerIndicator() == ISignal::EMultiplexer::NoMux)
                {
                    os << boost::format("       +-- %s\n") % sig.Name();
                }
            }
        };
    print_signal_tree();
    os << boost::format("\n");
    auto print_value_tables =
        [&]()
        {
            os << boost::format("  Signal choices:\n\n");
            for (const ISignal& sig : msg.Signals())
            {
                if (sig.ValueEncodingDescriptions_Size())
                {
                    os << boost::format("    %s\n") % sig.Name();
                    for (const IValueEncodingDescription& ved : sig.ValueEncodingDescriptions())
                    {
                        os << boost::format("        %d %s\n") % ved.Value() % ved.Description();
                    }
                    os << boost::format("\n");
                }
            }
        };
    print_value_tables();
    return os;
}
DBCPPP_API std::ostream& dbcppp::Network2Human::operator<<(std::ostream& os, const INetwork& net)
{
    os << boost::format("================================= Messages =================================\n");
    for (const IMessage& msg : net.Messages())
    {
        os << msg;
        os << boost::format("  ------------------------------------------------------------------------\n");
    }
    return os;
}