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

#include "util/coordinate_calculation.hpp"
#include "util/string_view.hpp"
#include "util/vector_tile.hpp"
#include "util/web_mercator.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
{

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 datafacade::ContiguousInternalMemoryDataFacadeBase &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 datafacade::ContiguousInternalMemoryDataFacadeBase &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)
{

    // Vector tiles encode properties as references to a common lookup table.
    // When we add a property to a "feature", we actually attach the index of the value
    // rather than the value itself.  Thus, we need to keep a list of the unique
    // values we need, and we add this list to the tile as a lookup table.  This
    // vector holds all the actual used values, the feature refernce offsets in
    // this vector.
    // for integer values
    std::vector<int> used_line_ints;
    // While constructing the tile, we keep track of which integers we have in our table
    // and their offsets, so multiple features can re-use the same values
    std::unordered_map<int, std::size_t> line_int_offsets;

    // Same idea for street names - one lookup table for names for all features
    std::vector<util::StringView> names;
    std::unordered_map<util::StringView, std::size_t> name_offsets;

    // And again for integer values used by points.
    std::vector<int> used_point_ints;
    std::unordered_map<int, std::size_t> point_int_offsets;

    // And again for float values used by points
    std::vector<float> used_point_floats;
    std::unordered_map<float, std::size_t> point_float_offsets;

    std::uint8_t max_datasource_id = 0;

    // This is where we accumulate information on turns

    // Helper function for adding a new value to the line_ints lookup table.  Returns
    // the index of the value in the table, adding the value if it doesn't already
    // exist
    const auto use_line_value = [&used_line_ints, &line_int_offsets](const int value) {
        const auto found = line_int_offsets.find(value);

        if (found == line_int_offsets.end())
        {
            used_line_ints.push_back(value);
            line_int_offsets[value] = used_line_ints.size() - 1;
        }

        return;
    };

    // Same again
    const auto use_point_int_value = [&used_point_ints, &point_int_offsets](const int value) {
        const auto found = point_int_offsets.find(value);
        std::size_t offset;

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

        return offset;
    };

    // And a third time, should probably template this....
    const auto use_point_float_value = [&used_point_floats,
                                        &point_float_offsets](const float value) {
        const auto found = point_float_offsets.find(value);
        std::size_t offset;

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

        return offset;
    };

    // 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 forward_datasource_vector =
            facade.GetUncompressedForwardDatasources(edge.packed_geometry_id);
        const auto reverse_datasource_vector =
            facade.GetUncompressedReverseDatasources(edge.packed_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];

                // Weight values
                const auto forward_weight_vector =
                    facade.GetUncompressedForwardWeights(edge.packed_geometry_id);
                const auto reverse_weight_vector =
                    facade.GetUncompressedReverseWeights(edge.packed_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];
                use_line_value(forward_weight);
                use_line_value(reverse_weight);

                // Duration values
                const auto forward_duration_vector =
                    facade.GetUncompressedForwardDurations(edge.packed_geometry_id);
                const auto reverse_duration_vector =
                    facade.GetUncompressedReverseDurations(edge.packed_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];
                use_line_value(forward_duration);
                use_line_value(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];
                    // 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(edge.packed_geometry_id);
                    const auto reverse_weight_vector =
                        facade.GetUncompressedReverseWeights(edge.packed_geometry_id);
                    const auto forward_duration_vector =
                        facade.GetUncompressedForwardDurations(edge.packed_geometry_id);
                    const auto reverse_duration_vector =
                        facade.GetUncompressedReverseDurations(edge.packed_geometry_id);
                    const auto forward_datasource_vector =
                        facade.GetUncompressedForwardDatasources(edge.packed_geometry_id);
                    const auto reverse_datasource_vector =
                        facade.GetUncompressedReverseDatasources(edge.packed_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 =
                        forward_datasource_vector[edge.fwd_segment_position];
                    const auto reverse_datasource =
                        reverse_datasource_vector[reverse_datasource_vector.size() -
                                                  edge.fwd_segment_position - 1];

                    auto name = facade.GetNameForID(edge.name_id);

                    const auto name_offset = [&name, &names, &name_offsets]() {
                        auto iter = name_offsets.find(name);
                        if (iter == name_offsets.end())
                        {
                            auto offset = names.size();
                            name_offsets[name] = offset;
                            names.push_back(name);
                            return offset;
                        }
                        return iter->second;
                    }();

                    const auto encode_tile_line = [&line_layer_writer,
                                                   &edge,
                                                   &id,
                                                   &max_datasource_id,
                                                   &used_line_ints](const FixedLine &tile_line,
                                                                    const std::uint32_t speed_kmh,
                                                                    const std::size_t weight,
                                                                    const std::size_t duration,
                                                                    const DatasourceID datasource,
                                                                    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, 127u)); // save the speed value, capped at 127
                            field.add_element(1);           // "is_small" tag key offset
                            field.add_element(128 +
                                              (edge.component.is_tiny ? 0 : 1)); // is_small feature
                            field.add_element(2);                // "datasource" tag key offset
                            field.add_element(130 + datasource); // datasource value offset
                            field.add_element(3);                // "weight" tag key offset
                            field.add_element(130 + max_datasource_id + 1 +
                                              weight); // weight value offset
                            field.add_element(4);      // "duration" tag key offset
                            field.add_element(130 + max_datasource_id + 1 +
                                              duration); // duration value offset
                            field.add_element(5);        // "name" tag key offset

                            field.add_element(130 + max_datasource_id + 1 + used_line_ints.size() +
                                              name_idx); // name value offset
                        }
                        {

                            // 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
                        std::uint32_t speed_kmh =
                            static_cast<std::uint32_t>(round(length / forward_duration * 10 * 3.6));

                        auto tile_line = coordinatesToTileLine(a, b, tile_bbox);
                        if (!tile_line.empty())
                        {
                            encode_tile_line(tile_line,
                                             speed_kmh,
                                             line_int_offsets[forward_weight],
                                             line_int_offsets[forward_duration],
                                             forward_datasource,
                                             name_offset,
                                             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
                        std::uint32_t speed_kmh =
                            static_cast<std::uint32_t>(round(length / reverse_duration * 10 * 3.6));

                        auto tile_line = coordinatesToTileLine(b, a, tile_bbox);
                        if (!tile_line.empty())
                        {
                            encode_tile_line(tile_line,
                                             speed_kmh,
                                             line_int_offsets[reverse_weight],
                                             line_int_offsets[reverse_duration],
                                             reverse_datasource,
                                             name_offset,
                                             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");

            // 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 : used_line_ints)
            {
                // 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 : names)
            {
                // 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())
        {
            // we need to pre-encode all values here because we need the full offsets later
            // for encoding the actual features.
            std::vector<std::tuple<util::Coordinate, unsigned, unsigned, unsigned>>
                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 = use_point_int_value(t.in_angle);
                    auto turn_idx = use_point_int_value(t.turn_angle);
                    auto duration_idx =
                        use_point_float_value(t.duration / 10.0); // Note conversion to float here
                    return std::make_tuple(t.coordinate, angle_idx, turn_idx, duration_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 3 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(std::get<1>(point_turn_data));
                        field.add_element(1); // "turn_angle" tag key offset
                        field.add_element(std::get<2>(point_turn_data));
                        field.add_element(2); // "cost" tag key offset
                        field.add_element(used_point_ints.size() + std::get<3>(point_turn_data));
                    }
                    {
                        // 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(std::get<0>(turndata), 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");

            // Now, save the lists of integers and floats that our features refer to.
            for (const auto &value : used_point_ints)
            {
                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 : used_point_floats)
            {
                protozero::pbf_writer values_writer(point_layer_writer,
                                                    util::vector_tile::VARIANT_TAG);
                values_writer.add_float(util::vector_tile::VARIANT_TYPE_FLOAT, value);
            }
        }
    }
    // 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 datafacade::ContiguousInternalMemoryDataFacadeBase &facade,
                                 const RoutingAlgorithmsInterface &algorithms,
                                 const api::TileParameters &parameters,
                                 std::string &pbf_buffer) const
{
    BOOST_ASSERT(parameters.IsValid());

    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;
}
}
}
}


#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>
#include <cmath>

#include "config.h"
#ifdef HAVE_MPI
#include <boost/mpi.hpp>
#endif
#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "verbose.h"
#include "hg.h"
#include "prob.h"
#include "inside_outside.h"
#include "ff_register.h"
#include "decoder.h"
#include "filelib.h"
#include "weights.h"

using namespace std;
namespace po = boost::program_options;

bool InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("weights,w",po::value<string>(),"Input feature weights file")
        ("training_data,t",po::value<string>(),"Training data corpus")
        ("decoder_config,c",po::value<string>(),"Decoder configuration file");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || !conf->count("training_data") || !conf->count("decoder_config")) {
    cerr << dcmdline_options << endl;
    return false;
  }
  return true;
}

void ReadTrainingCorpus(const string& fname, int rank, int size, vector<string>* c, vector<int>* ids) {
  ReadFile rf(fname);
  istream& in = *rf.stream();
  string line;
  int lc = 0;
  while(in) {
    getline(in, line);
    if (!in) break;
    if (lc % size == rank) {
      c->push_back(line);
      ids->push_back(lc);
    }
    ++lc;
  }
}

static const double kMINUS_EPSILON = -1e-6;

struct TrainingObserver : public DecoderObserver {
  void Reset() {
    acc_obj = 0;
  }

  virtual void NotifyDecodingStart(const SentenceMetadata&) {
    cur_obj = 0;
    state = 1;
  }

  // compute model expectations, denominator of objective
  virtual void NotifyTranslationForest(const SentenceMetadata&, Hypergraph* hg) {
    assert(state == 1);
    state = 2;
    SparseVector<prob_t> cur_model_exp;
    const prob_t z = InsideOutside<prob_t,
                                   EdgeProb,
                                   SparseVector<prob_t>,
                                   EdgeFeaturesAndProbWeightFunction>(*hg, &cur_model_exp);
    cur_obj = log(z);
  }

  // compute "empirical" expectations, numerator of objective
  virtual void NotifyAlignmentForest(const SentenceMetadata& smeta, Hypergraph* hg) {
    assert(state == 2);
    state = 3;
    SparseVector<prob_t> ref_exp;
    const prob_t ref_z = InsideOutside<prob_t,
                                       EdgeProb,
                                       SparseVector<prob_t>,
                                       EdgeFeaturesAndProbWeightFunction>(*hg, &ref_exp);

    double log_ref_z;
#if 0
    if (crf_uniform_empirical) {
      log_ref_z = ref_exp.dot(feature_weights);
    } else {
      log_ref_z = log(ref_z);
    }
#else
    log_ref_z = log(ref_z);
#endif

    // rounding errors means that <0 is too strict
    if ((cur_obj - log_ref_z) < kMINUS_EPSILON) {
      cerr << "DIFF. ERR! log_model_z < log_ref_z: " << cur_obj << " " << log_ref_z << endl;
      exit(1);
    }
    assert(!isnan(log_ref_z));
    acc_obj += (cur_obj - log_ref_z);
  }

  double acc_obj;
  double cur_obj;
  int state;
};

#ifdef HAVE_MPI
namespace mpi = boost::mpi;
#endif

int main(int argc, char** argv) {
#ifdef HAVE_MPI
  mpi::environment env(argc, argv);
  mpi::communicator world;
  const int size = world.size();
  const int rank = world.rank();
#else
  const int size = 1;
  const int rank = 0;
#endif
  if (size > 1) SetSilent(true);  // turn off verbose decoder output
  register_feature_functions();

  po::variables_map conf;
  if (!InitCommandLine(argc, argv, &conf))
    return false;

  // load initial weights
  Weights weights;
  if (conf.count("weights"))
    weights.InitFromFile(conf["weights"].as<string>());

  // freeze feature set
  //const bool freeze_feature_set = conf.count("freeze_feature_set");
  //if (freeze_feature_set) FD::Freeze();

  // load cdec.ini and set up decoder
  ReadFile ini_rf(conf["decoder_config"].as<string>());
  Decoder decoder(ini_rf.stream());
  if (decoder.GetConf()["input"].as<string>() != "-") {
    cerr << "cdec.ini must not set an input file\n";
    abort();
  }

  vector<string> corpus; vector<int> ids;
  ReadTrainingCorpus(conf["training_data"].as<string>(), rank, size, &corpus, &ids);
  assert(corpus.size() > 0);
  assert(corpus.size() == ids.size());

  vector<double> wv;
  weights.InitVector(&wv);
  decoder.SetWeights(wv);
  TrainingObserver observer;
  double objective = 0;
  bool converged = false;

  observer.Reset();
  if (rank == 0)
    cerr << "Each processor is decoding " << corpus.size() << " training examples...\n";

  for (int i = 0; i < corpus.size(); ++i) {
    decoder.SetId(ids[i]);
    decoder.Decode(corpus[i], &observer);
  }

#ifdef HAVE_MPI
  reduce(world, observer.acc_obj, objective, std::plus<double>(), 0);
#else
  objective = observer.acc_obj;
#endif

  if (rank == 0)
    cout << "OBJECTIVE: " << objective << endl;

  return 0;
}


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <dlib/matrix.h>
#include <dlib/svm.h>
#include "testing_results.h"
#include <pybind11/stl_bind.h>

using namespace dlib;
using namespace std;
namespace py = pybind11;

typedef matrix<double,0,1> sample_type;
typedef std::vector<std::pair<unsigned long,double> > sparse_vect;
typedef std::vector<ranking_pair<sample_type> > ranking_pairs;
typedef std::vector<ranking_pair<sparse_vect> > sparse_ranking_pairs;

PYBIND11_MAKE_OPAQUE(ranking_pairs);
PYBIND11_MAKE_OPAQUE(sparse_ranking_pairs);

// ----------------------------------------------------------------------------------------

namespace dlib
{
    template <typename T>
    bool operator== (
        const ranking_pair<T>&,
        const ranking_pair<T>&
    )
    {
        pyassert(false, "It is illegal to compare ranking pair objects for equality.");
        return false;
    }
}

template <typename T>
void resize(T& v, unsigned long n) { v.resize(n); }

// ----------------------------------------------------------------------------------------

template <typename trainer_type>
typename trainer_type::trained_function_type train1 (
    const trainer_type& trainer,
    const ranking_pair<typename trainer_type::sample_type>& sample
)
{
    typedef ranking_pair<typename trainer_type::sample_type> st;
    pyassert(is_ranking_problem(std::vector<st>(1, sample)), "Invalid inputs");
    return trainer.train(sample);
}

template <typename trainer_type>
typename trainer_type::trained_function_type train2 (
    const trainer_type& trainer,
    const std::vector<ranking_pair<typename trainer_type::sample_type> >& samples
)
{
    pyassert(is_ranking_problem(samples), "Invalid inputs");
    return trainer.train(samples);
}

template <typename trainer_type>
void set_epsilon ( trainer_type& trainer, double eps)
{
    pyassert(eps > 0, "epsilon must be > 0");
    trainer.set_epsilon(eps);
}

template <typename trainer_type>
double get_epsilon ( const trainer_type& trainer) { return trainer.get_epsilon(); }

template <typename trainer_type>
void set_c ( trainer_type& trainer, double C)
{
    pyassert(C > 0, "C must be > 0");
    trainer.set_c(C);
}

template <typename trainer_type>
double get_c (const trainer_type& trainer)
{
    return trainer.get_c();
}

template <typename trainer>
void add_ranker (
    py::module& m,
    const char* name
)
{
    py::class_<trainer>(m, name)
        .def(py::init())
        .def_property("epsilon", get_epsilon<trainer>, set_epsilon<trainer>)
        .def_property("c", get_c<trainer>, set_c<trainer>)
        .def_property("max_iterations", &trainer::get_max_iterations, &trainer::set_max_iterations)
        .def_property("force_last_weight_to_1", &trainer::forces_last_weight_to_1, &trainer::force_last_weight_to_1)
        .def_property("learns_nonnegative_weights", &trainer::learns_nonnegative_weights, &trainer::set_learns_nonnegative_weights)
        .def_property_readonly("has_prior", &trainer::has_prior)
        .def("train", train1<trainer>)
        .def("train", train2<trainer>)
        .def("set_prior", &trainer::set_prior)
        .def("be_verbose", &trainer::be_verbose)
        .def("be_quiet", &trainer::be_quiet);
}

// ----------------------------------------------------------------------------------------

template <
    typename trainer_type,
    typename T
    >
const ranking_test _cross_ranking_validate_trainer (
    const trainer_type& trainer,
    const std::vector<ranking_pair<T> >& samples,
    const unsigned long folds
)
{
    pyassert(is_ranking_problem(samples), "Training data does not make a valid training set.");
    pyassert(1 < folds && folds <= samples.size(), "Invalid number of folds given.");
    return cross_validate_ranking_trainer(trainer, samples, folds);
}

// ----------------------------------------------------------------------------------------

void bind_svm_rank_trainer(py::module& m)
{
    py::class_<ranking_pair<sample_type> >(m, "ranking_pair")
        .def(py::init())
        .def_readwrite("relevant", &ranking_pair<sample_type>::relevant)
        .def_readwrite("nonrelevant", &ranking_pair<sample_type>::nonrelevant)
        .def(py::pickle(&getstate<ranking_pair<sample_type>>, &setstate<ranking_pair<sample_type>>));

    py::class_<ranking_pair<sparse_vect> >(m, "sparse_ranking_pair")
        .def(py::init())
        .def_readwrite("relevant", &ranking_pair<sparse_vect>::relevant)
        .def_readwrite("nonrelevant", &ranking_pair<sparse_vect>::nonrelevant)
        .def(py::pickle(&getstate<ranking_pair<sparse_vect>>, &setstate<ranking_pair<sparse_vect>>));

    py::bind_vector<ranking_pairs>(m, "ranking_pairs")
        .def("clear", &ranking_pairs::clear)
        .def("resize", resize<ranking_pairs>)
        .def("extend", extend_vector_with_python_list<ranking_pair<sample_type>>)
        .def(py::pickle(&getstate<ranking_pairs>, &setstate<ranking_pairs>));

    py::bind_vector<sparse_ranking_pairs>(m, "sparse_ranking_pairs")
        .def("clear", &sparse_ranking_pairs::clear)
        .def("resize", resize<sparse_ranking_pairs>)
        .def("extend", extend_vector_with_python_list<ranking_pair<sparse_vect>>)
        .def(py::pickle(&getstate<sparse_ranking_pairs>, &setstate<sparse_ranking_pairs>));

    add_ranker<svm_rank_trainer<linear_kernel<sample_type> > >(m, "svm_rank_trainer");
    add_ranker<svm_rank_trainer<sparse_linear_kernel<sparse_vect> > >(m, "svm_rank_trainer_sparse");

    m.def("cross_validate_ranking_trainer", &_cross_ranking_validate_trainer<
                svm_rank_trainer<linear_kernel<sample_type> >,sample_type>,
                py::arg("trainer"), py::arg("samples"), py::arg("folds") );
    m.def("cross_validate_ranking_trainer", &_cross_ranking_validate_trainer<
                svm_rank_trainer<sparse_linear_kernel<sparse_vect> > ,sparse_vect>,
                py::arg("trainer"), py::arg("samples"), py::arg("folds") );
}


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

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_TRAITS_HPP
#define BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_TRAITS_HPP

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/static_assert.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

namespace boost { namespace numeric { namespace bindings { namespace traits {

  namespace detail {

    // select symmetric or hermitian matrix structure

    template <typename T>
    struct symm_herm_t {};
    template<>
    struct symm_herm_t<float> {
      typedef symmetric_t type;
    };
    template<>
    struct symm_herm_t<double> {
      typedef symmetric_t type;
    };
    template<>
    struct symm_herm_t<complex_f> {
      typedef hermitian_t type;
    };
    template<>
    struct symm_herm_t<complex_d> {
      typedef hermitian_t type;
    };

    template <typename T>
    struct symm_herm_pack_t {};
    template<>
    struct symm_herm_pack_t<float> {
      typedef symmetric_packed_t type;
    };
    template<>
    struct symm_herm_pack_t<double> {
      typedef symmetric_packed_t type;
    };
    template<>
    struct symm_herm_pack_t<complex_f> {
      typedef hermitian_packed_t type;
    };
    template<>
    struct symm_herm_pack_t<complex_d> {
      typedef hermitian_packed_t type;
    };

    template <class T, class S>
    struct symm_herm_compatible {
      BOOST_STATIC_CONSTANT( bool, value=false ) ;
    };

    template <class T>
    struct symm_herm_compatible< T, hermitian_t > {
      BOOST_STATIC_CONSTANT( bool, value=true ) ;
    };

    template <class T>
    struct symm_herm_compatible< T, symmetric_t > {
      BOOST_STATIC_CONSTANT( bool, value=true ) ;
    };

    template <class T>
    struct symm_herm_compatible< std::complex<T>, symmetric_t > {
      BOOST_STATIC_CONSTANT( bool, value=false ) ;
    };

  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_SYMM_HERM_TRAITS_HPP


/**
 * Implementation of KVH 1750 types.
 * \author Jason Ziglar <jpz@vt.edu>
 * \date 02/16/2015
 */
#include "kvh1750/types.h"

#include <byteswap.h>
#include <boost/crc.hpp>

namespace
{
  //! Offset between Celsius and Farenheit
  const double CF_Offset = 32.0;
  //! Scaling factor from Farenheit to Celsius
  const double FC_Scale = 5.0 / 9.0;
  //! Scaling factor from Celsius to Farenheit
  const double CF_Scale = 9.0 / 5.0;
}

namespace kvh
{

/**
 * Default Constructor, initializes to invalid memory
 */
Message::Message() :
  _ang_vel{0.0, 0.0, 0.0},
  _lin_accel{0.0, 0.0, 0.0},
  _secs(0),
  _nsecs(0),
  _temp(0),
  _status(0),
  _seq(0),
  _is_da(false),
  _is_c(false)
{
}

/**
 * Constructor which converts a raw message to a valid message
 */
Message::Message(const RawMessage& raw, uint32_t secs, uint32_t nsecs, bool is_c,
  bool is_da) :
  _ang_vel(),
  _lin_accel(),
  _secs(),
  _nsecs(),
  _temp(),
  _status(),
  _seq(),
  _is_da(),
  _is_c()
{
  from_raw(raw, secs, nsecs, is_c, is_da);
}

/**
 * Default destructor
 */
Message::~Message()
{
}

/**
 * Conversion function from raw message to a more meaningful representation.
 * \param[out] Flag indicating if the CRC came up valid.
 */
bool Message::from_raw(const RawMessage& raw, uint32_t secs, uint32_t nsecs,
  bool is_c, bool is_da)
{
  bool valid_msg = valid_checksum(raw);

  if(valid_msg)
  {
    const size_t Num_Values_Per_Array = 3;

    for(size_t ii = 0; ii < Num_Values_Per_Array; ++ii)
    {

      union swap_float
      {
        int32_t raw;
        float val;
      } ang, lin;

      ang.raw = bswap_32(raw.rots[ii]);
      lin.raw = bswap_32(raw.accels[ii]);
      _ang_vel[ii] = ang.val;
      _lin_accel[ii] = Gravity * lin.val;
    }

    _status = raw.status;
    _seq = raw.seq;
    _temp = bswap_16(raw.temp);
    _secs = secs;
    _nsecs = nsecs;
    _is_c = is_c;
    _is_da = is_da;
  }
  else
  {
    *this = Message(); //zero all data
  }

  return valid_msg;
}

/**
 * Accessing angular velocity along IMU's X.
 */
float Message::gyro_x() const
{
  return _ang_vel[X];
}

/**
 * Accessing angular velocity with IMU's Y.
 */
float Message::gyro_y() const
{
  return _ang_vel[Y];
}

/**
 * Accessing angular velocity with IMU's Z.
 */
float Message::gyro_z() const
{
  return _ang_vel[Z];
}

/**
 * Accessing linear acceleration with IMU's X.
 */
float Message::accel_x() const
{
  return _lin_accel[X];
}

/**
 * Accessing linear acceleration with IMU's Y.
 */
float Message::accel_y() const
{
  return _lin_accel[Y];
}

/**
 * Accessing linear acceleration with IMU's Z.
 */
float Message::accel_z() const
{
  return _lin_accel[Z];
}

/**
 * Accessing temperature rounded to the nearest degree. See
 * is_celsius() to determine units.
 */
int16_t Message::temp() const
{
  return _temp;
}

/**
 * Accesses temperature, and returns flag indicating units.
 * \param[in,out] Flag indicating if temperature is in Celsius
 * \param[out] Temperature, rounded to nearest degree
 */
int16_t Message::temp(bool& is_c) const
{
  is_c = _is_c;
  return _temp;
}

/**
 * Returns the time this message was recorded.
 */
void Message::time(uint32_t& secs, uint32_t& nsecs) const
{
  secs = _secs;
  nsecs = _nsecs;
}

/**
 * Sequential Number from IMU.
 * \param[out] Number
 */
uint8_t Message::sequence_number() const
{
  return _seq;
}

/**
 * Tests if this message contains valid data.
 */
bool Message::valid() const
{
  return _status != 0;
}

/**
 * Flag indicating if the X gyro was valid.
 */
bool Message::valid_gyro_x() const
{
  return _status & GYRO_X;
}

/**
 * Flag indicating if the Y gyro was valid.
 */
bool Message::valid_gyro_y() const
{
  return _status & GYRO_Y;
}

/**
 * Flag indicating if the Z gyro was valid.
 */
bool Message::valid_gyro_z() const
{
  return _status & GYRO_Z;
}

/**
 * Flag indicating if the X accelerometer was valid.
 */
bool Message::valid_accel_x() const
{
  return _status & ACCEL_X;
}

/**
 * Flag indicating if the Y accelerometer was valid.
 */
bool Message::valid_accel_y() const
{
  return _status & ACCEL_Y;
}

/**
 * Flag indicating if the Z accelerometer was valid.
 */
bool Message::valid_accel_z() const
{
  return _status & ACCEL_Z;
}

/**
 * Flag indicating if temperature is in Celsius.
 */
bool Message::is_celsius() const
{
  return _is_c;
}

/**
 * Flag indicating if angular velocities are delta-angles, as
 * opposed to some filtered angular velocity.
 */
bool Message::is_delta_angle() const
{
  return _is_da;
}

/**
 * Converts temperature to Celsius, if in Farenheit.
 */
void Message::to_celsius()
{
  if(_is_c)
  {
    return;
  }

  _temp = to_c(_temp);
  _is_c = true;
}

/**
 * Converts temperature to Farenheight, if in Celsius.
 */
void Message::to_farenheit()
{
  if(!_is_c)
  {
    return;
  }

  _temp = to_f(_temp);
  _is_c = false;
}

/**
 * Free function converting to Celsius.
 */
int16_t to_c(int16_t temp)
{
  return static_cast<int16_t>((static_cast<double>(temp) - CF_Offset) * FC_Scale);
}

/**
 * Free function converting to Farenheit.
 */
int16_t to_f(int16_t temp)
{
  return static_cast<int16_t>((static_cast<double>(temp) * CF_Scale) + CF_Offset);
}

/**
 * Function to test the CRC of a RawMessage using the parameters
 * defined by KVH.
 * \param[in] Message to test
 * \param[out] Flag indicating if CRC matches.
 */
bool valid_checksum(const kvh::RawMessage& msg)
{
  const size_t CRCSize = sizeof(kvh::RawMessage) - sizeof(msg.crc);
  const char* ptr = reinterpret_cast<const char*>(&msg);
  uint32_t check = compute_checksum(ptr, CRCSize);

  return (check == bswap_32(msg.crc));
}

/**
 * Computes checksum of an arbitrary buffer of a given length,
 * using the KVH CRC parameters
 * \param[in] buff Pointer to buffer
 * \param[in] len Length of buffer in bytes
 * \param[out] CRC checksum value
 */
uint32_t compute_checksum(const char* buff, size_t len)
{
  return boost::crc<crc::Width, crc::Poly, crc::XOr_In, crc::XOr_Out,
    crc::Reflect_In, crc::Reflect_Out>(buff, len);
}

}


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// variation on the original strowman snv caller -- implements a
/// compile-time specified grid in allele frequency space and requires
/// similar frequency as definition of non-somatic.
///

/// \author Chris Saunders
///
#ifndef __POSITION_SOMATIC_SNV_STRAND_GRID_HH
#define __POSITION_SOMATIC_SNV_STRAND_GRID_HH

#include "extended_pos_data.hh"
#include "position_somatic_snv_grid_shared.hh"

#include "blt_common/snp_pos_info.hh"
#include "blt_common/position_snp_call_pprob_digt.hh"
#include "blt_util/qscore.hh"
#include "strelka/strelka_shared.hh"

#include <boost/utility.hpp>

#include <iosfwd>

//#define SOMATIC_DEBUG

namespace DIGT_SGRID {

// HET_RES is the number of points sampled between 0 and 0.5 on
// the continuous frequency scale. Thus a fully sampled axis will
// be sampled HET_RES*2+3 times.
//
// Note that the single-strand tests are only implemented on the
// half axis containing the reference allele as a major
// allele. The "STRAND_STATE_SIZE" appended to the PRESTAND_SIZE
// represents these additional strand-specific error states.
//
enum constants { HET_RES = 4,
                 HET_COUNT = HET_RES*2+1,
                 HET_SIZE = DIGT::SIZE-N_BASE,
                 STRAND_COUNT = HET_RES,
                 STRAND_SIZE = HET_SIZE/2,
                 HET_STATE_SIZE = HET_SIZE*HET_COUNT,
                 PRESTRAND_SIZE = N_BASE+HET_STATE_SIZE,
                 STRAND_STATE_SIZE = STRAND_COUNT*STRAND_SIZE
               };

enum index_t { SIZE = PRESTRAND_SIZE+STRAND_STATE_SIZE };

// generates fast lookup-table to translate between stranded ref-only
// het states and DIGT het states:
//
struct strand_state_tables {
    strand_state_tables();

    unsigned digt_state[N_BASE][STRAND_SIZE];
};

extern const strand_state_tables stables;

inline
unsigned
get_het_count(const unsigned state) {
    if (state<N_BASE)         return 0;
    if (state<PRESTRAND_SIZE) return (state-N_BASE)/HET_SIZE;
    return (state-PRESTRAND_SIZE)/STRAND_SIZE;
}

inline
bool
is_strand_state(const unsigned state) {
    return (state>=PRESTRAND_SIZE);
}

inline
unsigned
get_strand_state(const unsigned state) {
    return (state-PRESTRAND_SIZE)%STRAND_SIZE;
}

inline
unsigned
get_digt_state(const unsigned state,
               const unsigned ref_base) {
    if (state<N_BASE)         return state;
    if (state<PRESTRAND_SIZE) return N_BASE+((state-N_BASE)%HET_SIZE);
    return stables.digt_state[ref_base][get_strand_state(state)];
}

// write only most representative genotype, ie "GT", for each state
void
write_state(const DIGT_SGRID::index_t gt,
            const unsigned ref_gt,
            std::ostream& os);

// write genotype, heterozygous id, and whether this is a single-strand state
void
write_full_state(const DIGT_SGRID::index_t gt,
                 const unsigned ref_gt,
                 std::ostream& os);
}

namespace DDIGT_SGRID {

enum constants { PRESTRAND_SIZE = DIGT_SGRID::PRESTRAND_SIZE*DIGT_SGRID::PRESTRAND_SIZE };
enum index_t { SIZE = PRESTRAND_SIZE+DIGT_SGRID::STRAND_STATE_SIZE };

inline
unsigned
get_state(const unsigned normal_gt,
          const unsigned tumor_gt) {
    if (normal_gt<DIGT_SGRID::PRESTRAND_SIZE) return normal_gt+DIGT_SGRID::PRESTRAND_SIZE*tumor_gt;
    return PRESTRAND_SIZE+normal_gt-DIGT_SGRID::PRESTRAND_SIZE;
}

inline
void
get_digt_grid_states(const unsigned dgt,
                     unsigned& normal_gt,
                     unsigned& tumor_gt) {
    if (dgt<PRESTRAND_SIZE) {
        normal_gt = (dgt%DIGT_SGRID::PRESTRAND_SIZE);
        tumor_gt = (dgt/DIGT_SGRID::PRESTRAND_SIZE);
    } else {
        normal_gt= dgt+DIGT_SGRID::PRESTRAND_SIZE-PRESTRAND_SIZE;
        tumor_gt=normal_gt;
    }
}

// writes state to pattern: "AA->AG"
void
write_state(const DDIGT_SGRID::index_t dgt,
            const unsigned ref_gt,
            std::ostream& os);

// writes state to pattern: "AA_0->AG_5_strand", using a unique id for
// each heterozygous state. Writing actual allele frequencies instead of just
// ids TBD
void
write_full_state(const DDIGT_SGRID::index_t dgt,
                 const unsigned ref_gt,
                 std::ostream& os);

void
write_alt_alleles(const DDIGT_SGRID::index_t dgt,
                  const unsigned ref_gt,
                  std::ostream& os);

struct is_nonsom_maker_t {
    is_nonsom_maker_t();

    std::vector<bool> val;
};
}

std::ostream& operator<<(std::ostream& os,const DDIGT_SGRID::index_t dgt);

// object used to pre-compute priors:
struct somatic_snv_caller_strand_grid {

    somatic_snv_caller_strand_grid(const strelka_options& opt,
                                   const pprob_digt_caller& pd_caller);

    //
    void
    position_somatic_snv_call(const extended_pos_info& normal_epi,
                              const extended_pos_info& tumor_epi,
                              const extended_pos_info* normal_epi_t2_ptr,
                              const extended_pos_info* tumor_epi_t2_ptr,
                              somatic_snv_genotype_grid& sgt) const;

    // compute a lot of prior information for various alternate
    // versions of the method -- we don't actually need all of this for any one computation:
    //
    struct prior_set {
        prior_set()
            : normal(DIGT_SGRID::SIZE)
            , somatic_marginal(DIGT_SGRID::SIZE)
            , normal_poly(DIGT_SGRID::SIZE)
            , somatic_marginal_poly(DIGT_SGRID::SIZE)
        {}

        typedef std::vector<blt_float_t> prior_t;

        prior_t normal;
        prior_t somatic_marginal;
        prior_t normal_poly;
        prior_t somatic_marginal_poly;
    };

private:

    const prior_set&
    get_prior_set(const unsigned ref_id) const {
        return _lnprior[ref_id];
    }

    const std::vector<blt_float_t>&
    lnprior_genomic(const unsigned ref_id) const {
        return _lnprior[ref_id].normal;
    }

    const std::vector<blt_float_t>&
    lnprior_polymorphic(const unsigned ref_id) const {
        return _lnprior[ref_id].normal_poly;
    }

    const strelka_options& _opt;
    prior_set _lnprior[N_BASE+1];
    blt_float_t _ln_som_match;
    blt_float_t _ln_som_mismatch;
};

// vcf output:
//
void
write_vcf_somatic_snv_genotype_strand_grid(const strelka_options& opt,
                                           const somatic_snv_genotype_grid& sgt,
                                           const extended_pos_data& n1_epd,
                                           const extended_pos_data& t1_epd,
                                           const extended_pos_data& n2_epd,
                                           const extended_pos_data& t2_epd,
                                           std::ostream& os);
#endif


// boost\math\distributions\bernoulli.hpp

// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

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

// http://en.wikipedia.org/wiki/bernoulli_distribution
// http://mathworld.wolfram.com/BernoulliDistribution.html

// bernoulli distribution is the discrete probability distribution of
// the number (k) of successes, in a single Bernoulli trials.
// It is a version of the binomial distribution when n = 1.

// But note that the bernoulli distribution
// (like others including the poisson, binomial & negative binomial)
// is strictly defined as a discrete function: only integral values of k are envisaged.
// However because of the method of calculation using a continuous gamma function,
// it is convenient to treat it as if a continuous function,
// and permit non-integral values of k.
// To enforce the strict mathematical model, users should use floor or ceil functions
// on k outside this function to ensure that k is integral.

#ifndef BOOST_MATH_SPECIAL_BERNOULLI_HPP
#define BOOST_MATH_SPECIAL_BERNOULLI_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.

#include <utility>

namespace boost
{
  namespace math
  {
    namespace bernoulli_detail
    {
      // Common error checking routines for bernoulli distribution functions:
      template <class RealType, class Policy>
      inline bool check_success_fraction(const char* function, const RealType& p, RealType* result, const Policy& /* pol */)
      {
        if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Success fraction argument is %1%, but must be >= 0 and <= 1 !", p, Policy());
          return false;
        }
        return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& p, RealType* result, const Policy& /* pol */, const std::true_type&)
      {
        return check_success_fraction(function, p, result, Policy());
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* , const RealType& , RealType* , const Policy& /* pol */, const std::false_type&)
      {
         return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& p, RealType* result, const Policy& /* pol */)
      {
         return check_dist(function, p, result, Policy(), typename policies::constructor_error_check<Policy>::type());
      }

      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function, const RealType& p, RealType k, RealType* result, const Policy& pol)
      {
        if(check_dist(function, p, result, Policy(), typename policies::method_error_check<Policy>::type()) == false)
        {
          return false;
        }
        if(!(boost::math::isfinite)(k) || !((k == 0) || (k == 1)))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of successes argument is %1%, but must be 0 or 1 !", k, pol);
          return false;
        }
       return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, RealType p, RealType prob, RealType* result, const Policy& /* pol */)
      {
        if((check_dist(function, p, result, Policy(), typename policies::method_error_check<Policy>::type()) && detail::check_probability(function, prob, result, Policy())) == false)
        {
          return false;
        }
        return true;
      }
    } // namespace bernoulli_detail

    template <class RealType = double, class Policy = policies::policy<> >
    class bernoulli_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      bernoulli_distribution(RealType p = 0.5) : m_p(p)
      { // Default probability = half suits 'fair' coin tossing
        // where probability of heads == probability of tails.
        RealType result; // of checks.
        bernoulli_detail::check_dist(
           "boost::math::bernoulli_distribution<%1%>::bernoulli_distribution",
          m_p,
          &result, Policy());
      } // bernoulli_distribution constructor.

      RealType success_fraction() const
      { // Probability.
        return m_p;
      }

    private:
      RealType m_p; // success_fraction
    }; // template <class RealType> class bernoulli_distribution

    typedef bernoulli_distribution<double> bernoulli;

    #ifdef __cpp_deduction_guides
    template <class RealType>
    bernoulli_distribution(RealType)->bernoulli_distribution<typename boost::math::tools::promote_args<RealType>::type>;
    #endif

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const bernoulli_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable k = {0, 1}.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const bernoulli_distribution<RealType, Policy>& /* dist */)
    { // Range of supported values for random variable k = {0, 1}.
      // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline RealType mean(const bernoulli_distribution<RealType, Policy>& dist)
    { // Mean of bernoulli distribution = p (n = 1).
      return dist.success_fraction();
    } // mean

    // Rely on dereived_accessors quantile(half)
    //template <class RealType>
    //inline RealType median(const bernoulli_distribution<RealType, Policy>& dist)
    //{ // Median of bernoulli distribution is not defined.
    //  return tools::domain_error<RealType>(BOOST_CURRENT_FUNCTION, "Median is not implemented, result is %1%!", std::numeric_limits<RealType>::quiet_NaN());
    //} // median

    template <class RealType, class Policy>
    inline RealType variance(const bernoulli_distribution<RealType, Policy>& dist)
    { // Variance of bernoulli distribution =p * q.
      return  dist.success_fraction() * (1 - dist.success_fraction());
    } // variance

    template <class RealType, class Policy>
    RealType pdf(const bernoulli_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      BOOST_FPU_EXCEPTION_GUARD
      // Error check:
      RealType result = 0; // of checks.
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::pdf(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(), // 0 to 1
        k, // 0 or 1
        &result, Policy()))
      {
        return result;
      }
      // Assume k is integral.
      if (k == 0)
      {
        return 1 - dist.success_fraction(); // 1 - p
      }
      else  // k == 1
      {
        return dist.success_fraction(); // p
      }
    } // pdf

    template <class RealType, class Policy>
    inline RealType cdf(const bernoulli_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function Bernoulli.
      RealType p = dist.success_fraction();
      // Error check:
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::cdf(bernoulli_distribution<%1%>, %1%)",
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if (k == 0)
      {
        return 1 - p;
      }
      else
      { // k == 1
        return 1;
      }
    } // bernoulli cdf

    template <class RealType, class Policy>
    inline RealType cdf(const complemented2_type<bernoulli_distribution<RealType, Policy>, RealType>& c)
    { // Complemented Cumulative Distribution Function bernoulli.
      RealType const& k = c.param;
      bernoulli_distribution<RealType, Policy> const& dist = c.dist;
      RealType p = dist.success_fraction();
      // Error checks:
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::cdf(bernoulli_distribution<%1%>, %1%)",
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if (k == 0)
      {
        return p;
      }
      else
      { // k == 1
        return 0;
      }
    } // bernoulli cdf complement

    template <class RealType, class Policy>
    inline RealType quantile(const bernoulli_distribution<RealType, Policy>& dist, const RealType& p)
    { // Quantile or Percent Point Bernoulli function.
      // Return the number of expected successes k either 0 or 1.
      // for a given probability p.

      RealType result = 0; // of error checks:
      if(false == bernoulli_detail::check_dist_and_prob(
        "boost::math::quantile(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(),
        p,
        &result, Policy()))
      {
        return result;
      }
      if (p <= (1 - dist.success_fraction()))
      { // p <= pdf(dist, 0) == cdf(dist, 0)
        return 0;
      }
      else
      {
        return 1;
      }
    } // quantile

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<bernoulli_distribution<RealType, Policy>, RealType>& c)
    { // Quantile or Percent Point bernoulli function.
      // Return the number of expected successes k for a given
      // complement of the probability q.
      //
      // Error checks:
      RealType q = c.param;
      const bernoulli_distribution<RealType, Policy>& dist = c.dist;
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_prob(
        "boost::math::quantile(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(),
        q,
        &result, Policy()))
      {
        return result;
      }

      if (q <= 1 - dist.success_fraction())
      { // // q <= cdf(complement(dist, 0)) == pdf(dist, 0)
        return 1;
      }
      else
      {
        return 0;
      }
    } // quantile complemented.

    template <class RealType, class Policy>
    inline RealType mode(const bernoulli_distribution<RealType, Policy>& dist)
    {
      return static_cast<RealType>((dist.success_fraction() <= 0.5) ? 0 : 1); // p = 0.5 can be 0 or 1
    }

    template <class RealType, class Policy>
    inline RealType skewness(const bernoulli_distribution<RealType, Policy>& dist)
    {
      BOOST_MATH_STD_USING; // Aid ADL for sqrt.
      RealType p = dist.success_fraction();
      return (1 - 2 * p) / sqrt(p * (1 - p));
    }

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const bernoulli_distribution<RealType, Policy>& dist)
    {
      RealType p = dist.success_fraction();
      // Note Wolfram says this is kurtosis in text, but gamma2 is the kurtosis excess,
      // and Wikipedia also says this is the kurtosis excess formula.
      // return (6 * p * p - 6 * p + 1) / (p * (1 - p));
      // But Wolfram kurtosis article gives this simpler formula for kurtosis excess:
      return 1 / (1 - p) + 1/p -6;
    }

    template <class RealType, class Policy>
    inline RealType kurtosis(const bernoulli_distribution<RealType, Policy>& dist)
    {
      RealType p = dist.success_fraction();
      return 1 / (1 - p) + 1/p -6 + 3;
      // Simpler than:
      // return (6 * p * p - 6 * p + 1) / (p * (1 - p)) + 3;
    }

  } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_BERNOULLI_HPP


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-reduction
    Function object implementing compare_le capabilities

    Returns a logical scalar that is the result of the lexicographic
    test for <= on all elements of the entries

    It is probably not what you wish. Have a look to @ref is_less_equal

    This is a convenient alias of @ref compare_less_equal
  **/
  const boost::dispatch::functor<tag::compare_le_> compare_le = {};
} }
#endif

