#include "extractor/guidance/turn_instruction.hpp"

#include "engine/plugins/plugin_base.hpp"
#include "engine/plugins/tile.hpp"

#include "util/coordinate_calculation.hpp"
#include "util/string_view.hpp"
#include "util/vector_tile.hpp"
#include "util/web_mercator.hpp"

#include "engine/api/json_factory.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>

#include <protozero/pbf_writer.hpp>
#include <protozero/varint.hpp>

#include <algorithm>
#include <numeric>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <cmath>
#include <cstdint>

namespace osrm
{
namespace engine
{
namespace plugins
{

constexpr const static int MIN_ZOOM_FOR_TURNS = 15;

namespace
{

// Creates an indexed lookup table for values - used to encoded the vector tile
// which uses a lookup table and index pointers for encoding
template <typename T> struct ValueIndexer
{
  private:
    std::vector<T> used_values;
    std::unordered_map<T, std::size_t> value_offsets;

  public:
    std::size_t add(const T &value)
    {
        const auto found = value_offsets.find(value);
        std::size_t offset;

        if (found == value_offsets.end())
        {
            used_values.push_back(value);
            offset = used_values.size() - 1;
            value_offsets[value] = offset;
        }
        else
        {
            offset = found->second;
        }

        return offset;
    }

    std::size_t indexOf(const T &value) { return value_offsets[value]; };

    const std::vector<T> &values() { return used_values; }

    std::size_t size() const { return used_values.size(); }
};

using RTreeLeaf = datafacade::BaseDataFacade::RTreeLeaf;
// TODO: Port all this encoding logic to https://github.com/mapbox/vector-tile, which wasn't
// available when this code was originally written.

// Simple container class for WGS84 coordinates
template <typename T> struct Point final
{
    Point(T _x, T _y) : x(_x), y(_y) {}

    const T x;
    const T y;
};

// Simple container to hold a bounding box
struct BBox final
{
    BBox(const double _minx, const double _miny, const double _maxx, const double _maxy)
        : minx(_minx), miny(_miny), maxx(_maxx), maxy(_maxy)
    {
    }

    double width() const { return maxx - minx; }
    double height() const { return maxy - miny; }

    const double minx;
    const double miny;
    const double maxx;
    const double maxy;
};

// Simple container for integer coordinates (i.e. pixel coords)
struct point_type_i final
{
    point_type_i(std::int64_t _x, std::int64_t _y) : x(_x), y(_y) {}

    const std::int64_t x;
    const std::int64_t y;
};

using FixedPoint = Point<std::int32_t>;
using FloatPoint = Point<double>;

using FixedLine = std::vector<FixedPoint>;
using FloatLine = std::vector<FloatPoint>;

// We use boost::geometry to clip lines/points that are outside or cross the boundary
// of the tile we're rendering.  We need these types defined to use boosts clipping
// logic
typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> point_t;
typedef boost::geometry::model::linestring<point_t> linestring_t;
typedef boost::geometry::model::box<point_t> box_t;
typedef boost::geometry::model::multi_linestring<linestring_t> multi_linestring_t;
const static box_t clip_box(point_t(-util::vector_tile::BUFFER, -util::vector_tile::BUFFER),
                            point_t(util::vector_tile::EXTENT + util::vector_tile::BUFFER,
                                    util::vector_tile::EXTENT + util::vector_tile::BUFFER));

// from mapnik-vector-tile
// Encodes a linestring using protobuf zigzag encoding
inline bool encodeLinestring(const FixedLine &line,
                             protozero::packed_field_uint32 &geometry,
                             std::int32_t &start_x,
                             std::int32_t &start_y)
{
    const std::size_t line_size = line.size();
    if (line_size < 2)
    {
        return false;
    }

    const unsigned lineto_count = static_cast<const unsigned>(line_size) - 1;

    auto pt = line.begin();
    const constexpr int MOVETO_COMMAND = 9;
    geometry.add_element(MOVETO_COMMAND); // move_to | (1 << 3)
    geometry.add_element(protozero::encode_zigzag32(pt->x - start_x));
    geometry.add_element(protozero::encode_zigzag32(pt->y - start_y));
    start_x = pt->x;
    start_y = pt->y;
    // This means LINETO repeated N times
    // See: https://github.com/mapbox/vector-tile-spec/tree/master/2.1#example-command-integers
    geometry.add_element((lineto_count << 3u) | 2u);
    // Now that we've issued the LINETO REPEAT N command, we append
    // N coordinate pairs immediately after the command.
    for (++pt; pt != line.end(); ++pt)
    {
        const std::int32_t dx = pt->x - start_x;
        const std::int32_t dy = pt->y - start_y;
        geometry.add_element(protozero::encode_zigzag32(dx));
        geometry.add_element(protozero::encode_zigzag32(dy));
        start_x = pt->x;
        start_y = pt->y;
    }
    return true;
}

// from mapnik-vctor-tile
// Encodes a point
inline void encodePoint(const FixedPoint &pt, protozero::packed_field_uint32 &geometry)
{
    const constexpr int MOVETO_COMMAND = 9;
    geometry.add_element(MOVETO_COMMAND);
    const std::int32_t dx = pt.x;
    const std::int32_t dy = pt.y;
    // Manual zigzag encoding.
    geometry.add_element(protozero::encode_zigzag32(dx));
    geometry.add_element(protozero::encode_zigzag32(dy));
}

/**
 * Returnx the x1,y1,x2,y2 pixel coordinates of a line in a given
 * tile.
 *
 * @param start the first coordinate of the line
 * @param target the last coordinate of the line
 * @param tile_bbox the boundaries of the tile, in mercator coordinates
 * @return a FixedLine with coordinates relative to the tile_bbox.
 */
FixedLine coordinatesToTileLine(const util::Coordinate start,
                                const util::Coordinate target,
                                const BBox &tile_bbox)
{
    FloatLine geo_line;
    geo_line.emplace_back(static_cast<double>(util::toFloating(start.lon)),
                          static_cast<double>(util::toFloating(start.lat)));
    geo_line.emplace_back(static_cast<double>(util::toFloating(target.lon)),
                          static_cast<double>(util::toFloating(target.lat)));

    linestring_t unclipped_line;

    for (auto const &pt : geo_line)
    {
        double px_merc = pt.x * util::web_mercator::DEGREE_TO_PX;
        double py_merc = util::web_mercator::latToY(util::FloatLatitude{pt.y}) *
                         util::web_mercator::DEGREE_TO_PX;
        // convert lon/lat to tile coordinates
        const auto px = std::round(
            ((px_merc - tile_bbox.minx) * util::web_mercator::TILE_SIZE / tile_bbox.width()) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);
        const auto py = std::round(
            ((tile_bbox.maxy - py_merc) * util::web_mercator::TILE_SIZE / tile_bbox.height()) *
            util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE);

        boost::geometry::append(unclipped_line, point_t(px, py));
    }

    multi_linestring_t clipped_line;

    boost::geometry::intersection(clip_box, unclipped_line, clipped_line);

    FixedLine tile_line;

    // b::g::intersection might return a line with one point if the
    // original line was very short and coords were dupes
    if (!clipped_line.empty() && clipped_line[0].size() == 2)
    {
        if (clipped_line[0].size() == 2)
        {
            for (const auto &p : clipped_line[0])
            {
                tile_line.emplace_back(p.get<0>(), p.get<1>());
            }
        }
    }

    return tile_line;
}

/**
 * Converts lon/lat into coordinates inside a Mercator projection tile (x/y pixel values)
 *
 * @param point the lon/lat you want the tile coords for
 * @param tile_bbox the mercator boundaries of the tile
 * @return a point (x,y) on the tile defined by tile_bbox
 */
FixedPoint coordinatesToTilePoint(const util::Coordinate point, const BBox &tile_bbox)
{
    const FloatPoint geo_point{static_cast<double>(util::toFloating(point.lon)),
                               static_cast<double>(util::toFloating(point.lat))};

    const double px_merc = geo_point.x * util::web_mercator::DEGREE_TO_PX;
    const double py_merc = util::web_mercator::latToY(util::FloatLatitude{geo_point.y}) *
                           util::web_mercator::DEGREE_TO_PX;

    const auto px = static_cast<std::int32_t>(std::round(
        ((px_merc - tile_bbox.minx) * util::web_mercator::TILE_SIZE / tile_bbox.width()) *
        util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE));
    const auto py = static_cast<std::int32_t>(std::round(
        ((tile_bbox.maxy - py_merc) * util::web_mercator::TILE_SIZE / tile_bbox.height()) *
        util::vector_tile::EXTENT / util::web_mercator::TILE_SIZE));

    return FixedPoint{px, py};
}

std::vector<RTreeLeaf> getEdges(const DataFacadeBase &facade, unsigned x, unsigned y, unsigned z)
{
    double min_lon, min_lat, max_lon, max_lat;

    // Convert the z,x,y mercator tile coordinates into WGS84 lon/lat values
    //
    util::web_mercator::xyzToWGS84(
        x, y, z, min_lon, min_lat, max_lon, max_lat, util::web_mercator::TILE_SIZE * 0.10);

    util::Coordinate southwest{util::FloatLongitude{min_lon}, util::FloatLatitude{min_lat}};
    util::Coordinate northeast{util::FloatLongitude{max_lon}, util::FloatLatitude{max_lat}};

    // Fetch all the segments that are in our bounding box.
    // This hits the OSRM StaticRTree
    return facade.GetEdgesInBox(southwest, northeast);
}

std::vector<std::size_t> getEdgeIndex(const std::vector<RTreeLeaf> &edges)
{
    // In order to ensure consistent tile encoding, we need to process
    // all edges in the same order.  Differences in OSX/Linux/Windows
    // sorting methods mean that GetEdgesInBox doesn't return the same
    // ordered array on all platforms.
    // GetEdgesInBox is marked `const`, so we can't sort the array itself,
    // instead we create an array of indexes and sort that instead.
    std::vector<std::size_t> sorted_edge_indexes(edges.size(), 0);
    std::iota(
        sorted_edge_indexes.begin(), sorted_edge_indexes.end(), 0); // fill with 0,1,2,3,...N-1

    // Now, sort that array based on the edges list, using the u/v node IDs
    // as the sort condition
    std::sort(sorted_edge_indexes.begin(),
              sorted_edge_indexes.end(),
              [&edges](const std::size_t &left, const std::size_t &right) -> bool {
                  return (edges[left].u != edges[right].u) ? edges[left].u < edges[right].u
                                                           : edges[left].v < edges[right].v;
              });

    return sorted_edge_indexes;
}

void encodeVectorTile(const DataFacadeBase &facade,
                      unsigned x,
                      unsigned y,
                      unsigned z,
                      const std::vector<RTreeLeaf> &edges,
                      const std::vector<std::size_t> &sorted_edge_indexes,
                      const std::vector<routing_algorithms::TurnData> &all_turn_data,
                      std::string &pbf_buffer)
{

    std::uint8_t max_datasource_id = 0;

    // Vector tiles encode properties on features as indexes into a layer-specific
    // lookup table.  These ValueIndexer's act as memoizers for values as we discover
    // them during edge explioration, and are then used to generate the lookup
    // tables for each tile layer.
    ValueIndexer<int> line_int_index;
    ValueIndexer<util::StringView> line_string_index;
    ValueIndexer<int> point_int_index;
    ValueIndexer<float> point_float_index;
    ValueIndexer<std::string> point_string_index;

    const auto get_geometry_id = [&facade](auto edge) {
        return facade.GetGeometryIndex(edge.forward_segment_id.id).id;
    };

    // Vector tiles encode feature properties as indexes into a lookup table.  So, we need
    // to "pre-loop" over all the edges to create the lookup tables.  Once we have those, we
    // can then encode the features, and we'll know the indexes that feature properties
    // need to refer to.
    for (const auto &edge_index : sorted_edge_indexes)
    {
        const auto &edge = edges[edge_index];

        const auto geometry_id = get_geometry_id(edge);
        const auto forward_datasource_vector =
            facade.GetUncompressedForwardDatasources(geometry_id);
        const auto reverse_datasource_vector =
            facade.GetUncompressedReverseDatasources(geometry_id);

        BOOST_ASSERT(edge.fwd_segment_position < forward_datasource_vector.size());
        const auto forward_datasource = forward_datasource_vector[edge.fwd_segment_position];
        BOOST_ASSERT(edge.fwd_segment_position < reverse_datasource_vector.size());
        const auto reverse_datasource = reverse_datasource_vector[reverse_datasource_vector.size() -
                                                                  edge.fwd_segment_position - 1];

        // Keep track of the highest datasource seen so that we don't write unnecessary
        // data to the layer attribute values
        max_datasource_id = std::max(max_datasource_id, forward_datasource);
        max_datasource_id = std::max(max_datasource_id, reverse_datasource);
    }

    // Convert tile coordinates into mercator coordinates
    double min_mercator_lon, min_mercator_lat, max_mercator_lon, max_mercator_lat;
    util::web_mercator::xyzToMercator(
        x, y, z, min_mercator_lon, min_mercator_lat, max_mercator_lon, max_mercator_lat);
    const BBox tile_bbox{min_mercator_lon, min_mercator_lat, max_mercator_lon, max_mercator_lat};

    // Protobuf serializes blocks when objects go out of scope, hence
    // the extra scoping below.
    protozero::pbf_writer tile_writer{pbf_buffer};
    {
        {
            // Add a layer object to the PBF stream.  3=='layer' from the vector tile spec
            // (2.1)
            protozero::pbf_writer line_layer_writer(tile_writer, util::vector_tile::LAYER_TAG);
            // TODO: don't write a layer if there are no features

            line_layer_writer.add_uint32(util::vector_tile::VERSION_TAG, 2); // version
            // Field 1 is the "layer name" field, it's a string
            line_layer_writer.add_string(util::vector_tile::NAME_TAG, "speeds"); // name
            // Field 5 is the tile extent.  It's a uint32 and should be set to 4096
            // for normal vector tiles.
            line_layer_writer.add_uint32(util::vector_tile::EXTENT_TAG,
                                         util::vector_tile::EXTENT); // extent

            // Because we need to know the indexes into the vector tile lookup table,
            // we need to do an initial pass over the data and create the complete
            // index of used values.
            for (const auto &edge_index : sorted_edge_indexes)
            {
                const auto &edge = edges[edge_index];
                const auto geometry_id = get_geometry_id(edge);

                // Get coordinates for start/end nodes of segment (NodeIDs u and v)
                const auto a = facade.GetCoordinateOfNode(edge.u);
                const auto b = facade.GetCoordinateOfNode(edge.v);
                // Calculate the length in meters
                const double length = osrm::util::coordinate_calculation::haversineDistance(a, b);

                // Weight values
                const auto forward_weight_vector =
                    facade.GetUncompressedForwardWeights(geometry_id);
                const auto reverse_weight_vector =
                    facade.GetUncompressedReverseWeights(geometry_id);
                const auto forward_weight = forward_weight_vector[edge.fwd_segment_position];
                const auto reverse_weight = reverse_weight_vector[reverse_weight_vector.size() -
                                                                  edge.fwd_segment_position - 1];
                line_int_index.add(forward_weight);
                line_int_index.add(reverse_weight);

                std::uint32_t forward_rate =
                    static_cast<std::uint32_t>(round(length / forward_weight * 10.));
                std::uint32_t reverse_rate =
                    static_cast<std::uint32_t>(round(length / reverse_weight * 10.));

                line_int_index.add(forward_rate);
                line_int_index.add(reverse_rate);

                // Duration values
                const auto forward_duration_vector =
                    facade.GetUncompressedForwardDurations(geometry_id);
                const auto reverse_duration_vector =
                    facade.GetUncompressedReverseDurations(geometry_id);
                const auto forward_duration = forward_duration_vector[edge.fwd_segment_position];
                const auto reverse_duration =
                    reverse_duration_vector[reverse_duration_vector.size() -
                                            edge.fwd_segment_position - 1];
                line_int_index.add(forward_duration);
                line_int_index.add(reverse_duration);
            }

            // Begin the layer features block
            {
                // Each feature gets a unique id, starting at 1
                unsigned id = 1;
                for (const auto &edge_index : sorted_edge_indexes)
                {
                    const auto &edge = edges[edge_index];
                    const auto geometry_id = get_geometry_id(edge);

                    // Get coordinates for start/end nodes of segment (NodeIDs u and v)
                    const auto a = facade.GetCoordinateOfNode(edge.u);
                    const auto b = facade.GetCoordinateOfNode(edge.v);
                    // Calculate the length in meters
                    const double length =
                        osrm::util::coordinate_calculation::haversineDistance(a, b);

                    const auto forward_weight_vector =
                        facade.GetUncompressedForwardWeights(geometry_id);
                    const auto reverse_weight_vector =
                        facade.GetUncompressedReverseWeights(geometry_id);
                    const auto forward_duration_vector =
                        facade.GetUncompressedForwardDurations(geometry_id);
                    const auto reverse_duration_vector =
                        facade.GetUncompressedReverseDurations(geometry_id);
                    const auto forward_datasource_vector =
                        facade.GetUncompressedForwardDatasources(geometry_id);
                    const auto reverse_datasource_vector =
                        facade.GetUncompressedReverseDatasources(geometry_id);
                    const auto forward_weight = forward_weight_vector[edge.fwd_segment_position];
                    const auto reverse_weight =
                        reverse_weight_vector[reverse_weight_vector.size() -
                                              edge.fwd_segment_position - 1];
                    const auto forward_duration =
                        forward_duration_vector[edge.fwd_segment_position];
                    const auto reverse_duration =
                        reverse_duration_vector[reverse_duration_vector.size() -
                                                edge.fwd_segment_position - 1];
                    const auto forward_datasource_idx =
                        forward_datasource_vector[edge.fwd_segment_position];
                    const auto reverse_datasource_idx =
                        reverse_datasource_vector[reverse_datasource_vector.size() -
                                                  edge.fwd_segment_position - 1];

                    const auto component_id = facade.GetComponentID(edge.forward_segment_id.id);
                    const auto name_id = facade.GetNameIndex(edge.forward_segment_id.id);
                    auto name = facade.GetNameForID(name_id);

                    line_string_index.add(name);

                    const auto encode_tile_line = [&line_layer_writer,
                                                   &edge,
                                                   &component_id,
                                                   &id,
                                                   &max_datasource_id,
                                                   &line_int_index](
                        const FixedLine &tile_line,
                        const std::uint32_t speed_kmh_idx,
                        const std::uint32_t rate_idx,
                        const std::size_t weight_idx,
                        const std::size_t duration_idx,
                        const DatasourceID datasource_idx,
                        const std::size_t name_idx,
                        std::int32_t &start_x,
                        std::int32_t &start_y) {
                        // Here, we save the two attributes for our feature: the speed and
                        // the is_small boolean.  We only serve up speeds from 0-139, so all we
                        // do is save the first
                        protozero::pbf_writer feature_writer(line_layer_writer,
                                                             util::vector_tile::FEATURE_TAG);
                        // Field 3 is the "geometry type" field.  Value 2 is "line"
                        feature_writer.add_enum(
                            util::vector_tile::GEOMETRY_TAG,
                            util::vector_tile::GEOMETRY_TYPE_LINE); // geometry type
                        // Field 1 for the feature is the "id" field.
                        feature_writer.add_uint64(util::vector_tile::ID_TAG, id++); // id
                        {
                            // When adding attributes to a feature, we have to write
                            // pairs of numbers.  The first value is the index in the
                            // keys array (written later), and the second value is the
                            // index into the "values" array (also written later).  We're
                            // not writing the actual speed or bool value here, we're saving
                            // an index into the "values" array.  This means many features
                            // can share the same value data, leading to smaller tiles.
                            protozero::packed_field_uint32 field(
                                feature_writer, util::vector_tile::FEATURE_ATTRIBUTES_TAG);

                            field.add_element(0); // "speed" tag key offset
                            field.add_element(std::min(
                                speed_kmh_idx, 127u)); // save the speed value, capped at 127
                            field.add_element(1);      // "is_small" tag key offset
                            field.add_element(
                                128 + (component_id.is_tiny ? 0 : 1)); // is_small feature offset
                            field.add_element(2);                    // "datasource" tag key offset
                            field.add_element(130 + datasource_idx); // datasource value offset
                            field.add_element(3);                    // "weight" tag key offset
                            field.add_element(130 + max_datasource_id + 1 +
                                              weight_idx); // weight value offset
                            field.add_element(4);          // "duration" tag key offset
                            field.add_element(130 + max_datasource_id + 1 +
                                              duration_idx); // duration value offset
                            field.add_element(5);            // "name" tag key offset

                            field.add_element(130 + max_datasource_id + 1 +
                                              line_int_index.values().size() + name_idx);

                            field.add_element(6); // rate tag key offset
                            field.add_element(130 + max_datasource_id + 1 + rate_idx);
                        }
                        {

                            // Encode the geometry for the feature
                            protozero::packed_field_uint32 geometry(
                                feature_writer, util::vector_tile::FEATURE_GEOMETRIES_TAG);
                            encodeLinestring(tile_line, geometry, start_x, start_y);
                        }
                    };

                    // If this is a valid forward edge, go ahead and add it to the tile
                    if (forward_duration != 0 && edge.forward_segment_id.enabled)
                    {
                        std::int32_t start_x = 0;
                        std::int32_t start_y = 0;

                        // Calculate the speed for this line
                        // Speeds are looked up in a simple 1:1 table, so the speed value == lookup
                        // table index
                        std::uint32_t speed_kmh_idx =
                            static_cast<std::uint32_t>(round(length / forward_duration * 10 * 3.6));

                        // Rate values are in meters per weight-unit - and similar to speeds, we
                        // present 1 decimal place of precision (these values are added as
                        // double/10) lower down
                        std::uint32_t forward_rate =
                            static_cast<std::uint32_t>(round(length / forward_weight * 10.));

                        auto tile_line = coordinatesToTileLine(a, b, tile_bbox);
                        if (!tile_line.empty())
                        {
                            encode_tile_line(tile_line,
                                             speed_kmh_idx,
                                             line_int_index.indexOf(forward_rate),
                                             line_int_index.indexOf(forward_weight),
                                             line_int_index.indexOf(forward_duration),
                                             forward_datasource_idx,
                                             line_string_index.indexOf(name),
                                             start_x,
                                             start_y);
                        }
                    }

                    // Repeat the above for the coordinates reversed and using the `reverse`
                    // properties
                    if (reverse_duration != 0 && edge.reverse_segment_id.enabled)
                    {
                        std::int32_t start_x = 0;
                        std::int32_t start_y = 0;

                        // Calculate the speed for this line
                        // Speeds are looked up in a simple 1:1 table, so the speed value == lookup
                        // table index
                        std::uint32_t speed_kmh_idx =
                            static_cast<std::uint32_t>(round(length / reverse_duration * 10 * 3.6));

                        // Rate values are in meters per weight-unit - and similar to speeds, we
                        // present 1 decimal place of precision (these values are added as
                        // double/10) lower down
                        std::uint32_t reverse_rate =
                            static_cast<std::uint32_t>(round(length / reverse_weight * 10.));

                        auto tile_line = coordinatesToTileLine(b, a, tile_bbox);
                        if (!tile_line.empty())
                        {
                            encode_tile_line(tile_line,
                                             speed_kmh_idx,
                                             line_int_index.indexOf(reverse_rate),
                                             line_int_index.indexOf(reverse_weight),
                                             line_int_index.indexOf(reverse_duration),
                                             reverse_datasource_idx,
                                             line_string_index.indexOf(name),
                                             start_x,
                                             start_y);
                        }
                    }
                }
            }

            // Field id 3 is the "keys" attribute
            // We need two "key" fields, these are referred to with 0 and 1 (their array
            // indexes) earlier
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "speed");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "is_small");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "datasource");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "weight");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "duration");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "name");
            line_layer_writer.add_string(util::vector_tile::KEY_TAG, "rate");