#include <boost/simd/function/scalar/compare_less_equal.hpp>
#include <boost/simd/function/simd/compare_le.hpp>

#endif


#include <boost/regex/icu.hpp>
#include <iostream>

int main(int argc, const char * const argv[])
{
    return boost::u32regex_match( "Is icu 図書館 there ?", boost::make_u32regex( "書" ) );
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Samuel Debionne, Grenoble, France.

// This file was modified by Oracle on 2015, 2016, 2017, 2018, 2019.
// Modifications copyright (c) 2015-2019, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_EXPAND_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_EXPAND_BOX_HPP

#include <algorithm>
#include <cstddef>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>
#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>
#include <boost/geometry/algorithms/detail/envelope/range_of_boxes.hpp>
#include <boost/geometry/algorithms/dispatch/envelope.hpp>

#include <boost/geometry/geometries/helper_geometry.hpp>

#include <boost/geometry/strategies/expand.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

template
<
    std::size_t Index,
    std::size_t DimensionCount
>
struct envelope_indexed_box_on_spheroid
{
    template <typename BoxIn, typename BoxOut>
    static inline void apply(BoxIn const& box_in, BoxOut& mbr)
    {
        // transform() does not work with boxes of dimension higher
        // than 2; to account for such boxes we transform the min/max
        // points of the boxes using the indexed_point_view
        detail::indexed_point_view<BoxIn const, Index> box_in_corner(box_in);
        detail::indexed_point_view<BoxOut, Index> mbr_corner(mbr);

        // first transform the units
        transform_units(box_in_corner, mbr_corner);

        // now transform the remaining coordinates
        detail::conversion::point_to_point
            <
                detail::indexed_point_view<BoxIn const, Index>,
                detail::indexed_point_view<BoxOut, Index>,
                2,
                DimensionCount
            >::apply(box_in_corner, mbr_corner);
    }
};

struct envelope_box_on_spheroid
{
    template <typename BoxIn, typename BoxOut>
    static inline void apply(BoxIn const& box_in, BoxOut& mbr)
    {
        // BoxIn can be non-mutable
        typename helper_geometry<BoxIn>::type box_in_normalized;
        geometry::convert(box_in, box_in_normalized);

        if (! is_inverse_spheroidal_coordinates(box_in))
        {
            strategy::normalize::spherical_box::apply(box_in, box_in_normalized);
        }

        geometry::detail::envelope::envelope_indexed_box_on_spheroid
            <
                min_corner, dimension<BoxIn>::value
            >::apply(box_in_normalized, mbr);

        geometry::detail::envelope::envelope_indexed_box_on_spheroid
            <
                max_corner, dimension<BoxIn>::value
            >::apply(box_in_normalized, mbr);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

namespace strategy { namespace expand
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

struct box_on_spheroid
{
    template <typename BoxOut, typename BoxIn>
    static inline void apply(BoxOut& box_out, BoxIn const& box_in)
    {
        // normalize both boxes and convert box-in to be of type of box-out
        BoxOut mbrs[2];
        geometry::detail::envelope::envelope_box_on_spheroid::apply(box_in, mbrs[0]);
        geometry::detail::envelope::envelope_box_on_spheroid::apply(box_out, mbrs[1]);

        // compute the envelope of the two boxes
        geometry::detail::envelope::envelope_range_of_boxes::apply(mbrs, box_out);
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

struct spherical_box
    : detail::box_on_spheroid
{};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename CalculationType>
struct default_strategy<box_tag, spherical_equatorial_tag, CalculationType>
{
    typedef spherical_box type;
};

template <typename CalculationType>
struct default_strategy<box_tag, spherical_polar_tag, CalculationType>
{
    typedef spherical_box type;
};

template <typename CalculationType>
struct default_strategy<box_tag, geographic_tag, CalculationType>
{
    typedef spherical_box type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::expand

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_EXPAND_BOX_HPP


#include <thread>
#include <string>
#include <fstream>
#include <unistd.h>
#include <Eigen/Core>
//#include <tbb/concurrent_unordered_map.h>
#include <unordered_map>
#include <gflags/gflags.h>
#include <comlog/comlog.h>
#include <baidu/rpc/server.h>
#include "document.pb.h"
#include "rank_service.pb.h"
#include <pthread.h>

pthread_rwlock_t l;

typedef ::Eigen::Matrix<float, 32, 1> Vector;
typedef ::Eigen::Map<Vector> MVector;
using ::baidu::lijiang01::proto::Document;

DEFINE_int32(log_level, 4, "");
DEFINE_uint64(limit, 10000, "");
DEFINE_int32(port, 8899, "");
DEFINE_int32(sleep, 1000, "");

static ::std::atomic<uint64_t> all;
static int64_t latency_us;

class Node {
public:
    Node(const Node& other) {
        char* x = NULL;
        CFATAL_LOG(" copy");
        x[10000] = 10;
    }

    Node(Node&& other) : _brife(other._brife.exchange(nullptr, ::std::memory_order_relaxed)) {}

    Node(const Document& brife) : _brife(new Document(brife)) {}

    const Document* brife() const {
        return _brife.load(::std::memory_order_relaxed);
    }

    void brife(Document* new_brife) {
        auto old_brife = _brife.exchange(new_brife, ::std::memory_order_relaxed);
        if (old_brife != nullptr) {
            usleep(1000);
            delete old_brife;
        }
    }

    void brife(Document&& new_brife) {
        auto created_brife = new Document();
        created_brife->Swap(&new_brife);
        brife(created_brife);
    }

    ~Node() {
        brife(nullptr);
    }

private:
    ::std::atomic<const Document*> _brife;
};

class RankServiceImpl : public ::baidu::lijiang01::proto::RankService {
public:
    RankServiceImpl(
        //::tbb::concurrent_unordered_map<uint64_t, Node>& store
        ::std::unordered_map<uint64_t, Node>& store
        ) : _store(store) {};
    virtual ~RankServiceImpl() {};
    virtual void rank(google::protobuf::RpcController* cntl_base,
                      const ::baidu::lijiang01::proto::RankRequest* request,
                      ::baidu::lijiang01::proto::RankResponse* response,
                      google::protobuf::Closure* done) {
        auto begin = ::base::cpuwide_time_ns();
        Vector v;
        for (size_t i = 0; i < 32; i++) {
            v(i, 0) = 0.0;
        }
        response->mutable_nid()->Reserve(request->nid_size());
        response->mutable_score()->Reserve(request->nid_size());
            pthread_rwlock_rdlock(&l);
        for (auto nid : request->nid()) {
            auto it = _store.find(nid);
            auto& node = it->second;
            const Document* x = node.brife();
            MVector mv((float*)(x->ann_vector().data()));
            float score = v.dot(mv);
            response->add_nid(nid);
            response->add_score(score);

        }
            pthread_rwlock_unlock(&l);
        all.fetch_add(1, ::std::memory_order_relaxed);
        auto end = ::base::cpuwide_time_ns();
        latency_us = end - begin;
        done->Run();
    }

private:
    //::tbb::concurrent_unordered_map<uint64_t, Node>& _store;
    ::std::unordered_map<uint64_t, Node>& _store;
};

//::tbb::concurrent_unordered_map<uint64_t, Node> store;
::std::unordered_map<uint64_t, Node> store;
static void status() {
    uint64_t last_all = all.load(::std::memory_order_relaxed);
    ::std::ifstream doc("d");
    ::std::string line;
    Document document;
    for (size_t i = 0; i < 32; i++) {
        document.add_ann_vector(0.0);
    }

    while (true) {
        usleep(FLAGS_sleep);
        uint64_t current_all = all.load(::std::memory_order_relaxed);
        last_all = current_all;
        ::std::getline(doc, line);
        char* brife_start = nullptr;
        uint64_t nid = strtoull(line.c_str(), &brife_start, 10);
        ++brife_start;
        document.set_data(brife_start);

        pthread_rwlock_wrlock(&l);
        store.emplace(nid, Node(document));
        pthread_rwlock_unlock(&l);
    }
}

int32_t main(int32_t argc, char** argv) {
    ::google::ParseCommandLineFlags(&argc, &argv, true);
    pthread_rwlock_init(&l, NULL);

    com_logstat_t logstat;
    logstat.sysevents = FLAGS_log_level;
    com_device_t dev[1];
    strcpy(dev[0].type, "TTY");
    COMLOG_SETSYSLOG(dev[0]);
    com_openlog("main", dev, 1, &logstat);

    {
        ::std::ifstream doc("doc");
        ::std::string line;
        uint64_t num = 0;
        Document document;
        for (size_t i = 0; i < 32; i++) {
            document.add_ann_vector(0.0);
        }
        while (::std::getline(doc, line)) {
            if (++num > FLAGS_limit) {
                break;
            }
            char* brife_start = nullptr;
            uint64_t nid = strtoull(line.c_str(), &brife_start, 10);
            ++brife_start;
            document.set_data(brife_start);
            store.emplace(nid, Node(document));
        }
    }

    CNOTICE_LOG(" doc = %llu", store.size());

    ::baidu::rpc::Server server;
    ::RankServiceImpl rank_service(store);
    server.AddService(&rank_service, ::baidu::rpc::SERVER_DOESNT_OWN_SERVICE);

    baidu::rpc::ServerOptions options;
    server.Start(FLAGS_port, &options);

    ::std::thread status_thread(status);
    status_thread.detach();

    server.RunUntilAskedToQuit();

    return 0;
}


/**
 ** Copyright (c) 2014 Illumina, Inc.
 **
 ** This file is part of Illumina's Enhanced Artificial Genome Engine (EAGLE),
 ** covered by the "BSD 2-Clause License" (see accompanying LICENSE file)
 **
 ** \author Lilian Janin
 **/

#ifndef EAGLE_GENOME_QUALITY_MODEL_HH
#define EAGLE_GENOME_QUALITY_MODEL_HH

#include <fstream>
#include <boost/assign.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>

#include "model/Nucleotides.hh"
#include "genome/ErrorModelPlugin.hh"

namespace eagle
{
namespace genome
{

class MotifRepeatQualityDropInfo;

// ClusterErrorModelContext contains all the data that needs to be remembered to process consequent cycles of a read cluster
// It contains a substructure for each error plugin
struct ClusterErrorModelContext
{
    struct
    {
        unsigned int profileNumber;
    } qualityModelContext;

    struct
    {
        int qualityDrop;
    } phasingContext;

    struct
    {
        char lastBase;
        signed char errorDirection; // insertion=+1, deletion=-1
        unsigned int homopolymerLength;
    } homopolymerModelContext;

    struct
    {
        uint64_t kmer;
        unsigned int kmerLength;
        double qualityDropLevel;
        MotifRepeatQualityDropInfo *shortTermEffect;
        float shortTermQualityDrop;
    } motifQualityDropModelContext;

    ClusterErrorModelContext()
    {
        initialiseForNewRead();
    }

    void initialiseForNewRead()
    {
        qualityModelContext.profileNumber = 0;
        homopolymerModelContext.lastBase = 0;
        homopolymerModelContext.errorDirection = 0;
        homopolymerModelContext.homopolymerLength = 0;
        motifQualityDropModelContext.kmer = 0;
        motifQualityDropModelContext.kmerLength = 0;
        motifQualityDropModelContext.shortTermQualityDrop = 0;
        motifQualityDropModelContext.shortTermEffect = NULL;
        motifQualityDropModelContext.qualityDropLevel = 0;
        phasingContext.qualityDrop = 0;
    }
};

// QualityModel provides a way to generate the phred quality values
class QualityModel
{
public:
    QualityModel( const std::vector<boost::filesystem::path>& qualityTableFiles );

    unsigned int getQuality( boost::mt19937& randomGen, const unsigned int cycle, const char bclBase, ClusterErrorModelContext& clusterErrorModelContext );
    unsigned int getQuality( boost::mt19937& randomGen, const unsigned int cycle, ClusterErrorModelContext& clusterErrorModelContext );
    double qualToProbError(unsigned int qual);

private:
    unsigned int parseQualityTableFile( const boost::filesystem::path& filename, const int cycleOffset = 0 );
    void createDiscreteDistributionPerCycle();

    std::vector< std::vector< boost::random::discrete_distribution<> > > qualityDistPerCyclePerLastQuality;

    // New stuff
    unsigned int parseBigQualityTableFile( const boost::filesystem::path& filename );
    bool useNewStuff_;
    std::vector< unsigned int > bigTable_;
};

class SequencingMismatchModel
{
public:
    SequencingMismatchModel( const boost::filesystem::path& mismatchTableFilename );
    void apply( boost::mt19937& randomGen, const double errorRate, unsigned int& randomErrorType, char& bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
    std::vector< boost::random::discrete_distribution<> > errorDistPerBase;
};

class HomopolymerIndelModel
{
public:
    HomopolymerIndelModel( const boost::filesystem::path& homopolymerIndelTableFilename );
    void apply( boost::mt19937& randomGen, const double errorRate, unsigned int& randomErrorType, char& bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
    std::vector< double > homoDeletionTable_;
    std::vector< double > homoInsertionTable_;
};

class MotifQualityDropModel
{
public:
    MotifQualityDropModel( const boost::filesystem::path& tableFilename );
    void applyQualityDrop( unsigned int& quality, const char bclBase, ClusterErrorModelContext& clusterErrorModelContext, const unsigned int cycle, boost::mt19937& randomGen );

private:
    MotifRepeatQualityDropInfo* getMotifRepeatQualityDrop( const uint64_t kmer1, const unsigned int repeatKmerLength, const unsigned int repeatCount );
    bool active_;
    std::vector< std::map< uint64_t, boost::shared_ptr< std::vector< MotifRepeatQualityDropInfo > > > > tableData_;
    eagle::model::IUPAC baseConverter_;
};

class RandomQualityDropModel
{
public:
    RandomQualityDropModel( /*const boost::filesystem::path& tableFilename*/ );
    void applyQualityDrop( unsigned int& quality, const char bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
};

class QualityGlitchModel
{
public:
    QualityGlitchModel( /*const boost::filesystem::path& tableFilename*/ );
    void applyQualityDrop( unsigned int& quality, const char bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
};

class HappyPhasingModel
{
public:
    HappyPhasingModel( /*const boost::filesystem::path& tableFilename*/ );
    void applyQualityDrop( unsigned int& quality, const char bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
};

class QQTable
{
public:
    QQTable( const boost::filesystem::path& qqTableFilename );
    double qualToErrorRate( const unsigned int quality );

private:
    std::vector< double > qualityToProbability_;
};

class ErrorModel
{
public:
    enum ErrorType { NoError, BaseSubstitution, BaseDeletion, BaseInsertion } ;

    ErrorModel( const std::vector<boost::filesystem::path>& qualityTableFiles, const boost::filesystem::path& mismatchTableFile, const boost::filesystem::path& homopolymerIndelTableFilename, const boost::filesystem::path& motifQualityDropTableFilename, const boost::filesystem::path& qqTableFilename, const std::vector< std::string >& errorModelOptions );
    void getQualityAndRandomError( boost::mt19937& randomGen, const unsigned int cycle, const char base, unsigned int& quality, unsigned int& randomErrorType, char& bclBase, ClusterErrorModelContext& clusterErrorModelContext );

private:
    QualityModel qualityModel_;
    SequencingMismatchModel sequencingMismatchModel_;
    HomopolymerIndelModel homopolymerIndelModel_;
    MotifQualityDropModel motifQualityDropModel_;
    RandomQualityDropModel randomQualityDropModel_;
    QualityGlitchModel qualityGlitchModel_;
    HappyPhasingModel happyPhasingModel_;
    LongreadBaseDuplicationModel longreadBaseDuplicationModel_;
    LongreadDeletionModel longreadDeletionModel_;
    QQTable qqTable_;
    eagle::model::IUPAC baseConverter_;
};

} // namespace genome
} // namespace eagle

#endif // EAGLE_GENOME_QUALITY_MODEL_HH