            // Now, we write out the possible speed value arrays and possible is_tiny
            // values.  Field type 4 is the "values" field.  It's a variable type field,
            // so requires a two-step write (create the field, then write its value)
            for (std::size_t i = 0; i < 128; i++)
            {
                // Writing field type 4 == variant type
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 5 == uint64 type
                values_writer.add_uint64(util::vector_tile::VARIANT_TYPE_UINT64, i);
            }
            {
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 7 == bool type
                values_writer.add_bool(util::vector_tile::VARIANT_TYPE_BOOL, true);
            }
            {
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 7 == bool type
                values_writer.add_bool(util::vector_tile::VARIANT_TYPE_BOOL, false);
            }
            for (std::size_t i = 0; i <= max_datasource_id; i++)
            {
                // Writing field type 4 == variant type
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 1 == string type
                values_writer.add_string(util::vector_tile::VARIANT_TYPE_STRING,
                                         facade.GetDatasourceName(i).to_string());
            }
            for (auto value : line_int_index.values())
            {
                // Writing field type 4 == variant type
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 2 == float type
                // Durations come out of OSRM in integer deciseconds, so we convert them
                // to seconds with a simple /10 for display
                values_writer.add_double(util::vector_tile::VARIANT_TYPE_DOUBLE, value / 10.);
            }

            for (const auto &name : line_string_index.values())
            {
                // Writing field type 4 == variant type
                protozero::pbf_writer values_writer(line_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                // Attribute value 1 == string type
                values_writer.add_string(
                    util::vector_tile::VARIANT_TYPE_STRING, name.data(), name.size());
            }
        }

        // Only add the turn layer to the tile if it has some features (we sometimes won't
        // for tiles of z<16, and tiles that don't show any intersections)
        if (!all_turn_data.empty())
        {

            struct EncodedTurnData
            {
                util::Coordinate coordinate;
                std::size_t angle_index;
                std::size_t turn_index;
                std::size_t duration_index;
                std::size_t weight_index;
                std::size_t turntype_index;
                std::size_t turnmodifier_index;
            };
            // we need to pre-encode all values here because we need the full offsets later
            // for encoding the actual features.
            std::vector<EncodedTurnData> encoded_turn_data(all_turn_data.size());
            std::transform(
                all_turn_data.begin(),
                all_turn_data.end(),
                encoded_turn_data.begin(),
                [&](const routing_algorithms::TurnData &t) {
                    auto angle_idx = point_int_index.add(t.in_angle);
                    auto turn_idx = point_int_index.add(t.turn_angle);
                    auto duration_idx =
                        point_float_index.add(t.duration / 10.0); // Note conversion to float here
                    auto weight_idx =
                        point_float_index.add(t.weight / 10.0); // Note conversion to float here

                    auto turntype_idx =
                        point_string_index.add(extractor::guidance::internalInstructionTypeToString(
                            t.turn_instruction.type));
                    auto turnmodifier_idx =
                        point_string_index.add(extractor::guidance::instructionModifierToString(
                            t.turn_instruction.direction_modifier));
                    return EncodedTurnData{t.coordinate,
                                           angle_idx,
                                           turn_idx,
                                           duration_idx,
                                           weight_idx,
                                           turntype_idx,
                                           turnmodifier_idx};
                });

            // Now write the points layer for turn penalty data:
            // Add a layer object to the PBF stream.  3=='layer' from the vector tile spec
            // (2.1)
            protozero::pbf_writer point_layer_writer(tile_writer, util::vector_tile::LAYER_TAG);
            point_layer_writer.add_uint32(util::vector_tile::VERSION_TAG, 2);    // version
            point_layer_writer.add_string(util::vector_tile::NAME_TAG, "turns"); // name
            point_layer_writer.add_uint32(util::vector_tile::EXTENT_TAG,
                                          util::vector_tile::EXTENT); // extent

            // Begin writing the set of point features
            {
                // Start each features with an ID starting at 1
                int id = 1;

                // Helper function to encode a new point feature on a vector tile.
                const auto encode_tile_point = [&](const FixedPoint &tile_point,
                                                   const auto &point_turn_data) {
                    protozero::pbf_writer feature_writer(point_layer_writer,
                                                         util::vector_tile::FEATURE_TAG);
                    // Field 3 is the "geometry type" field.  Value 1 is "point"
                    feature_writer.add_enum(
                        util::vector_tile::GEOMETRY_TAG,
                        util::vector_tile::GEOMETRY_TYPE_POINT);                // geometry type
                    feature_writer.add_uint64(util::vector_tile::ID_TAG, id++); // id
                    {
                        // Write out the 4 properties we want on the feature.  These
                        // refer to indexes in the properties lookup table, which we
                        // add to the tile after we add all features.
                        protozero::packed_field_uint32 field(
                            feature_writer, util::vector_tile::FEATURE_ATTRIBUTES_TAG);
                        field.add_element(0); // "bearing_in" tag key offset
                        field.add_element(point_turn_data.angle_index);
                        field.add_element(1); // "turn_angle" tag key offset
                        field.add_element(point_turn_data.turn_index);
                        field.add_element(2); // "cost" tag key offset
                        field.add_element(point_int_index.size() + point_turn_data.duration_index);
                        field.add_element(3); // "weight" tag key offset
                        field.add_element(point_int_index.size() + point_turn_data.weight_index);
                        field.add_element(4); // "type" tag key offset
                        field.add_element(point_int_index.size() + point_float_index.size() +
                                          point_turn_data.turntype_index);
                        field.add_element(5); // "modifier" tag key offset
                        field.add_element(point_int_index.size() + point_float_index.size() +
                                          point_turn_data.turnmodifier_index);
                    }
                    {
                        // Add the geometry as the last field in this feature
                        protozero::packed_field_uint32 geometry(
                            feature_writer, util::vector_tile::FEATURE_GEOMETRIES_TAG);
                        encodePoint(tile_point, geometry);
                    }
                };

                // Loop over all the turns we found and add them as features to the layer
                for (const auto &turndata : encoded_turn_data)
                {
                    const auto tile_point = coordinatesToTilePoint(turndata.coordinate, tile_bbox);
                    if (!boost::geometry::within(point_t(tile_point.x, tile_point.y), clip_box))
                    {
                        continue;
                    }
                    encode_tile_point(tile_point, turndata);
                }
            }

            // Add the names of the three attributes we added to all the turn penalty
            // features previously.  The indexes used there refer to these keys.
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "bearing_in");
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "turn_angle");
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "cost");
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "weight");
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "type");
            point_layer_writer.add_string(util::vector_tile::KEY_TAG, "modifier");

            // Now, save the lists of integers and floats that our features refer to.
            for (const auto &value : point_int_index.values())
            {
                protozero::pbf_writer values_writer(point_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                values_writer.add_sint64(util::vector_tile::VARIANT_TYPE_SINT64, value);
            }
            for (const auto &value : point_float_index.values())
            {
                protozero::pbf_writer values_writer(point_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                values_writer.add_float(util::vector_tile::VARIANT_TYPE_FLOAT, value);
            }
            for (const auto &value : point_string_index.values())
            {
                protozero::pbf_writer values_writer(point_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                values_writer.add_string(util::vector_tile::VARIANT_TYPE_STRING, value);
            }
        }

        // OSM Node tile layer
        {
            protozero::pbf_writer point_layer_writer(tile_writer, util::vector_tile::LAYER_TAG);
            point_layer_writer.add_uint32(util::vector_tile::VERSION_TAG, 2);       // version
            point_layer_writer.add_string(util::vector_tile::NAME_TAG, "osmnodes"); // name
            point_layer_writer.add_uint32(util::vector_tile::EXTENT_TAG,
                                          util::vector_tile::EXTENT); // extent

            std::vector<NodeID> internal_nodes;
            internal_nodes.reserve(edges.size() * 2);
            for (const auto &edge : edges)
            {
                internal_nodes.push_back(edge.u);
                internal_nodes.push_back(edge.v);
            }
            std::sort(internal_nodes.begin(), internal_nodes.end());
            auto new_end = std::unique(internal_nodes.begin(), internal_nodes.end());
            internal_nodes.resize(new_end - internal_nodes.begin());

            for (const auto &internal_node : internal_nodes)
            {
                const auto coord = facade.GetCoordinateOfNode(internal_node);
                const auto tile_point = coordinatesToTilePoint(coord, tile_bbox);
                if (!boost::geometry::within(point_t(tile_point.x, tile_point.y), clip_box))
                {
                    continue;
                }
                protozero::pbf_writer feature_writer(point_layer_writer,
                                                     util::vector_tile::FEATURE_TAG);
                // Field 3 is the "geometry type" field.  Value 1 is "point"
                feature_writer.add_enum(util::vector_tile::GEOMETRY_TAG,
                                        util::vector_tile::GEOMETRY_TYPE_POINT); // geometry type
                const auto osmid =
                    static_cast<OSMNodeID::value_type>(facade.GetOSMNodeIDOfNode(internal_node));
                feature_writer.add_uint64(util::vector_tile::ID_TAG, osmid); // id
                // There are no additional properties, just the ID and the geometry
                {
                    // Add the geometry as the last field in this feature
                    protozero::packed_field_uint32 geometry(
                        feature_writer, util::vector_tile::FEATURE_GEOMETRIES_TAG);
                    encodePoint(tile_point, geometry);
                }
            }
        }
    }
    // protozero serializes data during object destructors, so once the scope closes,
    // our result buffer will have all the tile data encoded into it.
}
}

Status TilePlugin::HandleRequest(const RoutingAlgorithmsInterface &algorithms,
                                 const api::TileParameters &parameters,
                                 std::string &pbf_buffer) const
{
    BOOST_ASSERT(parameters.IsValid());

    const auto &facade = algorithms.GetFacade();
    auto edges = getEdges(facade, parameters.x, parameters.y, parameters.z);

    auto edge_index = getEdgeIndex(edges);

    std::vector<routing_algorithms::TurnData> turns;

    // If we're zooming into 16 or higher, include turn data.  Why?  Because turns make the map
    // really cramped, so we don't bother including the data for tiles that span a large area.
    if (parameters.z >= MIN_ZOOM_FOR_TURNS && algorithms.HasGetTileTurns())
    {
        turns = algorithms.GetTileTurns(edges, edge_index);
    }

    encodeVectorTile(
        facade, parameters.x, parameters.y, parameters.z, edges, edge_index, turns, pbf_buffer);

    return Status::Ok;
}
}
}
}


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:51:19

#ifndef MSSM_PHYSICAL_H
#define MSSM_PHYSICAL_H

#include "linalg2.hpp"
#include <Eigen/Core>

#include <iosfwd>
#include <string>

namespace flexiblesusy {

struct MSSM_physical {
   MSSM_physical();
   void clear();
   void convert_to_hk();   ///< converts pole masses to HK convention
   void convert_to_slha(); ///< converts pole masses to SLHA convention
   Eigen::ArrayXd get() const; ///< returns array with all masses and mixings
   void set(const Eigen::ArrayXd&); ///< set all masses and mixings
   Eigen::ArrayXd get_masses() const; ///< returns array with all masses
   void set_masses(const Eigen::ArrayXd&); ///< set all masses
   void print(std::ostream&) const;

   double MVG;
   double MGlu;
   Eigen::Array<double,3,1> MFv;
   Eigen::Array<double,6,1> MSd;
   Eigen::Array<double,3,1> MSv;
   Eigen::Array<double,6,1> MSu;
   Eigen::Array<double,6,1> MSe;
   Eigen::Array<double,2,1> Mhh;
   Eigen::Array<double,2,1> MAh;
   Eigen::Array<double,2,1> MHpm;
   Eigen::Array<double,4,1> MChi;
   Eigen::Array<double,2,1> MCha;
   Eigen::Array<double,3,1> MFe;
   Eigen::Array<double,3,1> MFd;
   Eigen::Array<double,3,1> MFu;
   double MVWm;
   double MVP;
   double MVZ;

   Eigen::Matrix<double,6,6> ZD;
   Eigen::Matrix<double,3,3> ZV;
   Eigen::Matrix<double,6,6> ZU;
   Eigen::Matrix<double,6,6> ZE;
   Eigen::Matrix<double,2,2> ZH;
   Eigen::Matrix<double,2,2> ZA;
   Eigen::Matrix<double,2,2> ZP;
   Eigen::Matrix<std::complex<double>,4,4> ZN;
   Eigen::Matrix<std::complex<double>,2,2> UM;
   Eigen::Matrix<std::complex<double>,2,2> UP;
   Eigen::Matrix<std::complex<double>,3,3> ZEL;
   Eigen::Matrix<std::complex<double>,3,3> ZER;
   Eigen::Matrix<std::complex<double>,3,3> ZDL;
   Eigen::Matrix<std::complex<double>,3,3> ZDR;
   Eigen::Matrix<std::complex<double>,3,3> ZUL;
   Eigen::Matrix<std::complex<double>,3,3> ZUR;
   Eigen::Matrix<double,2,2> ZZ;

};

std::ostream& operator<<(std::ostream&, const MSSM_physical&);

} // namespace flexiblesusy

#endif


#include <FeePolicyEstimator.h>

#include <boost/foreach.hpp>
#include <streams.h>
#include <Logging.h>
#include <MemPoolEntry.h>

template <typename T>
std::vector<T> buf2vec(boost::circular_buffer<T> buf)
{
    std::vector<T> vec(buf.begin(), buf.end());
    return vec;
}

CBlockAverage::CBlockAverage() : feeSamples(100), prioritySamples(100) {}

void CBlockAverage::RecordFee(const CFeeRate& feeRate)
{
    feeSamples.push_back(feeRate);
}

void CBlockAverage::RecordPriority(double priority)
{
    prioritySamples.push_back(priority);
}

size_t CBlockAverage::FeeSamples() const { return feeSamples.size(); }
size_t CBlockAverage::GetFeeSamples(std::vector<CFeeRate>& insertInto) const
{
    BOOST_FOREACH (const CFeeRate& f, feeSamples)
        insertInto.push_back(f);
    return feeSamples.size();
}
size_t CBlockAverage::PrioritySamples() const { return prioritySamples.size(); }
size_t CBlockAverage::GetPrioritySamples(std::vector<double>& insertInto) const
{
    BOOST_FOREACH (double d, prioritySamples)
        insertInto.push_back(d);
    return prioritySamples.size();
}

/**
 * Used as belt-and-suspenders check when reading to detect
 * file corruption
 */
bool CBlockAverage::AreSane(const CFeeRate fee, const CFeeRate& minRelayFee)
{
    if (fee < CFeeRate(0))
        return false;
    if (fee.GetFeePerK() > minRelayFee.GetFeePerK() * 10000)
        return false;
    return true;
}
bool CBlockAverage::AreSane(const std::vector<CFeeRate>& vecFee, const CFeeRate& minRelayFee)
{
    BOOST_FOREACH (CFeeRate fee, vecFee) {
        if (!AreSane(fee, minRelayFee))
            return false;
    }
    return true;
}
bool CBlockAverage::AreSane(const double priority)
{
    return priority >= 0;
}
bool CBlockAverage::AreSane(const std::vector<double> vecPriority)
{
    BOOST_FOREACH (double priority, vecPriority) {
        if (!AreSane(priority))
            return false;
    }
    return true;
}

void CBlockAverage::Write(CAutoFile& fileout) const
{
    std::vector<CFeeRate> vecFee = buf2vec(feeSamples);
    fileout << vecFee;
    std::vector<double> vecPriority = buf2vec(prioritySamples);
    fileout << vecPriority;
}

void CBlockAverage::Read(CAutoFile& filein, const CFeeRate& minRelayFee)
{
    std::vector<CFeeRate> vecFee;
    filein >> vecFee;
    if (AreSane(vecFee, minRelayFee))
        feeSamples.insert(feeSamples.end(), vecFee.begin(), vecFee.end());
    else
        throw std::runtime_error("Corrupt fee value in estimates file.");
    std::vector<double> vecPriority;
    filein >> vecPriority;
    if (AreSane(vecPriority))
        prioritySamples.insert(prioritySamples.end(), vecPriority.begin(), vecPriority.end());
    else
        throw std::runtime_error("Corrupt priority value in estimates file.");
    if (feeSamples.size() + prioritySamples.size() > 0)
        LogPrint("estimatefee", "Read %d fee samples and %d priority samples\n",
            feeSamples.size(), prioritySamples.size());
}

FeePolicyEstimator::FeePolicyEstimator(
    int nEntries
    ): history()
    , sortedFeeSamples()
    , sortedPrioritySamples()
    , nBestSeenHeight(0)
{
    history.resize(nEntries);
}

void FeePolicyEstimator::seenTxConfirm(const CFeeRate& feeRate, const CFeeRate& minRelayFee, double coinAgeOfInputsPerByte, int nBlocksAgo)
{
    // Last entry records "everything else".
    int nBlocksTruncated = std::min(nBlocksAgo, (int)history.size() - 1);
    assert(nBlocksTruncated >= 0);

    // We need to guess why the transaction was included in a block-- either
    // because it is high-priority or because it has sufficient fees.
    bool sufficientFee = (feeRate > minRelayFee);
    bool sufficientPriority = CTxMemPoolEntry::AllowFree(coinAgeOfInputsPerByte);
    const char* assignedTo = "unassigned";
    if (sufficientFee && !sufficientPriority && CBlockAverage::AreSane(feeRate, minRelayFee)) {
        history[nBlocksTruncated].RecordFee(feeRate);
        assignedTo = "fee";
    } else if (sufficientPriority && !sufficientFee && CBlockAverage::AreSane(coinAgeOfInputsPerByte)) {
        history[nBlocksTruncated].RecordPriority(coinAgeOfInputsPerByte);
        assignedTo = "priority";
    } else {
        // Neither or both fee and priority sufficient to get confirmed:
        // don't know why they got confirmed.
    }
    LogPrint("estimatefee", "Seen TX confirm: %s : %s fee/%g priority, took %d blocks\n",
        assignedTo, feeRate, coinAgeOfInputsPerByte, nBlocksAgo);
}

void FeePolicyEstimator::seenBlock(const std::vector<const CTxMemPoolEntry*>& entries, int nBlockHeight, const CFeeRate minRelayFee)
{
    if (nBlockHeight <= nBestSeenHeight) {
        // Ignore side chains and re-orgs; assuming they are random
        // they don't affect the estimate.
        // And if an attacker can re-org the chain at will, then
        // you've got much bigger problems than "attacker can influence
        // transaction fees."
        return;
    }
    nBestSeenHeight = nBlockHeight;

    // Fill up the history buckets based on how long transactions took
    // to confirm.
    std::vector<std::vector<const CTxMemPoolEntry*> > entriesByConfirmations;
    entriesByConfirmations.resize(history.size());
    BOOST_FOREACH (const CTxMemPoolEntry* entryReference, entries) {
        // How many blocks did it take for miners to include this transaction?
        if (!entryReference) continue;
        const CTxMemPoolEntry& entry = *entryReference;
        int delta = nBlockHeight - entry.GetHeight();
        if (delta <= 0) {
            // Re-org made us lose height, this should only happen if we happen
            // to re-org on a difficulty transition point: very rare!
            continue;
        }
        if ((delta - 1) >= (int)history.size())
            delta = history.size(); // Last bucket is catch-all
        entriesByConfirmations.at(delta - 1).push_back(&entry);
    }
    for (size_t i = 0; i < entriesByConfirmations.size(); i++) {
        std::vector<const CTxMemPoolEntry*>& e = entriesByConfirmations.at(i);
        // Insert at most 10 random entries per bucket, otherwise a single block
        // can dominate an estimate:
        if (e.size() > 10) {
            std::random_shuffle(e.begin(), e.end());
            e.resize(10);
        }
        BOOST_FOREACH (const CTxMemPoolEntry* entry, e) {
            // Fees are stored and reported as DIVI-per-kb:
            CFeeRate feeRate(entry->GetFee(), entry->GetTxSize());
            double coinAgeOfInputsPerByte = entry->ComputeInputCoinAgePerByte(entry->GetHeight()); // Want priority when it went IN
            seenTxConfirm(feeRate, minRelayFee, coinAgeOfInputsPerByte, i);
        }
    }

    //After new samples are added, we have to clear the sorted lists,
    //so they'll be resorted the next time someone asks for an estimate
    sortedFeeSamples.clear();
    sortedPrioritySamples.clear();

    for (size_t i = 0; i < history.size(); i++) {
        if (history[i].FeeSamples() + history[i].PrioritySamples() > 0)
            LogPrint("estimatefee", "estimates: for confirming within %d blocks based on %d/%d samples, fee=%s, prio=%g\n",
                i,
                history[i].FeeSamples(), history[i].PrioritySamples(),
                estimateFee(i + 1), estimatePriority(i + 1));
    }
}

/**
 * Can return CFeeRate(0) if we don't have any data for that many blocks back. nBlocksToConfirm is 1 based.
 */
CFeeRate FeePolicyEstimator::estimateFee(int nBlocksToConfirm)
{
    nBlocksToConfirm--;

    if (nBlocksToConfirm < 0 || nBlocksToConfirm >= (int)history.size())
        return CFeeRate(0);

    if (sortedFeeSamples.size() == 0) {
        for (size_t i = 0; i < history.size(); i++)
            history.at(i).GetFeeSamples(sortedFeeSamples);
        std::sort(sortedFeeSamples.begin(), sortedFeeSamples.end(),
            std::greater<CFeeRate>());
    }
    if (sortedFeeSamples.size() < 11) {
        // Eleven is Gavin's Favorite Number
        // ... but we also take a maximum of 10 samples per block so eleven means
        // we're getting samples from at least two different blocks
        return CFeeRate(0);
    }

    int nBucketSize = history.at(nBlocksToConfirm).FeeSamples();

    // Estimates should not increase as number of confirmations goes up,
    // but the estimates are noisy because confirmations happen discretely
    // in blocks. To smooth out the estimates, use all samples in the history
    // and use the nth highest where n is (number of samples in previous bucket +
    // half the samples in nBlocksToConfirm bucket):
    size_t nPrevSize = 0;
    for (int i = 0; i < nBlocksToConfirm; i++)
        nPrevSize += history.at(i).FeeSamples();
    size_t index = std::min(nPrevSize + nBucketSize / 2, sortedFeeSamples.size() - 1);
    return sortedFeeSamples[index];
}
double FeePolicyEstimator::estimatePriority(int nBlocksToConfirm)
{
    nBlocksToConfirm--;

    if (nBlocksToConfirm < 0 || nBlocksToConfirm >= (int)history.size())
        return -1;

    if (sortedPrioritySamples.size() == 0) {
        for (size_t i = 0; i < history.size(); i++)
            history.at(i).GetPrioritySamples(sortedPrioritySamples);
        std::sort(sortedPrioritySamples.begin(), sortedPrioritySamples.end(),
            std::greater<double>());
    }
    if (sortedPrioritySamples.size() < 11)
        return -1.0;

    int nBucketSize = history.at(nBlocksToConfirm).PrioritySamples();

    // Estimates should not increase as number of confirmations needed goes up,
    // but the estimates are noisy because confirmations happen discretely
    // in blocks. To smooth out the estimates, use all samples in the history
    // and use the nth highest where n is (number of samples in previous buckets +
    // half the samples in nBlocksToConfirm bucket).
    size_t nPrevSize = 0;
    for (int i = 0; i < nBlocksToConfirm; i++)
        nPrevSize += history.at(i).PrioritySamples();
    size_t index = std::min(nPrevSize + nBucketSize / 2, sortedPrioritySamples.size() - 1);
    return sortedPrioritySamples[index];
}

void FeePolicyEstimator::Write(CAutoFile& fileout) const
{
    fileout << nBestSeenHeight;
    fileout << history.size();
    BOOST_FOREACH (const CBlockAverage& entry, history) {
        entry.Write(fileout);
    }
}

void FeePolicyEstimator::Read(CAutoFile& filein, const CFeeRate& minRelayFee)
{
    int nFileBestSeenHeight;
    filein >> nFileBestSeenHeight;
    size_t numEntries;
    filein >> numEntries;
    if (numEntries <= 0 || numEntries > 10000)
        throw std::runtime_error("Corrupt estimates file. Must have between 1 and 10k entries.");

    std::vector<CBlockAverage> fileHistory;

    for (size_t i = 0; i < numEntries; i++) {
        CBlockAverage entry;
        entry.Read(filein, minRelayFee);
        fileHistory.push_back(entry);
    }

    // Now that we've processed the entire fee estimate data file and not
    // thrown any errors, we can copy it to our history
    nBestSeenHeight = nFileBestSeenHeight;
    history = fileHistory;
    assert(history.size() > 0);
}


#include <iostream>
#include <fstream>
#include <iterator>
#include <limits.h>
#include <cmath>
#include <immintrin.h>
#include <omp.h>
#include "bayes.hpp"
#include "utilities.hpp"
#include "dotp_lut.hpp"
#include "na_lut.hpp"
#include "xfiles.hpp"
#include <boost/math/special_functions/gamma.hpp>

void Bayes::predict() {

    MPI_Barrier(MPI_COMM_WORLD);
    double ts = MPI_Wtime();

    check_openmp();

    MPI_Status status;
    MPI_Offset file_size = 0;
    MPI_File*  fh;

    cross_bim_files();

    int pidx = 0;
    for (auto& phen : pmgr.get_phens()) {
        pidx += 1;

        phen.delete_output_prediction_files();
        phen.open_prediction_files();

        const std::vector<unsigned char> mask4 = phen.get_mask4();
        const int im4 = phen.get_im4();

        fh = phen.get_inbet_fh();
        check_mpi(MPI_File_get_size(*fh, &file_size), __LINE__, __FILE__);
        //printf("file_size = %u B\n", file_size);

        // First element of the .bet is the total number of processed markers
        // Then: iteration (uint) beta (double) for all markers
        uint Mtot_ = 0;
        MPI_Offset betoff = size_t(0);
        check_mpi(MPI_File_read_at_all(*fh, betoff, &Mtot_, 1, MPI_UNSIGNED, &status), __LINE__, __FILE__);
        if (Mtot_ != m_refrsid.size()) {
            printf("Mismatch between expected and Mtot read from .bet file: %lu vs %d\n", rsid.size(), Mtot_);
            MPI_Abort(MPI_COMM_WORLD, 1);
        }

        assert((file_size - sizeof(uint)) % (Mtot_ * sizeof(double) + sizeof(uint)) == 0);
        uint niter = (file_size - sizeof(uint)) / (Mtot_ * sizeof(double) + sizeof(uint));
        if (rank == 0)
            printf("INFO   : Number of recorded iterations in .bet file %d: %u\n", pidx-1, niter);

        double* beta_sum = (double*) _mm_malloc(size_t(Mtot_) * sizeof(double), 32);
        check_malloc(beta_sum, __LINE__, __FILE__);
        for (int i=0; i<Mtot_; i++) beta_sum[i] = 0.0;

        double* beta_it = (double*) _mm_malloc(size_t(Mtot_) * sizeof(double), 32);
        check_malloc(beta_it, __LINE__, __FILE__);

        uint start_iter = 0;
        //EO: use this one to speed up testing (avoids to read the entire bet history)
        //if (niter > 3) start_iter = niter - 3;

        for (uint i=start_iter; i<niter; i++) {
            betoff
                = sizeof(uint) // Mtot
                + (sizeof(uint) + size_t(Mtot_) * sizeof(double)) * size_t(i)
                + sizeof(uint);
            check_mpi(MPI_File_read_at_all(*fh, betoff, beta_it, Mtot_, MPI_DOUBLE, &status), __LINE__, __FILE__);
            for (int j=0; j<Mtot_;j++)
                beta_sum[j] += beta_it[j];
        }

        for (int j=0; j<Mtot_;j++)
            beta_sum[j] /= double(niter);

        fflush(stdout);
        double t_1 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 1 = %.2f seconds.\n", t_1 - ts);

        double* g_k = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(g_k, __LINE__, __FILE__);
        for (int i=0; i<im4*4; i++) g_k[i] = 0.0;

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
        for (int mrki=0; mrki<M; mrki++) {

            // Get rsid from current
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);

            // Skip markers with no corresponding rsid in reference bim file
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //printf("%d -> %d = %s not found in reference bim\n", mrki, mglo, id.c_str());
                continue;
            }

            int rmglo = m_refrsid.find(id)->second; // global marker index in reference bed

            size_t bedix = size_t(mrki) * size_t(mbytes);
            const unsigned char* bedm = &bed_data[bedix];

            double mave = phen.get_marker_ave(mrki);
            double msig = phen.get_marker_sig(mrki);

            for (int j=0; j<im4; j++) {
                for (int k=0; k<4; k++) {
                    double val = (dotp_lut_a[bedm[j] * 4 + k] - mave) * dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k] * msig;
#ifdef _OPENMP
#pragma omp atomic update
#endif
                    g_k[j*4+k] += val * beta_sum[mglo];
                }
            }
        }

        fflush(stdout);
        double t_2 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 2 = %.2f seconds.\n", t_2 - t_1);

        double* g = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(g, __LINE__, __FILE__);

        check_mpi(MPI_Allreduce(g_k, g, im4*4, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD), __LINE__, __FILE__);

        double* y_k = (double*) _mm_malloc(size_t(im4*4) * sizeof(double), 32);
        check_malloc(y_k, __LINE__, __FILE__);

        phen.get_centered_and_scaled_y(y_k);

        //EO: already done when computing original epsilon when reading .phen
	//phen.set_nas_to_zero(y_k, im4*4);
        // NAs are 0.0 in all, so should be safe
        for (int i=0; i<im4*4; i++)
            y_k[i] -= (g[i] - g_k[i]);

        double sigma = 0.0;
        for (int i=0; i<N; i++)
            sigma += y_k[i] * y_k[i];
        sigma /= phen.get_nonas();
        //printf("### r: %d sigma = %20.15f\n", rank, sigma);

        MPI_File* mlma_fh = phen.get_outmlma_fh();

        double* Beta  = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Beta, __LINE__, __FILE__);
        double* Tdist = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Tdist, __LINE__, __FILE__);
        double* Se    = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Se, __LINE__, __FILE__);
        double* Pval  = (double*) _mm_malloc(size_t(M) * sizeof(double), 32);
        check_malloc(Pval, __LINE__, __FILE__);

#ifdef _OPENMP
#pragma omp parallel for
#endif
        for (int mrki=0; mrki<M; mrki++) {

            // Global index in current .bim
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);

            // Skip markers with no corresponding rsid in reference bim file
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //printf("%d -> %d = %s not found in reference bim\n", mrki, mglo, id.c_str());
                continue;
            }

            // Global marker index in reference bed
            int rmglo = m_refrsid.find(id)->second;

            size_t bedix = size_t(mrki) * size_t(mbytes);
            const unsigned char* bedm = &bed_data[bedix];

            double mave = phen.get_marker_ave(mrki);
            double msig = phen.get_marker_sig(mrki);

            double xtx = 0.0;
            double xty = 0.0;
            double chk = 0.0;
            for (int j=0; j<im4; j++) {
                for (int k=0; k<4; k++) {
                    double val = dotp_lut_a[bedm[j] * 4 + k] * dotp_lut_b[bedm[j] * 4 + k] * na_lut[mask4[j] * 4 + k];
                    xtx += val * val;
                    xty += val * y_k[j*4 + k];
                }
            }

            double beta  = xty / xtx;
            double tdist = xty / sqrt(sigma * xtx);
            double se    = beta / tdist;
            double pval  = 1.0 - boost::math::gamma_p(0.5, tdist * tdist * 0.5);

            Beta[mrki]  = beta;
            Tdist[mrki] = tdist;
            Se[mrki]    = se;
            Pval[mrki]  = pval;

            //if (mrki < 5) {
            //    printf("r: %3d  m: %8d %8d (%5s) %8d  beta=%20.15f  se=%20.15f  tdist=%20.15f pval=%20.15f   xtx=%10.1f xty=%15.6f\n", rank, mrki, mglo, id.c_str(), rmglo, beta, se, tdist, pval, xtx, xty);
            //}
        }

        fflush(stdout);
        double t_3 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 3 = %.2f seconds.\n", t_3 - t_2);

        const int LLEN = 123 + 1;
        char* todump  = (char*) _mm_malloc(size_t(LLEN) * size_t(M) * sizeof(char), 32);
        check_malloc(todump, __LINE__, __FILE__);

        int n_rem = 0;
        for (int mrki=0; mrki<M; mrki++) {
            int mglo = S + mrki;
            std::string id = rsid.at(mglo);
            if (m_refrsid.find(id) == m_refrsid.end()) {
                //|| id.compare("rs12562034") == 0m|| id.compare("rs188466450") == 0)
                printf("WARNING: marker id %s excluded -- no match\n", id.c_str());
                n_rem++;
                continue;
            }
            int rmglo = m_refrsid.find(id)->second;
            int cx = snprintf(&todump[(mrki - n_rem) * (LLEN - 1)], LLEN, "%20s %8d %8d %20.15f %20.15f %20.15f %20.15f\n",
                              id.c_str(), mglo, rmglo, Beta[mrki], Tdist[mrki], Se[mrki], Pval[mrki]);
            assert(cx >= 0 && cx < LLEN);
        }

        // Collect numbers of markers to print in each task
        int mp = M - n_rem;
        int* mps = (int*) malloc(nranks * sizeof(int));
        check_mpi(MPI_Allgather(&mp, 1, MPI_INTEGER, mps, 1, MPI_INTEGER, MPI_COMM_WORLD), __LINE__, __FILE__);

        int ps = 0;
        for (int i=0; i<rank; i++) { ps += mps[i]; }

        MPI_Offset offset = size_t(ps) * size_t(LLEN-1);
        check_mpi(MPI_File_write_at(*mlma_fh,
                                    offset, todump, size_t(LLEN-1) * size_t(mp), MPI_CHAR, &status),
                  __LINE__, __FILE__);

        _mm_free(todump);

        fflush(stdout);
        double t_4 = MPI_Wtime();
        MPI_Barrier(MPI_COMM_WORLD);
        //if (rank >= 0)
        //    printf("INFO   : intermediate time 4 = %.2f seconds.\n", t_4 - t_3);

        MPI_Barrier(MPI_COMM_WORLD);

        phen.close_prediction_files();

        _mm_free(Beta);
        _mm_free(Tdist);
        _mm_free(Se);
        _mm_free(Pval);
        _mm_free(beta_sum);
        _mm_free(beta_it);
        _mm_free(g_k);
        _mm_free(g);
        _mm_free(y_k);
    }

    fflush(stdout);
    double te = MPI_Wtime();
    MPI_Barrier(MPI_COMM_WORLD);
    if (rank == 0)
        printf("INFO   : Time to compute the predictions: %.2f seconds.\n", te - ts);
}

// EO: bim_file - I assume that the row number is the index
//
void Bayes::cross_bim_files() {

    if (rank == 0) {
        printf("INFO   : bim file:     %s\n", opt.get_bim_file().c_str());
        printf("INFO   : ref bim file: %s\n", opt.get_ref_bim_file().c_str());
    }
    std::ifstream in(opt.get_bim_file().c_str());
    if (!in) throw ("Error: can not open the file [" + opt.get_bim_file() + "] to read.");
    std::string   id, allele1, allele2;
    unsigned chr, physPos, idx = 0;
    float    genPos;
    while (in >> chr >> id >> genPos >> physPos >> allele1 >> allele2) {
        rsid.push_back(id);
    }
    in.close();
    int nrsid = rsid.size();
    if (rank == 0)
        printf("INFO   : found %d ids in bim file\n", nrsid);

    std::ifstream refin(opt.get_ref_bim_file().c_str());
    if (!refin) throw ("Error: can not open the file [" + opt.get_ref_bim_file() + "] to read.");
    idx = 0;
    while (refin >> chr >> id >> genPos >> physPos >> allele1 >> allele2) {
        m_refrsid[id] = idx++;
    }
    refin.close();
    if (rank == 0)
        printf("INFO   : found %d ids in reference bim file\n", idx);
}

void Bayes::process() {

    check_openmp();

    for (auto& phen : pmgr.get_phens()) {
        phen.delete_output_files();
        phen.open_output_files();
        phen.set_midx();
        for (int i=0; i<opt.get_ngroups(); i++) {
            phen.set_sigmag_for_group(i, phen.sample_beta_rng(1.0, 1.0));
            if (mtotgrp.at(i) == 0)
                phen.set_sigmag_for_group(i, 0.0);
            //printf("sample sigmag[%d] = %20.15f\n", i, phen.get_sigmag_for_group(i));
        }
        check_mpi(MPI_Bcast(phen.get_sigmag()->data(), phen.get_sigmag()->size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
        phen.set_pi_est(pi_prior);
        //printf("sample sigmag[0] = %20.15f\n", phen.get_sigmag()->at(0));
    }

    const int NPHEN =  pmgr.get_phens().size();
    bool recv_update[nranks];

    for (unsigned int it = 1; it <= opt.get_iterations(); it++) {

        double ts_it = MPI_Wtime();

        if (rank == 0)
            printf("\n\n@@@ ITERATION %5d\n", it);

        int pidx = 0;
        for (auto& phen : pmgr.get_phens()) {
            pidx += 1;
            //printf("phen %d has mu = %20.15f\n", pidx, phen.get_mu());
            phen.offset_epsilon(phen.get_mu());
            //phen.update_epsilon_sum();
            if (it == 1) {
                phen.update_epsilon_sigma();
                //printf("epssum = %20.15f, sigmae = %20.15f\n", phen.get_epsilon_sum(), phen.get_sigmae());
            }
            phen.set_mu(phen.sample_norm_rng());
            //printf("new mu = %20.15f\n", phen.get_mu());
            phen.offset_epsilon(-phen.get_mu());
            //**BUG in original phen.epsilon_stats();

            // Shuffling of the markers on its own PRNG (see README/wiki)
            if (opt.shuffle_markers())
                phen.shuffle_midx(opt.mimic_hydra());

            phen.reset_m0();
            phen.reset_cass();
        }
        fflush(stdout);

        double dbetas[NPHEN * 3]; // [ dbeta:mave:msig | ... | ]

        double t_it_sync = 0.0;

        for (int mrki=0; mrki<Mm; mrki++) {

            bool share_mrk = false;
            for (int i=0; i<NPHEN*3; i++)
                dbetas[i] = 0.0;
            int mloc = 0;

            if (mrki < M) {

                mloc = pmgr.get_phens()[0].get_marker_local_index(mrki);
                const int mglo = S + mloc;
                const int mgrp = get_marker_group(mglo);
                //std::cout << "mloc = " << mloc << ", mglo = " << mglo << ", mgrp = " << mgrp << std::endl;

                int pheni = -1;

                for (auto& phen : pmgr.get_phens()) {

                    pheni += 1;

                    // Adav
                    if (phen.get_sigmag_for_group(mgrp) == 0.0) {
                        phen.set_marker_acum(mloc, 1.0);
                        phen.set_marker_beta(mloc, 0.0);
                        continue;
                    }

                    double beta   = phen.get_marker_beta(mloc);
                    double sige_g = phen.get_sigmae() / phen.get_sigmag_for_group(get_marker_group(mglo));
                    double sigg_e = 1.0 / sige_g;
                    double inv2sige = 1.0 / (2.0 * phen.get_sigmae());
                    //if (mrki < 3)
                    //    printf("mrk = %3d has beta = %20.15f; sige_g = %20.15f = %20.15f / %20.15f\n", mloc, phen.get_marker_beta(mloc), sige_g, phen.get_sigmae(), phen.get_sigmag());

                    std::vector<double> denom = phen.get_denom();
                    std::vector<double> muk   = phen.get_muk();
                    std::vector<double> logl  = phen.get_logl();

                    for (int i=1; i<=K-1; ++i) {
                        denom.at(i-1) = (double)(N - 1) + sige_g * cvai[mgrp][i];
                        //printf("it %d, rank %d, m %d: denom[%d] = %20.15f, cvai = %20.15f\n", it, rank, mloc, i-1, denom.at(i-1), cvai[mgrp][i]);
                    }

                    double num = dot_product(mloc, phen.get_epsilon(), phen.get_marker_ave(mloc), phen.get_marker_sig(mloc));

                    //printf("num = %20.15f\n", num);
                    num += beta * double(phen.get_nonas() - 1);

                    //printf("i:%d r:%d m:%d: num = %.17g, %20.15f, %20.15f\n", it, rank, mloc, num, phen.get_marker_ave(mloc), phen.get_marker_sig(mloc));

                    for (int i=1; i<=K-1; ++i)
                        muk.at(i) = num / denom.at(i - 1);

                    for (int i=0; i<K; i++) {
                        logl[i] = log(phen.get_pi_est(mgrp, i));
                        if (i>0)
                            logl[i] += -0.5 * log(sigg_e * double(phen.get_nonas() - 1) * cva[mgrp][i] + 1.0) + muk[i] * num * inv2sige;
                        //printf("logl[%d] = %20.15f\n", i, logl[i]);
                    }

                    double prob = phen.sample_unif_rng();

                    bool zero_acum = false;
                    double tmp1 = 0.0;
                    for (int i=0; i<K; i++) {
                        if (abs(logl[i] - logl[0]) > 700.0)
                            zero_acum = true;
                        tmp1 += exp(logl[i] - logl[0]);
                    }
                    zero_acum ? tmp1 = 0.0 : tmp1 = 1.0 / tmp1;
                    phen.set_marker_acum(mloc, tmp1);
                    //printf("i:%d r:%d m:%d p:%d  num = %20.15f, acum = %20.15f, prob = %20.15f\n", it, rank, mloc, pheni, num, phen.get_marker_acum(mloc), prob);

                    double dbeta = phen.get_marker_beta(mloc);

                    for (int i=0; i<K; i++) {
                        if (prob <= phen.get_marker_acum(mloc) || i == K - 1) {
                            if (i == 0) {
                                phen.set_marker_beta(mloc, 0.0);
                                //printf("@0@ i:%4d r:%4d m:%4d: beta reset to 0.0\n", it, rank, mloc);
                            } else {
                                phen.set_marker_beta(mloc, phen.sample_norm_rng(muk[i], phen.get_sigmae() / denom[i-1]));
                                //printf("@B@ i:%4d r:%4d m:%4d:  dbetat = %20.15f, muk[%4d] = %15.10f with prob=%15.10f <= acum = %15.10f, denom = %15.10f, sigmaE = %15.10f: beta = %15.10f\n", it, rank, mloc, phen.get_marker_beta(mloc) - dbeta, i, muk[i], prob, phen.get_marker_acum(mloc), denom[i-1], phen.get_sigmae(), phen.get_marker_beta(mloc));
                                //fflush(stdout);
                            }
                            phen.increment_cass(mgrp, i, 1);
                            //std::cout << "cass " << mgrp << " " << i << " = " << phen.get_cass_for_group(mgrp,i) << std::endl;
                            phen.set_comp(mloc, i);
                            break;
                        } else {
                            bool zero_inc = false;
                            for (int j=i+1; j<K; j++) {
                                if (abs(logl[j] - logl[i+1]) > 700.0)
                                    zero_inc = true;
                            }
                            if (!zero_inc) {
                                double esum = 0.0;
                                for (int k=0; k<logl.size(); k++)
                                    esum += exp(logl[k] - logl[i+1]);
                                phen.set_marker_acum(mloc, phen.get_marker_acum(mloc) + 1.0 / esum);
                            }
                        }
                    }

                    dbeta -= phen.get_marker_beta(mloc);

                    //printf("iteration %3d, rank %3d, marker %5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni + 1], dbetas[pheni + 2]);

                    if (abs(dbeta) > 0.0) {
                        share_mrk = true;
                        dbetas[pheni * 3 + 0] = dbeta;
                        dbetas[pheni * 3 + 1] = phen.get_marker_ave(mloc);
                        dbetas[pheni * 3 + 2] = phen.get_marker_sig(mloc);
                        //printf("@¢@ i:%3d r:%3d m:%5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni * 3 + 1], dbetas[pheni * 3 + 2]);
                    } else {
                        //printf("-¢- i:%3d r:%3d m:%5d: dbeta = %20.15f, pheni = %d, %20.15f %20.15f\n", it, rank, mloc, dbeta, pheni, dbetas[pheni * 3 + 1], dbetas[pheni * 3 + 2]);
                    }
                }
            }

            MPI_Barrier(MPI_COMM_WORLD);
            double ts_sync = MPI_Wtime();

            // Collect information on which tasks need to share its just processed marker;
            // Tasks with M < Mm, share_mrk is false by default.
            MPI_Allgather(&share_mrk,  1, MPI_C_BOOL,
                          recv_update, 1, MPI_C_BOOL,
                          MPI_COMM_WORLD);

            /*
            if (rank == 0) {
                printf("rank:share mrk %d?: ", mrki);
                for (int i=0; i<nranks; i++) {
                    printf("%d:%d ", i, recv_update[i] == true ? 1:0);
                }
                printf("\n");
            }
            fflush(stdout);
            */

            int totbytes = 0;

            int dis_bet[nranks], cnt_bet[nranks];
            int dis_bed[nranks], cnt_bed[nranks];
            int disp_bet = 0, disp_bed = 0;
            for (int i=0; i<nranks; i++) {
                if (recv_update[i]) {
                    cnt_bet[i] = NPHEN * 3;
                    cnt_bed[i] = mbytes;
                    totbytes += mbytes;
                } else {
                    cnt_bet[i] = 0;
                    cnt_bed[i] = 0;
                }
                dis_bet[i] = disp_bet;
                dis_bed[i] = disp_bed;
                //printf("mrki = %d, recv_update[%d] = %s %d:%d\n", mrki, i, recv_update[i] ? "T" : "F", dis_bet[i], cnt_bet[i]);
                disp_bet += cnt_bet[i];
                disp_bed += cnt_bed[i];
            }

            //printf("rank %d bytes vs %d\n", totbytes, disp_bed);

            double recv_dbetas[disp_bet];
            MPI_Allgatherv(&dbetas, share_mrk ? NPHEN * 3 : 0, MPI_DOUBLE,
                           recv_dbetas, cnt_bet, dis_bet, MPI_DOUBLE, MPI_COMM_WORLD);

            unsigned char* recv_bed = (unsigned char*) _mm_malloc(disp_bed, 32);
            check_malloc(recv_bed, __LINE__, __FILE__);
            MPI_Allgatherv(&bed_data[mloc * mbytes], share_mrk ? mbytes : 0, MPI_UNSIGNED_CHAR,
                           recv_bed, cnt_bed, dis_bed, MPI_UNSIGNED_CHAR, MPI_COMM_WORLD);

            update_epsilon(cnt_bet, recv_dbetas, recv_bed);

            MPI_Barrier(MPI_COMM_WORLD);
            double te_sync = MPI_Wtime();
            t_it_sync += te_sync - ts_sync;

            _mm_free(recv_bed);

        } // End marker loop

        //exit(0);
        //continue;

        //MPI_Barrier(MPI_COMM_WORLD);

        int pheni = -1;
        for (auto& phen : pmgr.get_phens()) {
            pheni++;

            phen.reset_beta_sqn_to_zero();
            for (int i=0; i<M; i++) {
                phen.increment_beta_sqn(get_marker_group(S + i), phen.get_marker_beta(i) * phen.get_marker_beta(i));
            }

            //for (int i=0; i<G; i++)
            //    printf("i:%d r:%d p:%d g:%d: beta_sqn[%d] = %20.15f\n", it, rank, pheni, i, i, phen.get_beta_sqn_for_group(i));

            double* beta_sqn_sum = (double*) malloc(G * sizeof(double));
            check_mpi(MPI_Allreduce(phen.get_beta_sqn()->data(),
                                    beta_sqn_sum,
                                    G,
                                    MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_beta_sqn(beta_sqn_sum);
            free(beta_sqn_sum);

            int* cass_sum = (int*) malloc(G * K * sizeof(int));
            check_mpi(MPI_Allreduce(phen.get_cass(),
                                    cass_sum,
                                    G * K,
                                    MPI_INT,
                                    MPI_SUM,
                                    MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_cass(cass_sum);
            free(cass_sum);

            // Update global parameters
            //
            for (int i=0; i<G; i++) {

                // Skip empty groups
                if (mtotgrp.at(i) == 0)
                    continue;

                //printf(" !!! i:%d r:%d p:%d g:%d:  OLD sigmag = %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i));

                //if (rank == 0)
                //    printf("i:%d r:%d p:%d g:%d:  m0 = %d - %d = %d\n", it, rank, pheni, i, mtotgrp.at(i), phen.get_cass_for_group(i, 0), mtotgrp.at(i) - phen.get_cass_for_group(i, 0));

                phen.set_m0_for_group(i, mtotgrp.at(i) - phen.get_cass_for_group(i, 0));

                // Skip groups with m0 being null or empty cass (adaV in action)
                if (phen.get_m0_for_group(i) == 0 || phen.get_cass_sum_for_group(i) == 0) {
                    phen.set_sigmag_for_group(i, 0.0);
                    continue;
                }

                phen.set_sigmag_for_group(i, phen.sample_inv_scaled_chisq_rng(V0G + (double) phen.get_m0_for_group(i), (phen.get_beta_sqn_for_group(i) * (double) phen.get_m0_for_group(i) + V0G * S02G) / (V0G + (double) phen.get_m0_for_group(i))));
                //printf(" !!! i:%d r:%d p:%d g:%d:  NEW sigmag = %20.15f %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i), phen.get_beta_sqn_for_group(i));

                phen.update_pi_est_dirichlet(i);
            }

            //continue;

            //if (rank == 0)
            //    phen.print_cass(mtotgrp);

            // Broadcast sigmaG of rank 0
            check_mpi(MPI_Bcast(phen.get_sigmag()->data(), phen.get_sigmag()->size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            for (int i=0; i<opt.get_ngroups(); i++) {
                //printf("i:%d r:%d p:%d g:%d:  sigmag = %20.15f\n", it, rank, pheni, i, phen.get_sigmag_for_group(i));
            }

            double e_sqn = phen.epsilon_sumsqr();
            //printf("i:%d r:%d p:%d  e_sqn = %20.15f\n", it, rank, pheni, e_sqn);

            //EO: sample sigmaE and broadcast the one from rank 0 to all the others
            phen.set_sigmae(phen.sample_inv_scaled_chisq_rng(V0E + (double)N, (e_sqn + V0E * S02E) / (V0E + (double)N)));

            double sigmae_r0 = phen.get_sigmae();
            check_mpi(MPI_Bcast(&sigmae_r0, 1, MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            phen.set_sigmae(sigmae_r0);
            if (rank % 10 == 0) {
                printf("RESULT : i:%d r:%d p:%d  sum sigmaG = %20.15f  sigmaE = %20.15f\n", it, rank, pheni, phen.get_sigmag_sum(), phen.get_sigmae());
                //for (int i=0; i<opt.get_ngroups(); i++) {
                //    printf("i:%d r:%d p:%d  sigmaG = %20.15f  sigmaE = %20.15f\n", it, rank, pheni, phen.get_sigmag_for_group(i), phen.get_sigmae());
                //}
            }

            // Broadcast pi_est from rank 0
            for (int i=0; i<G; i++) {
                check_mpi(MPI_Bcast(phen.get_pi_est()->at(i).data(), phen.get_pi_est()->at(i).size(), MPI_DOUBLE, 0, MPI_COMM_WORLD), __LINE__, __FILE__);
            }
        }

        double te_it = MPI_Wtime();
        if (rank == 0)
            printf("RESULT : It %d  total proc time = %7.3f sec, with sync time = %7.3f\n", it, te_it - ts_it, t_it_sync);

        // Write output files
        if (it % opt.get_output_thin_rate() == 0) {
            const unsigned nthinned = it / opt.get_output_thin_rate() - 1;
            for (auto& phen : pmgr.get_phens()) {
                if (rank == 0) {
                    //phen.print_pi_est();
                    write_ofile_csv(*(phen.get_outcsv_fh()), it,  phen.get_sigmag(), phen.get_sigmae(), phen.get_m0_sum(), nthinned, phen.get_pi_est());
                }
                write_ofile_h1(*(phen.get_outbet_fh()), rank, Mt, it, nthinned, S, M, phen.get_betas().data(), MPI_DOUBLE);
                write_ofile_h1(*(phen.get_outcpn_fh()), rank, Mt, it, nthinned, S, M, phen.get_comp().data(),  MPI_INTEGER);
            }
        }

    } // End iteration loop

    MPI_Barrier(MPI_COMM_WORLD);

    for (auto& phen : pmgr.get_phens())
        phen.close_output_files();
}

// counts[rank]: holds either NPHEN * 3 or 0

void Bayes::update_epsilon(const int* counts, const double* dbetas, const unsigned char* bed) {

    const int NPHEN =  pmgr.get_phens().size();

    int cnt_tot = 0;
    int bedi = 0;
    for (int i=0; i<nranks; i++) {
        int cnt = counts[i];
        //printf("r:%d  cnt = %d\n", i, cnt);
        if (cnt == 0) continue;
        assert(cnt == NPHEN * 3);
        //printf(" cnt > 0: task %d, get for rank %d cnt = %d dbetas:\n", rank, i, cnt);
        int pheni = 0;
        for (auto& phen : pmgr.get_phens()) {
            //printf("r:%d p:%d - dbeta = %20.15f\n", rank, pheni, dbetas[cnt_tot + pheni * 3]);
            if (dbetas[cnt_tot + pheni * 3] != 0.0)
                phen.update_epsilon(&dbetas[cnt_tot + pheni * 3], &bed[bedi * mbytes]);
            //phen.update_epsilon_sum();
            //printf(" - r:%d p:%d   epssum = %.17g, cnt_tot = %d\n", i, pheni, phen.get_epsilon_sum(), cnt_tot);
            pheni += 1;
        }
        cnt_tot += cnt;
        bedi    += 1;
    }
    fflush(stdout);
}

// EO, review!!
double Bayes::dot_product(const int mloc, double* __restrict__ phen, const double mu, const double sigma_inv) {

    unsigned char* bed = &bed_data[mloc * mbytes];

#ifdef MANVEC
    __m256d luta, lutb; //, lutna;
    __m512d lutab, p42;
    __m256d p4   = _mm256_set1_pd(0.0);
    __m256d suma = _mm256_set1_pd(0.0);
    __m256d sumb = _mm256_set1_pd(0.0);
    __m512d sum42 = _mm512_set1_pd(0.0);

#ifdef _OPENMP
    //#pragma omp parallel for schedule(static) reduction(addpd4:suma,sumb)
#pragma omp parallel for schedule(static) reduction(addpd8:sum42)
#endif
    for (int j=0; j<mbytes; j++) {
        //luta  = _mm256_load_pd(&dotp_lut_a[bed[j] * 4]);
        //lutb  = _mm256_load_pd(&dotp_lut_b[bed[j] * 4]);
        //luta  = _mm256_load_pd(&dotp_lut_ab[bed[j] * 8]);
        //lutb  = _mm256_load_pd(&dotp_lut_ab[bed[j] * 8 + 4]);
        lutab = _mm512_load_pd(&dotp_lut_ab[bed[j] * 8]);
        //p4    = _mm256_load_pd(&phen[j * 4]);
        p42 = _mm512_broadcast_f64x4(_mm256_load_pd(&phen[j * 4]));
        ////lutna = _mm256_load_pd(&na_lut[mask4[j] * 4]); // phen = 0.0 on NAs!
        //luta  = _mm256_mul_pd(luta, p4);
        //lutb  = _mm256_mul_pd(lutb, p4);
        p42 = _mm512_mul_pd(p42, lutab);
        //suma  = _mm256_add_pd(suma, luta);
        //sumb  = _mm256_add_pd(sumb, lutb);
        sum42 = _mm512_add_pd(sum42, p42);
    }

    //return sigma_inv *
    //    (suma[0] + suma[1] + suma[2] + suma[3] - mu * (sumb[0] + sumb[1] + sumb[2] + sumb[3]));
    return sigma_inv *
        (sum42[0] + sum42[1] + sum42[2] + sum42[3] - mu * (sum42[4] + sum42[5] + sum42[6] + sum42[7]));

#else

    double dpa = 0.0;
    double dpb = 0.0;

#ifdef _OPENMP
    //#pragma omp parallel for schedule(static) reduction(addpd4:suma,sumb)
#pragma omp parallel for schedule(static) reduction(+:dpa,dpb)
#endif
    for (int i=0; i<mbytes; i++) {
#ifdef _OPENMP
#pragma omp simd aligned(dotp_lut_a,dotp_lut_b,phen:32)
#endif
        for (int j=0; j<4; j++) {
            dpa += dotp_lut_a[bed[i] * 4 + j] * phen[i * 4 + j];
            dpb += dotp_lut_b[bed[i] * 4 + j] * phen[i * 4 + j];
        }
    }

    return sigma_inv * (dpa - mu * dpb);

#endif

}

// Setup processing: load input files and define MPI task workload
void Bayes::setup_processing() {

    mbytes = (N %  4) ? (size_t) N /  4 + 1 : (size_t) N /  4;
    load_genotype();

    pmgr.read_phen_files(opt, get_N(), get_M());

    check_processing_setup();

    if (rank == 0)
        printf("INFO   : output directory: %s\n", opt.get_out_dir().c_str());

    MPI_Barrier(MPI_COMM_WORLD);
    double ts = MPI_Wtime();
    pmgr.compute_markers_statistics(bed_data, get_N(), get_M(), mbytes);
    MPI_Barrier(MPI_COMM_WORLD);
    double te = MPI_Wtime();
    if (rank == 0)
        printf("INFO   : Time to compute the markers' statistics: %.2f seconds.\n", te - ts);

    if (opt.predict()) return;

    for (auto& phen : pmgr.get_phens()) {
        phen.set_prng_m((unsigned int)(opt.get_seed() + (rank + 0)));
        if (opt.mimic_hydra()) {
            phen.set_prng_d((unsigned int)(opt.get_seed() + (rank + 0) * 1000));
        } else {
            phen.set_prng_d((unsigned int)(opt.get_seed() + (rank + 1) * 1000));
        }
    }

    read_group_index_file(opt.get_group_index_file());

    for (int i=0; i<Mt; i++) {
        mtotgrp.at(get_marker_group(i)) += 1;
    }
    //for (int i=0; i<G; i++)
    //    printf("mtotgrp at %d = %d\n", i, mtotgrp.at(i));
}

void Bayes::check_openmp() {
#ifdef _OPENMP
#pragma omp parallel
    {
        if (rank == 0) {
            int nt = omp_get_num_threads();
            if (omp_get_thread_num() == 0)
                printf("INFO   : OMP parallel regions will use %d thread(s)\n", nt);
        }
    }
#else
    printf("WARNING: no OpenMP support!\n");
#endif
}

void Bayes::read_group_index_file(const std::string& file) {

    std::ifstream infile(file.c_str());
    if (! infile)
        throw ("Error: can not open the group file [" + file + "] to read. Use the --group-index-file option!");

    if (rank == 0)
        std::cout << "INFO   : Reading groups from " + file + "." << std::endl;

    std::string label;
    int group;

    group_index.clear();

    while (infile >> label >> group) {
        //std::cout << label << " - " << group << std::endl;
        if (group > G) {
            printf("FATAL  : group index file contains a value that exceeds the number of groups given in group mixture file.\n");
            printf("       : check the consistency between your group index and mixture input files.\n");
            exit(1);
        }
        group_index.push_back(group);
    }
}

void Bayes::check_processing_setup() {
    for (const auto& phen : pmgr.get_phens()) {
        const int Np = phen.get_nas() + phen.get_nonas();
        if (Np != N) {
            std::cout << "Fatal: N = " << N << " while phen file " << phen.get_filepath() << " has " << Np << " individuals!" << std::endl;
            exit(1);
        }
    }
}

void Bayes::load_genotype() {

    double ts = MPI_Wtime();

    MPI_File bedfh;
    const std::string bedfp = opt.get_bed_file();
    check_mpi(MPI_File_open(MPI_COMM_WORLD, bedfp.c_str(), MPI_MODE_RDONLY, MPI_INFO_NULL, &bedfh),  __LINE__, __FILE__);

    const size_t size_bytes = size_t(M) * size_t(mbytes) * sizeof(unsigned char);

    bed_data = (unsigned char*)_mm_malloc(size_bytes, 64);
    check_malloc(bed_data, __LINE__, __FILE__);
    printf("INFO   : rank %4d has allocated %zu bytes (%.3f GB) for raw data.\n", rank, size_bytes, double(size_bytes) / 1.0E9);

    // Offset to section of bed file to be processed by task
    MPI_Offset offset = size_t(3) + size_t(S) * size_t(mbytes) * sizeof(unsigned char);

    // Gather the sizes to determine common number of reads
    size_t max_size_bytes = 0;
    check_mpi(MPI_Allreduce(&size_bytes, &max_size_bytes, 1, MPI_UNSIGNED_LONG_LONG, MPI_MAX, MPI_COMM_WORLD), __LINE__, __FILE__);

    const int NREADS = check_int_overflow(size_t(ceil(double(max_size_bytes)/double(INT_MAX/2))), __LINE__, __FILE__);
    size_t bytes = 0;
    mpi_file_read_at_all <unsigned char*> (size_bytes, offset, bedfh, MPI_UNSIGNED_CHAR, NREADS, bed_data, bytes);
    //@@@MPI_Barrier(MPI_COMM_WORLD);

    check_mpi(MPI_File_close(&bedfh), __LINE__, __FILE__);

    double te = MPI_Wtime();
    MPI_Barrier(MPI_COMM_WORLD);
    if (rank >= 0)
        printf("INFO   : time to load genotype data = %.2f seconds.\n", te - ts);

}

void Bayes::set_block_of_markers() {

    const int modu = Mt % nranks;
    const int size = Mt / nranks;

    Mm = Mt % nranks != 0 ? size + 1 : size;

    int len[nranks], start[nranks];
    int cum = 0;
    for (int i=0; i<nranks; i++) {
        len[i]  = i < modu ? size + 1 : size;
        start[i] = cum;
        cum += len[i];
    }
    //printf("cum %d vs %d  Mm = %d\n", cum, Mt, Mm);
    assert(cum == Mt);

    M = len[rank];
    S = start[rank];

    printf("INFO   : rank %4d has %d markers over tot Mt = %d, max Mm = %d, starting at S = %d\n", rank, M, Mt, Mm, S);
    //@todo: mpi check sum over tasks == Mt
}

void Bayes::print_cva() {
    printf("INFO   : mixtures for all groups:\n");
    for (int i=0; i<G; i++) {
        printf("         grp %2d: ", i);
        for (int j=0; j<K; j++) {
            printf("%7.5f ", cva[i][j]);
        }
        printf("\n");
    }
}

void Bayes::print_cvai() {
    printf("INFO   : inverse mixtures for all groups:\n");
    for (int i=0; i<G; i++) {
        printf("         grp %2d: ", i);
        for (int j=0; j<K; j++) {
            printf("%10.3f ", cvai[i][j]);
        }
        printf("\n");
    }
}