#include <fstream>
#include <string>
#include <cmath>

// BOOST GRAPH LIBRARY (BGL)
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

// BOOST GEOMETRY
#include <boost/geometry.hpp>

// BOOST POLYGON
#include <boost/polygon/polygon.hpp>

// BOOST PROGRAM OPTIONS
#include<boost/program_options.hpp>

// FLAGS TO SET BEHAVIOUR
bool is_log;
double simplify_quantity;
bool fill_inside;
double travel_speed;
std::vector<double> radii;
double buffer_distance;
double additional_buffer_distance;
const double default_buffer_distance{ 0.0002 };
int points_per_circle;
bool subtract_smaller;
std::string in_file;
std::string out_file;
std::string centres;
bool predecessor;

// OTHER CONSTANTS
const double MULTIPLIER{ 100000000.0 };
// radius of Earth in km
const double R{ 6371.0 };
// factor to change degrees to radians
const double TO_RAD{ 3.1415926536 / 180.0 };

/*
ATTRIBUTES AT EACH VERTEX
We could choose to include osmid and highway, but for the sake of
saving memory we do not include them as they are not needed in the
scope of the project. We leave them in comment.
*/
struct VertexProperty
{
    // longitude at the vertex
    double x;
    // latitude at the vertex
    double y;
    // std::string osmid;
    // std::string highway;

    // predecessor vertex id of the vertex after shortest distance calculation
    int pred;
    // distance from the closest source vertex
    double dist;
};

/*
ATTRIBUTES OF EACH EDGE
Similarly we ignore osmid, name, highway, maxspeed, oneway.
*/
struct EdgeProperty
{
    // std::string osmid;
    // std::string name;
    // std::string highway;
    // std::string maxspeed;
    // std::string oneway;

    // length of the edge in meters
    double length;
    // Linestring that describes all points an edge composed of
    std::string geometry;
    // the weight in our calculations, the time it takes to travel the edge with a given speed
    double time;
    // true if a the edge is within <radius> distance from the closest cource
    std::vector<bool> included;
};

// the graph type we use with directed edges
using Graph_t = boost::adjacency_list<boost::vecS,
                                    boost::vecS,
                                    boost::directedS,
                                    VertexProperty,
                                    EdgeProperty>;

using Vertex_t = Graph_t::vertex_descriptor;
using Edge_t = Graph_t::edge_descriptor;

namespace bg = boost::geometry;
namespace gtl = boost::polygon;
namespace po = boost::program_options;

// types we define for the Boost Geometry Library
typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::polygon<point_t, false> polygon_t;
typedef bg::model::multi_polygon<polygon_t> mpolygon_t;
typedef bg::model::multi_point<point_t> mpoint_t;
typedef bg::model::linestring<point_t> linestring_t;
typedef bg::model::multi_linestring<linestring_t> mlinestring_t;
typedef bg::ring_type<polygon_t>::type ring_t;

// types we define for the Boost Polygon Library
typedef gtl::polygon_with_holes_data<long int> Polygon_Holes;
typedef gtl::polygon_data<long int> Polygon_NoHoles;
typedef gtl::point_data<long int> Point;
typedef gtl::polygon_set_data<long int> PolygonSet;
template <typename T>
struct lookup_polygon_set_type { typedef PolygonSet type; };
typedef typename lookup_polygon_set_type<gtl::property_merge<long int, long int>>::type polygon_set_type;
typedef std::map<std::set<long int>, polygon_set_type> property_merge_result_type;

// my_visitor ensures that we stop when up to radius we covered the graph,
// and do not compute the distance and predicate to all vertices
struct my_visitor : boost::default_dijkstra_visitor
{
    using base = boost::default_dijkstra_visitor;
    struct done{};

    my_visitor(double dist) : distance(dist) {}

    void finish_vertex(Vertex_t v, Graph_t const& g)
    {
        if (g[v].dist > distance)
            throw done{};

        base::finish_vertex(v, g);
    }

  private:
    double distance;
};

// spherical distance between two vertices given their azimuth angle (longitude)
// and zenith angle (latitude)
double haversine_dist(double ph1, double th1, double ph2, double th2)
{
	double dx, dy, dz;
	ph1 -= ph2;
	ph1 *= TO_RAD, th1 *= TO_RAD, th2 *= TO_RAD;

	dz = sin(th1) - sin(th2);
	dx = cos(ph1) * cos(th1) - cos(th2);
	dy = sin(ph1) * cos(th1);
	return asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * R;
}

// the haversine formula computes the id of the closest node in the graph
int closest_node(double x, double y, Graph_t g)
{
    double min_dist = 1000000000.0;
    double dist = 0.0;
    int min_id;
    for (auto vd : boost::make_iterator_range(vertices(g))) {
        dist = haversine_dist(x, y, g[vd].x, g[vd].y);
        if (dist < min_dist){
            min_dist = dist;
            min_id = vd;
        }
    }
    return min_id;
}

// converts BGL type polygon into GTL type polygon
// the credit goes to
// https://github.com/mmccoo/nerd_mmccoo/blob/master/boost_polygon_geometry/poly_utils.cpp
void geom_poly2gtl_poly(const polygon_t& boost_poly, Polygon_Holes& gtlpoly)
{
    std::vector<Point> pts;
    for (point_t pt : boost_poly.outer()) {
        pts.push_back(gtl::construct<Point>((long int) (pt.get<0>()*MULTIPLIER),
                                            (long int) (pt.get<1>()*MULTIPLIER)));
    }
    gtl::set_points(gtlpoly, pts.begin(), pts.end());

    std::vector<Polygon_NoHoles> holes;
    for (ring_t r: boost_poly.inners()) {
        std::vector<Point> pts;
        for (point_t pt : r) {
            pts.push_back(gtl::construct<Point>((long int) (pt.get<0>()*MULTIPLIER),
                                                (long int) (pt.get<1>()*MULTIPLIER)));
        }
        Polygon_NoHoles hole;
        gtl::set_points(hole, pts.begin(), pts.end());
        holes.push_back(hole);
    }
    gtl::set_holes(gtlpoly, holes.begin(), holes.end());
}

// converts GTL type polygon into BGL type polygon
// the credit goes to
// https://github.com/mmccoo/nerd_mmccoo/blob/master/boost_polygon_geometry/poly_utils.cpp
void gtl_poly2geom_poly(const Polygon_Holes& gtlpoly, polygon_t& boost_poly)
{
    boost_poly.clear();
    for(Point pt : gtlpoly) {
        boost_poly.outer().push_back(
            point_t((double)gtl::x(pt)/MULTIPLIER,(double)gtl::y(pt)/MULTIPLIER));
    }

    int num_holes{ 0 };
    for(auto iter = gtlpoly.begin_holes(); iter != gtlpoly.end_holes(); ++iter) {
        num_holes++;
        const gtl::polygon_data<long int> h = *iter;

        ring_t r;
        for(Point pt : h) {
            r.push_back(point_t((double)gtl::x(pt)/MULTIPLIER,(double)gtl::y(pt)/MULTIPLIER));
        }
        boost_poly.inners().push_back(r);
    }

    // if gtl goes in the wrong direction. clockwise vs counter-clockwise.
    bg::correct(boost_poly);
}

// rescales Linestring
void scale_geom(mlinestring_t mls, mlinestring_t &mls_scaled, double scale_x, double scale_y)
{
    mls_scaled.clear();

    for (linestring_t ls : mls) {
        std::vector<point_t> pts;
        linestring_t ls_out;

        for (point_t pt : ls){
            pts.push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
        }
        bg::assign_points(ls_out, pts);
        mls_scaled.push_back(ls_out);
    }
}

// rescales Polygon
void scale_geom(polygon_t  poly, polygon_t &poly_scaled, double scale_x, double scale_y)
{
    poly_scaled.clear();

    for (point_t pt : poly.outer()) {
        poly_scaled.outer().push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
    }

    std::vector<Polygon_NoHoles> holes;
    for (ring_t r: poly.inners()) {
        std::vector<point_t> pts;
        for (point_t pt : r) {
            pts.push_back(point_t(pt.get<0>()*scale_x, pt.get<1>()*scale_y));
        }
        ring_t ring;
        bg::assign_points(ring, pts);
        poly_scaled.inners().push_back(ring);
    }
}

// rescales Multipolygon
void scale_geom(mpolygon_t  mpoly, mpolygon_t &mpoly_scaled, double scale_x, double scale_y)
{
    mpoly_scaled.clear();

    for (polygon_t poly : mpoly) {
        polygon_t poly_scaled;
        scale_geom(poly, poly_scaled, scale_x, scale_y);
        mpoly_scaled.push_back(poly_scaled);
    }
}

// reads graphml file into Graph_t
// Again we omit certain OSM properties to save memory. These are left in comment.
Graph_t ReadGraph(std::string fn)
{
    std::ifstream is(fn.c_str());
    if (!is.is_open()) {
        std::cout << "loading file '" << fn << "'failed." << std::endl;
        throw "Could not load file.";
    }
    Graph_t graph;
    boost::dynamic_properties dp(boost::ignore_other_properties);
    dp.property("x", boost::get(&VertexProperty::x, graph));
    dp.property("y", boost::get(&VertexProperty::y, graph));
    // dp.property("osmid", boost::get(&VertexProperty::osmid, graph));
    // dp.property("highway", boost::get(&VertexProperty::highway, graph));

    // dp.property("osmid", boost::get(&EdgeProperty::osmid, graph));
    // dp.property("name", boost::get(&EdgeProperty::name, graph));
    // dp.property("highwaye", boost::get(&EdgeProperty::highway, graph));
    // dp.property("maxspeed", boost::get(&EdgeProperty::maxspeed, graph));
    // dp.property("oneway", boost::get(&EdgeProperty::oneway, graph));
    dp.property("length", boost::get(&EdgeProperty::length, graph));
    dp.property("geometry", boost::get(&EdgeProperty::geometry, graph));

    boost::read_graphml(is, graph, dp);

    return graph;
};

// read the source points from text file which contains a WKT Linestring
linestring_t read_src_points(std::string centres)
{
    std::ifstream input(centres);
    std::string input_line;
    for(std::string line; getline(input, line); ) {
        input_line += line;
    }
    linestring_t src_points;
    bg::read_wkt(input_line, src_points);
    return src_points;
}

// writes radii and Multipolygons into files
void write_isochrones(std::string out_file, std::vector<mpolygon_t> isochrone_polys)
{
    std::ofstream poly_file(out_file);
    for (int r : radii) {
        poly_file << r << " ";
    }
    poly_file << std::endl;
    for (int i=0; i<radii.size(); i++) {
        poly_file << std::setprecision(12) << bg::wkt(isochrone_polys[i]) << std::endl;
    }
      poly_file.flush();
      poly_file.close();
}

// only changes the "included" property if the edge
// is reached within a certain radius with, this is done with Dijkstra's algorithm
void get_isochrones(int source, std::vector<double> radii, Graph_t &g) {
    double maxi = -1;
    for (auto radius : radii) {
        if (maxi < radius) maxi = radius;
    }
    my_visitor vis { maxi };

    try {
    boost::dijkstra_shortest_paths(g, source,
                          boost::visitor(vis)
                          .predecessor_map(get(&VertexProperty::pred, g))
                          .distance_map(get(&VertexProperty::dist, g))
                          .weight_map(get(&EdgeProperty::time, g)));
    } catch(my_visitor::done const&) {
    }

    for (int i=0; i<radii.size(); i++) {
        Vertex_t src, tar;
        auto es = boost::edges(g);
        for (auto eit = es.first; eit != es.second; ++eit) {
            src = boost::source(*eit, g);
            tar = boost::target(*eit, g);
            if (g[tar].dist < radii[i] && g[src].dist < radii[i]) {
                if(predecessor == true && g[tar].pred != src) {
                    continue;
                }
                g[*eit].included[i] = true;
            }
        }
    }
}

// unary union from a property_merge into mpolygon_t
void polygon_merge(gtl::property_merge<long int, long int> pm, mpolygon_t &multipolygon)
{
    property_merge_result_type merge_result;
    pm.merge(merge_result);
    PolygonSet result = merge_result.begin()->second;
    std::vector<Polygon_Holes> polys;
    result.get(polys);
    for (int k=0; k<polys.size(); k++) {
        polygon_t poly;
        gtl_poly2geom_poly(polys[k], poly);
        multipolygon.push_back(poly);
    }
}

// computes the Multilinestring within a given radius
void get_multilinestring_within(int i, Graph_t g, mlinestring_t &mls)
{
    auto es = boost::edges(g);

    // for a directed edge src is the source and tar is the target vertex
    Vertex_t src, tar;

    linestring_t ls;

    // we loop through all the edges in the graph
    for (auto eit = es.first; eit != es.second; ++eit) {
        // if the i-th smallest radius is in reach
        if (g[*eit].included[i] == true) {
            // if i > 0 and we already processed the edge in the previous run
            // of the outmost loop then we continue in the loop
            if (i > 0 && g[*eit].included[i-1] == true) {
                continue;
            }

            src = boost::source(*eit, g);
            tar = boost::target(*eit, g);
            std::string str(g[*eit].geometry);

            // if the geometry of the edge is not empty then we read
            // the geometry into Linestring
            if (str.length() > 1) {
                bg::read_wkt(str, ls);
            }
            // else we consider the endpoints of of the edge as Linestring
            else {
                std::vector<point_t> pts;
                pts.push_back(point_t(g[src].x, g[src].y));
                pts.push_back(point_t(g[tar].x, g[tar].y));
                bg::assign_points(ls, pts);
            }
            mls.push_back(ls);
        }
    }
}

// applies transformations on the geometry
void build_isochrone_geometry(std::vector<mpolygon_t> &isochrone_polys,
                            std::vector<Polygon_Holes> polys, double lat_lon_ratio)
{
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(buffer_distance);
    bg::strategy::buffer::join_round join_strategy(points_per_circle);
    bg::strategy::buffer::end_round end_strategy(points_per_circle);
    bg::strategy::buffer::point_circle circle_strategy(points_per_circle);
    bg::strategy::buffer::side_straight side_strategy;

    // auxiliary variables for further computations with polygons
    polygon_t    poly, poly_aux;
    ring_t       ring;
    mpolygon_t   mpoly, mpoly_scaled, mpoly_collect;

    // we loop over radii starting with the smallest
    for (int j=0; j<polys.size(); j++) {
        gtl_poly2geom_poly(polys[j], poly);
            if (fill_inside == true && predecessor == false) {
                ring = poly.outer();
                bg::convert(ring, poly_aux);
                bg::simplify(poly_aux, poly, simplify_quantity);
                bg::strategy::buffer::distance_symmetric<double>
                    distance_strategy(additional_buffer_distance);
                bg::buffer(poly, mpoly, distance_strategy, side_strategy,
                           join_strategy, end_strategy, circle_strategy);
                scale_geom(mpoly, mpoly_scaled, 1/lat_lon_ratio, 1);
                ring.clear();
                ring = mpoly_scaled[0].outer();
                mpoly_scaled.clear();
                mpoly.clear();
                poly.clear();
                bg::convert(ring, poly);
            } else {
                bg::simplify(poly, poly_aux, simplify_quantity);
                scale_geom(poly_aux, poly, 1/lat_lon_ratio, 1);
            }
            mpoly_collect.push_back(poly);
    }
    isochrone_polys.push_back(mpoly_collect);
}

void union_of_fill_inside_true_predecessor_false(std::vector<mpolygon_t> &isochrone_polys)
{
    std::vector<mpolygon_t> isochrone_polys_union;
    gtl::property_merge<long int, long int> pm;
    for (mpolygon_t mpoly : isochrone_polys) {
        for (polygon_t poly : mpoly){
            Polygon_Holes pl;
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 0);
        }
        mpolygon_t multipolygon, multipolygon_outer;
        polygon_merge(pm, multipolygon);
        for (polygon_t poly: multipolygon) {
            ring_t ring = poly.outer();
            poly.clear();
            bg::convert(ring, poly);
            multipolygon_outer.push_back(poly);
        }
        isochrone_polys_union.push_back(multipolygon_outer);
    }
    isochrone_polys.clear();
    isochrone_polys.insert(isochrone_polys.end(),
                           isochrone_polys_union.begin(), isochrone_polys_union.end());
}

void subtract_smaller_polygon(std::vector<mpolygon_t> &isochrone_polys)
{
    std::vector<mpolygon_t> isochrone_polys_difference;
    for (int j=0; j<isochrone_polys.size()-1; j++) {
        if (j == 0) {
            isochrone_polys_difference.push_back(isochrone_polys[0]);
        }
        gtl::property_merge<long int, long int> pm;
        Polygon_Holes pl;
        for (polygon_t poly : isochrone_polys[j+1]) {
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 0);
        }
        for (polygon_t poly : isochrone_polys[j]) {
            geom_poly2gtl_poly(poly, pl);
            pm.insert(pl, 1);
        }
        mpolygon_t multipolygon;
        polygon_merge(pm, multipolygon);
        isochrone_polys_difference.push_back(multipolygon);
    }
    isochrone_polys.clear();
    isochrone_polys.insert(isochrone_polys.end(),
                           isochrone_polys_difference.begin(), isochrone_polys_difference.end());
}

// obtaining command line options
int cmd_input(int ac, char * av[],
            bool &is_log,
            double &simplify_quantity,
            bool &fill_inside,
            double &travel_speed,
            std::vector<double> &radii,
            double &buffer_distance,
            double &additional_buffer_distance,
            int &points_per_circle,
            bool &subtract_smaller,
            std::string &in_file,
            std::string &out_file,
            std::string &centres,
            bool &predecessor)
{
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("verbose", po::bool_switch(&is_log)->default_value(false),
        "prints log details")
        ("simplify", po::value<double>(&simplify_quantity),
        "the epsilon in Ramer-Douglas-Peucker algorithm, the bigger the number is, the less points shall be present in the output")
        ("fill", po::bool_switch(&fill_inside)->default_value(false),
        "if the polygon contains holes it removes them")
        ("travel-speed", po::value<double>(&travel_speed)->default_value(20.0),
        "the travel speed in km/h")
        ("radii", po::value<std::vector<double>>(&radii)
                ->multitoken()->default_value(std::vector<double>{10,20,30,40,50,60},
        "10,20,30,40,50,60"), "the radii of the isochrone curves in minutes")
        ("circle-points", po::value<int>(&points_per_circle)->default_value(1),
        "the radii of the isochrone curves in minutes")
        ("subtract", po::bool_switch(&subtract_smaller)->default_value(false),
        "subtracts the smaller radius polygon from the larger thus creating a hole in that")
        ("input-file", po::value<std::string>(&in_file)->required(),
        "the input .graphml file")
        ("output-file,o", po::value<std::string>(&out_file)->default_value("out.txt"),
        "the output text file")
        ("sources", po::value<std::string>(&centres)->default_value("sources.txt"),
        "a file containing a WKT Linestring whose vertices are the sources")
        ("predecessor", po::bool_switch(&predecessor)->default_value(false),
        "computes the isochrone curves with a shortest path tree, fill does not affect it, it is not adviced to have multiple sources in this case")
        ("buffer-distance", po::value<double>(&buffer_distance)->default_value(default_buffer_distance),
        "thickness added to the roads and polygons")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc), vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 1;
    }

    po::notify(vm);

    if (buffer_distance <= 0) {
        std::cout << "the buffer distance must be positive\n";
        return 1;
    }

    if (fill_inside == true and predecessor == false) {
        if (buffer_distance > default_buffer_distance*2) {
            additional_buffer_distance = buffer_distance - default_buffer_distance;
            buffer_distance = default_buffer_distance;
        }
        else {
            buffer_distance /= 2;
            additional_buffer_distance = buffer_distance;
        }
    }
    else {
        if (buffer_distance > default_buffer_distance*2) {
            std::cout << "WARNING: too large buffer distance may result in the program never complete.\n";
        }
    }
    return 0;
}

int main(int ac, char * av[])
{
    // command line arguments
    int error = cmd_input(
            ac, av,
            is_log,
            simplify_quantity,
            fill_inside,
            travel_speed,
            radii,
            buffer_distance,
            additional_buffer_distance,
            points_per_circle,
            subtract_smaller,
            in_file,
            out_file,
            centres,
            predecessor
    );

    if (error == 1){
        return 1;
    }

    if (is_log == true) {
        std::cout << "File \"" << in_file << "\" starts being read.\n";
    }

    Graph_t g = ReadGraph(in_file);

    if (is_log == true) {
        std::cout << "File \"" << in_file << "\" is read.\n";
    }

    double meters_per_minute{ travel_speed * 1000.0 / 60.0 };
    auto es = boost::edges(g);

    // we initialise "included" as a vector of falses
    for (auto eit = es.first; eit != es.second; ++eit) {
        g[*eit].time =  (g[*eit].length / meters_per_minute);
        for (int i=0; i<radii.size(); i++) {
            g[*eit].included.push_back(false);
        }
    }

    // 40075==circumference of Earth in km;
    // 111.32==1 degree of latitude in km
    // lat_lon_ratio expresses the ratio of one meter of latitude and longitude
    // at given latitude
    double lat_lon_ratio{ 40075.0 * cos(g[0].y * TO_RAD) / 360.0 / 111.32 };

    // loads the source points
    linestring_t src_points = read_src_points(centres);

    int idx = 0;
    auto size = src_points.size();
    for (point_t pt : src_points) {
        idx++;
        int source = closest_node(pt.get<0>(), pt.get<1>(), g);
        if (is_log == true) {
            std::cout << "#" <<  idx << "/" << size << ", " << "x="
                      << pt.get<0>() << ", y=" << pt.get<1>() << "\n";
        }
        get_isochrones(source, radii, g);
    }
    if (is_log == true) {
        std::cout << "Dijkstra is done.\n";
        std::cout << "Geometry is being assembled.\n";
    }

    //buffer strategy, for reference visit the website of BGL
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(buffer_distance);
    bg::strategy::buffer::join_round join_strategy(points_per_circle);
    bg::strategy::buffer::end_round end_strategy(points_per_circle);
    bg::strategy::buffer::point_circle circle_strategy(points_per_circle);
    bg::strategy::buffer::side_straight side_strategy;

    // we collect the computed isochronic polygons in "isochrone_polys"
    std::vector<mpolygon_t> isochrone_polys;

    // the result of GTL property merge (union in our case) is converted into
    // std::vector<Polygon_Holes> which is stored in "polys,"
    // futhermore the previous (smaller) isochronic curve is inserted into the
    // next one thus speeding up computation, which is done with "polys"
    std::vector<Polygon_Holes> polys;

    // we loop over radii starting with the smallest
    for (int i=0; i<radii.size(); i++) {
        // auxiliary variables for polygon manipulations
        mlinestring_t mls, mls_scaled;
        mpolygon_t   mpoly_aux;
        Polygon_Holes pl;

        get_multilinestring_within(i, g, mls);

        // at this point if collected in a vector, the isochronic Multilinestrings
        // could be saved

        // we collect the polygons to be unioned in "pm"
        gtl::property_merge<long int, long int> pm;

        // we insert the previous isochronic curve here thus gaining speed
        for (Polygon_Holes polygon : polys) {
            pm.insert(polygon, 0);
        }
        // polys emptied for further computations after insertion
        polys.clear();

        // in buffer we scale the geometries thus buffer affects x and y directions equally
        scale_geom(mls, mls_scaled, lat_lon_ratio, 1);

        // memory saving for the rest of the loop
        mls.clear();

        // buffer applied to the scaled Multilinestring
        for (linestring_t ls : mls_scaled) {
            bg::buffer(ls, mpoly_aux, distance_strategy, side_strategy,
                       join_strategy, end_strategy, circle_strategy);
            geom_poly2gtl_poly(mpoly_aux.at(0), pl);
            pm.insert(pl, 0);
        }

        // "merge_result" holds the result for the unary union
        property_merge_result_type merge_result;
        pm.merge(merge_result);
        PolygonSet result = merge_result.begin()->second;
        result.get(polys);

        build_isochrone_geometry(isochrone_polys, polys, lat_lon_ratio);

    }

    if (fill_inside == true && predecessor == false) {
        union_of_fill_inside_true_predecessor_false(isochrone_polys);
    }

    if (subtract_smaller == true) {
        subtract_smaller_polygon(isochrone_polys);
    }

    // writing result into file
    write_isochrones(out_file, isochrone_polys);

      if (is_log == true) {
          std::cout << "Polygons are written into \"" << out_file << "\"\n";
      }
      return 0;
}


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

#include <x/vio/msckf_slam_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

MsckfSlamUpdate::MsckfSlamUpdate(const x::TrackList& trks,
                                 const x::AttitudeList& quats,
                                 const x::TranslationList& pos,
                                 const Triangulation& triangulator,
                                 const MatrixXd& cov_s,
                                 const int n_poses_max,
                                 const double sigma_img)
{
  // Number of features
  const size_t n_trks = trks.size();

  // Number of feature observations
  size_t n_obs = 0;
  for(size_t i=0; i < n_trks; i++)
    n_obs += trks[i].size();

  // Initialize MSCKF Kalman update matrices
  const size_t rows0 = 2 * n_obs - n_trks * 3;
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(rows0, cols);
  cov_m_diag_ = VectorXd::Ones(rows0);
  res_ = MatrixXd::Zero(rows0, 1);

  // Initialize MSCKF-SLAM feature initialization matrices
  const size_t rows1 = n_trks * 3;
  init_mats_.H1 = MatrixXd::Zero(rows1, cols);
  init_mats_.H2 = MatrixXd::Zero(rows1, n_trks * 3);
  init_mats_.r1 = MatrixXd::Zero(rows1, 1);
  init_mats_.features = MatrixXd::Zero(rows1, 1);

  // For each track, compute residual, Jacobian and covariance block
  const double var_img = sigma_img * sigma_img;
  size_t row_h = 0, row1 = 0;
  for (size_t i = 0; i < n_trks; ++i) {
    processOneTrack(trks[i],
                    quats,
                    pos,
                    triangulator,
                    cov_s,
                    n_poses_max,
                    var_img,
                    i,
                    row_h,
                    row1);
  }
}

void MsckfSlamUpdate::processOneTrack(const x::Track& track,
                                      const x::AttitudeList& C_q_G,
                                      const x::TranslationList& G_p_C,
                                      const Triangulation& triangulator,
																      const MatrixXd& P,
                                      const int n_poses_max,
                                      const double var_img,
                                      const size_t& j,
                                      size_t& row_h,
                                      size_t& row1)
{
  const size_t track_size = track.size();
  unsigned int rows_track_j = track_size * 2;
  const size_t cols = P.cols();
  MatrixXd h_j(MatrixXd::Zero(rows_track_j, cols));
  MatrixXd Hf_j(MatrixXd::Zero(h_j.rows(), kJacCols));
  MatrixXd res_j(MatrixXd::Zero(rows_track_j, 1));

  // Feature triangulation
  Vector3d feature; // inverse-depth parameters in last observation frame
  triangulator.triangulateGN(track, feature);
  const double alpha = feature(0);
  const double beta  = feature(1);
  const double rho   = feature(2);

  // Anchor pose
  x::Quaternion Cn_q_G;
  Cn_q_G.x() = C_q_G.back().ax;
  Cn_q_G.y() = C_q_G.back().ay;
  Cn_q_G.z() = C_q_G.back().az;
  Cn_q_G.w() = C_q_G.back().aw;

  Vector3d G_p_Cn(G_p_C.back().tx, G_p_C.back().ty, G_p_C.back().tz);

  // Coordinate of feature in global frame
  Vector3d G_p_fj = 1 / (rho)*Cn_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Cn;

  x::Quatern attitude_to_quaternion;

  // LOOP OVER ALL FEATURE OBSERVATIONS
  for (size_t i = 0; i < track_size; ++i)
  {
    const unsigned int pos = C_q_G.size() - track_size + i;

    Quaterniond Ci_q_G_ = attitude_to_quaternion(C_q_G[pos]);
    Vector3d G_p_Ci_(G_p_C[pos].tx, G_p_C[pos].ty, G_p_C[pos].tz);

    // Feature position expressed in camera frame.
    Vector3d Ci_p_fj;
    Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

    // eq. 20(a)
    Vector2d z;
    z(0) = track[i].getX();
    z(1) = track[i].getY();

    Vector2d z_hat(z);
    assert(Ci_p_fj(2));
    z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2);
    z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2);

    // eq. 20(b)
    res_j(i * 2, 0) = z(0) - z_hat(0);
    res_j(i * 2 + 1, 0) = z(1) - z_hat(1);

    //============================
    // Measurement Jacobian matrix
    //============================

    if (i == track_size - 1)  // Handle special case
    {
      // Inverse-depth feature coordinates jacobian
      MatrixXd mat(MatrixXd::Zero(2, 3));
      mat(0, 0) = 1.0;
      mat(1, 1) = 1.0;

      // Update stacked Jacobian matrices associated to the current feature
      unsigned int row = i * kVisJacRows;
      Hf_j.block<kVisJacRows, kJacCols>(row, 0) = mat;
    }
    else
    {
      // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
      VisJacBlock J_i(VisJacBlock::Zero());
      // first row
      J_i(0, 0) = 1.0 / Ci_p_fj(2);
      J_i(0, 1) = 0.0;
      J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2);
      // second row
      J_i(1, 0) = 0.0;
      J_i(1, 1) = 1.0 / Ci_p_fj(2);
      J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2);

      // Attitude
      Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);
      VisJacBlock J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix;

      // Position
      VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

      // Anchor attitude
      VisJacBlock J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                                      Cn_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix;

      // Anchor position
      VisJacBlock J_anchor_pos = -J_position;

      // Inverse-depth feature coordinates
      MatrixXd mat(MatrixXd::Identity(3, 3));
      mat(0, 2) = -alpha / rho;
      mat(1, 2) = -beta / rho;
      mat(2, 2) = -1 / rho;
      VisJacBlock Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                               Cn_q_G.normalized().toRotationMatrix() * mat;

      // Update stacked Jacobian matrices associated to the current feature
      unsigned int row = i * kVisJacRows;
      Hf_j.block<kVisJacRows, kJacCols>(row, 0) = Hf_j1;

      unsigned int col = pos * kJacCols;
      h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_position;

      col += n_poses_max * kJacCols;
      h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_attitude;

      col = (C_q_G.size() - 1) * kJacCols;
      h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_pos;

      col += n_poses_max * kJacCols;
      h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_att;
    }
  }  // LOOP OVER ALL FEATURE OBSERVATIONS

  //========================================================================
  // Left nullspace projection
  //========================================================================
  // Nullspace computation
  MatrixXd q = Hf_j.householderQr().householderQ();
  MatrixXd A = x::MatrixBlock(q, 0, 3);

  // Projections
  MatrixXd res0_j = A.transpose() * res_j;
  MatrixXd h0_j = A.transpose() * h_j;

  // New noise measurement matrix
  VectorXd r0_j_diag = var_img * VectorXd::Ones(rows_track_j - 3);
  MatrixXd r0_j = r0_j_diag.asDiagonal();

  //========================================================================
  // Column space projection
  //========================================================================
  // Only needed for persistent feature init (Li, 2012)

	// Column space
	const MatrixXd U = x::MatrixBlock(q, 0, 0, q.rows(), 3);

	// Projections to be used in Core::CorrectAfterCoreCorrection
	MatrixXd H1j = U.transpose() * h_j;
	MatrixXd H2j = U.transpose() * Hf_j;
	MatrixXd r1j = U.transpose() * res_j;

  init_mats_.H1.block(row1, 0, 3, cols) = H1j;
	init_mats_.H2.block(row1, row1, 3, 3) = H2j;
	init_mats_.r1.block(row1, 0, 3, 1)	= r1j;
	init_mats_.features.block(row1, 0, 3, 1)= feature;
	row1 += 3;

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  MatrixXd S_inv = (h0_j * P * h0_j.transpose() + r0_j).inverse();
  MatrixXd gamma = res0_j.transpose() * S_inv * res0_j;
  boost::math::chi_squared_distribution<> my_chisqr(2 * track_size - 3);  // 2*Mj-3 DoFs
  double chi = quantile(my_chisqr, 0.95);                            // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
#ifdef VERBOSE
    inliers_.push_back(G_p_fj);
#endif

    jac_.block(row_h,            // startRow
             0,                 // startCol
             rows_track_j - 3,  // numRows
             cols) = h0_j;    // numCols

    // Residual vector [feature j]
    res_.block(row_h, 0, rows_track_j - 3, 1) = res0_j;

    // Measurement covariance matrix [feature j]
    cov_m_diag_.segment(row_h, rows_track_j - 3) = r0_j_diag;

    row_h += rows_track_j - 3;
	}
  else  // outlier
  {
#ifdef VERBOSE
    outliers_.push_back(G_p_fj);
#endif
  }
}


/*
Copyright (c) 2020 Naomasa Matsubayashi

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

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

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

#include <cstdint>
#include <cmath>
#include <array>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>
#include <charconv>
#include <random>
#include <boost/container/flat_map.hpp>
#include <boost/program_options.hpp>
#include <omp.h>
#include <nlohmann/json.hpp>
#include "ifm/exp_match.h"
int main( int argc, char *argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("decay,d", boost::program_options::value<bool>()->default_value( true ), "decay")
    ("alpha,a", boost::program_options::value<float>()->default_value( 1 ), "a")
    ("beta,b", boost::program_options::value<float>()->default_value( 1 ), "b")
    ("length,l", boost::program_options::value<float>()->default_value( 1 ), "l");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") ) {
    std::cout << options << std::endl;
    return 0;
  }
  float x0, x1, x2, y0, y1, y2;
  float a = params["alpha"].as<float>();
  float b = params["beta"].as<float>();
  if( params["decay"].as<bool>() ) {
    const auto [d1,d2] = ifm::approxymate_decay( a, b, params["length"].as<float>() );
    x0 = 0;
    x1 = d1;
    x2 = d2;
    y0 = ifm::get_exp_envelope( a, b, x0 );
    y1 = ifm::get_exp_envelope( a, b, x1 );
    y2 = ifm::get_exp_envelope( a, b, x2 );
    std::cout << d1 << " " << d2 << std::endl;
  }
  else {
    x0 = 0;
    x1 = ifm::approxymate_attack( a, b, params["length"].as<float>() );
    x2 = params["length"].as<float>();
    y0 = ifm::get_exp_envelope( a, b, x0 );
    y1 = ifm::get_exp_envelope( a, b, x1 );
    y2 = ifm::get_exp_envelope( a, b, x2 );
    std::cout << a << std::endl;
  }
  const auto [tangent0,shift0] = ifm::get_linear_interpolation( x0, y0, x1, y1 );
  const auto [tangent1,shift1] = ifm::get_linear_interpolation( x1, y1, x2, y2 );
  float diff = 0;
  for( size_t i = 0; i != 1000; ++i ) {
    float x = i * params["length"].as<float>() / 1000.f;
    float approx_y;
    if( x < x1 ) approx_y = x * tangent0 + shift0;
    else if( x < x2 ) approx_y = x * tangent1 + shift1;
    else approx_y = y2;
    float y = ifm::get_exp_envelope( a, b, x );
    diff += approx_y - y;
  }
  std::cout << "max(" << tangent0 << "x+" << shift0 << "," << tangent1 << "x+" << shift1 << "," << y2 << ")" << std::endl;
  std::cout << "diff: " << diff/1000.f << std::endl;
}


/*
 * word2vec.cpp
 *
 */
#include <fstream>
#include <random>
#include <set>
#include <string>
#include <utility>

#include <boost/assert.hpp>

#include "core/utils.h"
#include "core/functions.h"
#include "core/updaters.h"
#include "layers/contlayer.h"
#include "layers/fclayer.h"
#include "layers/smaxlayer.h"

#include "word2vec.h"

using namespace std;
using namespace boost;
using namespace yann;
using namespace yann::word2vec;

namespace yann::word2vec {

class DataSource_SkipGram : public Trainer::DataSource
{
  typedef vector<pair<MatrixSize, MatrixSize>> TestsVector;

public:
  DataSource_SkipGram(
      const Text & text,
      const MatrixSize & window_size,
      const MatrixSize & batch_size
  ) :
    _text(text),
    _window_size(window_size),
    _batch_size(batch_size),
    _cur_pos(0)
  {
    YANN_CHECK_GT(_batch_size, 0);
    YANN_CHECK_GT(_text.get_dictionary_size(), 0);

    _tests = get_tests(_text, _window_size);
    resize_batch(_inputs_batch, _batch_size, _text.get_dictionary_size());
    resize_batch(_outputs_batch, _batch_size, _text.get_dictionary_size());
  }
  virtual ~DataSource_SkipGram()
  {
  }

  // Trainer::DataSource overwrites
  virtual std::string get_info() const
  {
    ostringstream oss;
    oss << "yann::DataSource_SkipGram("
        << "dictionary_size=" << _text.get_dictionary_size()
        << ", batch_size=" << _batch_size
        << ", window_size=" << _window_size
        << ", tests_num=" << _tests.size()
        << ")";
    return oss.str();
  }
  virtual MatrixSize get_batch_size() const
  {
    return _batch_size;
  }

  virtual MatrixSize get_num_batches() const
  {
    YANN_CHECK(_batch_size > 0);
    return _tests.size() / _batch_size;
  }

  virtual void start_epoch()
  {
    _cur_pos = 0;

    // todo: support sequential mode w/o shuffling too
    auto rng = default_random_engine { };
    shuffle(begin(_tests), end(_tests), rng);
  }

  virtual optional<Batch> get_next_batch()
  {
    YANN_CHECK_GE(_cur_pos, 0);
    auto res = create_batch(_cur_pos);
    if(res <= 0) {
      return boost::none;
    }
    _cur_pos += res;
    return Batch(_inputs_batch, _outputs_batch, get_batch_size()); // each row in the batch is a test
  }
  virtual void end_epoch()
  {
    // do nothing
  }

public:
  double test_all(const Network & nn)
  {
    size_t success_count = 0, total_count = 0;

    VectorBatch outputs;
    outputs.resizeLike(_outputs_batch);
    for(MatrixSize start_pos = 0; ; ) {
      auto res = create_batch(start_pos);
      if(res <= 0) {
        break;
      }
      start_pos += res;

      nn.calculate(_inputs_batch, outputs);
      for(MatrixSize jj = 0; jj < yann::get_batch_size(outputs); ++jj) {
        const auto & actual = get_batch(outputs, jj);
        const auto & expected = get_batch(_outputs_batch, jj);
        MatrixSize aa = 0, ee = 0;
        actual.array().maxCoeff(&aa);
        expected.array().maxCoeff(&ee);
        if(aa == ee) {
          ++success_count;
        }
        ++total_count;
      }
    }

    return total_count > 0 ? success_count / (double)total_count : 0.0;
  }

private:
  MatrixSize create_batch(const MatrixSize & start_pos)
  {
    YANN_CHECK_GE(start_pos, 0);

    _inputs_batch.setZero();
    _outputs_batch.setZero();
    for(MatrixSize ii = 0, pos = start_pos; ii < _batch_size; ++ii, ++pos) {
      if(pos >= (MatrixSize)_tests.size()) {
        return 0;
      }
      const auto & cur =  _tests[pos];
      _inputs_batch.insert(ii, cur.first) = 1.0; // RowMajor
      _outputs_batch(ii, cur.second) = 1.0; // RowMajor
    }
    return _batch_size;
  }

  static TestsVector get_tests(const Text & text, const MatrixSize & window_size)
  {
    TestsVector tests;
    for(MatrixSize ii = 0, size = text.sentences().size(); ii < size; ++ii) {
      const auto & sentence = text.sentences()[ii];
      // sentence[jj] is the "input" word
      for(MatrixSize jj = 0, sentence_size = sentence.size(); jj < sentence_size; ++jj) {
        // sentence[kk] is the "output" word
        for(MatrixSize kk = jj - window_size; kk <= jj + window_size; ++kk) {
          // check if we are in-range and that we are not pointing to the word itself
          if(kk < 0 || kk >= sentence_size || sentence[jj] == sentence[kk]) {
            continue;
          }
          tests.push_back(make_pair(sentence[jj], sentence[kk]));
        }
      }
    }
    return tests;
  }

private:
  const Text & _text;
  const MatrixSize _window_size;
  const MatrixSize _batch_size;

  TestsVector _tests;
  size_t      _cur_pos;

  SparseVectorBatch _inputs_batch;
  VectorBatch _outputs_batch;
}; // class DataSource_SkipGram

class DataSource_CBOW : public Trainer::DataSource
{
  typedef set<MatrixSize> BagOfWords;
  typedef vector<pair<MatrixSize, BagOfWords>> TestsVector;

public:
  DataSource_CBOW(
      const Text & text,
      const MatrixSize & window_size,
      const MatrixSize & batch_size
  ) :
    _text(text),
    _window_size(window_size),
    _batch_size(batch_size),
    _cur_pos(0)
  {
    YANN_CHECK_GT(_batch_size, 0);
    YANN_CHECK_GT(_text.get_dictionary_size(), 0);

    _tests = get_tests(_text, _window_size);
    resize_batch(_inputs_batch, _batch_size, _text.get_dictionary_size());
    resize_batch(_outputs_batch, _batch_size, _text.get_dictionary_size());
  }
  virtual ~DataSource_CBOW()
  {
  }

  // Trainer::DataSource overwrites
  virtual std::string get_info() const
  {
    ostringstream oss;
    oss << "yann::DataSource_CBOW("
        << "dictionary_size=" << _text.get_dictionary_size()
        << ", batch_size=" << _batch_size
        << ", window_size=" << _window_size
        << ", tests_num=" << _tests.size()
        << ")";
    return oss.str();
  }
  virtual MatrixSize get_batch_size() const
  {
    return _batch_size;
  }

  virtual MatrixSize get_num_batches() const
  {
    YANN_CHECK(_batch_size > 0);
    return _tests.size() / _batch_size;
  }
  virtual void start_epoch()
  {
    _cur_pos = 0;

    // todo: support sequential mode w/o shuffling too
    auto rng = default_random_engine { };
    shuffle(begin(_tests), end(_tests), rng);
  }

  virtual optional<Batch> get_next_batch()
  {
    YANN_CHECK_GE(_cur_pos, 0);
    auto res = create_batch(_cur_pos);
    if(res <= 0) {
      return boost::none;
    }
    _cur_pos += res;
    return Batch(_inputs_batch, _outputs_batch, get_batch_size()); // each row in the batch is a test
  }

  virtual void end_epoch()
  {
    // do nothing
  }

public:
  double test_all(const Network & nn)
  {
    size_t success_count = 0, total_count = 0;

    VectorBatch outputs;
    outputs.resizeLike(_outputs_batch);
    for(MatrixSize start_pos = 0; ; ) {
      auto res = create_batch(start_pos);
      if(res <= 0) {
        break;
      }
      start_pos += res;

      nn.calculate(_inputs_batch, outputs);
      for(MatrixSize jj = 0; jj < yann::get_batch_size(outputs); ++jj) {
        const auto & actual = get_batch(outputs, jj);
        const auto & expected = get_batch(_outputs_batch, jj);
        MatrixSize aa = 0, ee = 0;
        actual.array().maxCoeff(&aa);
        expected.array().maxCoeff(&ee);
        if(aa == ee) {
          ++success_count;
        }
        ++total_count;
      }
    }

    return total_count > 0 ? success_count / (double)total_count : 0.0;
  }

private:
  MatrixSize create_batch(const MatrixSize & start_pos)
  {
    YANN_CHECK_GE(start_pos, 0);

    _inputs_batch.setZero();
    _outputs_batch.setZero();
    for(MatrixSize ii = 0, pos = start_pos; ii < _batch_size; ++ii, ++pos) {
      if(pos >= (MatrixSize)_tests.size()) {
        return 0;
      }
      const auto & cur =  _tests[pos];
      _inputs_batch.insert(ii, cur.first) = 1.0; // RowMajor
      for(const auto & val : cur.second) {
        _outputs_batch(ii, val) = 1.0; // RowMajor
      }
    }
    return _batch_size;
  }

  static TestsVector get_tests(const Text & text, const MatrixSize & window_size)
  {
    TestsVector tests;
    for(MatrixSize ii = 0, size = text.sentences().size(); ii < size; ++ii) {
      const auto & sentence = text.sentences()[ii];
      // sentence[jj] is the "current" word
      for(MatrixSize jj = 0, sentence_size = sentence.size(); jj < sentence_size; ++jj) {
        // sentence[kk] is the "bag" word
        BagOfWords bow;
        for(MatrixSize kk = jj - window_size; kk <= jj + window_size; ++kk) {
          // check if we are in-range and that we are not pointing to the word itself
          if(kk < 0 || kk >= sentence_size || sentence[jj] == sentence[kk]) {
            continue;
          }
          bow.insert(sentence[kk]);
        }
        tests.push_back(make_pair(sentence[jj], bow));
      }
    }
    return tests;
  }

private:
  const Text & _text;
  const MatrixSize _window_size;
  const MatrixSize _batch_size;

  TestsVector _tests;
  size_t      _cur_pos;

  SparseVectorBatch _inputs_batch;
  VectorBatch _outputs_batch;
}; // class DataSource_CBOW

}; // namespace yann::word2vec

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Word2Vec::TrainingParams implementation
//
yann::word2vec::Word2Vec::TrainingParams::TrainingParams():
    _window_size(5),
    _dimensions(100),
    _updater(make_unique<Updater_GradientDescent>(0.9)),
    _training_sampling_rate(0.0),
    _training_batch_size(100),
    _epochs(10),
    _epochs_callback(nullptr),
    _batch_callback(nullptr)
{
}

////////////////////////////////////////////////////////////////////////////////////////////////
//
// Word2Vec implementation
//

// the format is (d:<dictionary>,v:<vectors>)
ostream& std::operator<<(ostream & os, const Word2Vec & w2v)
{
  os << "(d:" << w2v._dictionary << ",v:" << w2v._vectors << ")";
  return os;
}

// the format is (d:<dictionary>,v:<vectors>)
istream& std::operator>>(istream & is, Word2Vec & w2v)
{
  char ch;
  if(is >> ch && ch != '(') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  if(is >> ch && ch != 'd') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  if(is >> ch && ch != ':') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  is >> w2v._dictionary;
  if(is.fail()) {
    return is;
  }
  if(is >> ch && ch != ',') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  if(is >> ch && ch != 'v') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  if(is >> ch && ch != ':') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }
  is >> w2v._vectors;
  if(is.fail()) {
    return is;
  }
  if(is >> ch && ch != ')') {
    is.putback(ch);
    is.setstate(std::ios_base::failbit);
    return is;
  }

  return is;
}

// cos alpha = A * B / (|A| * |B|)
Value yann::word2vec::Word2Vec::cosine(const RefConstVector & v1, const RefConstVector & v2)
{
  return (v1.array() * v2.array()).sum() / (v1.norm() * v2.norm());
}

yann::word2vec::Word2Vec::Word2Vec()
{
}

yann::word2vec::Word2Vec::~Word2Vec()
{
}

bool yann::word2vec::Word2Vec::is_equal(const Word2Vec& other, double tolerance) const
{
  return _dictionary == other._dictionary && _vectors.isApprox(other._vectors, tolerance);
}

optional<MatrixSize> yann::word2vec::Word2Vec::find_word(const std::string & word) const
{
  return _dictionary.find_word(word);
}
optional<const std::string &> yann::word2vec::Word2Vec::find_word(const MatrixSize & word_num) const
{
  return _dictionary.find_word(word_num);
}

optional<RefConstVector> yann::word2vec::Word2Vec::find_vector(const MatrixSize & word_num) const
{
  YANN_CHECK_LT(word_num, get_vectors_size());
  RefConstVector vv = _vectors.row(word_num);
  return vv;
}

optional<RefConstVector> yann::word2vec::Word2Vec::find_vector(const std::string & word) const
{
  auto word_num = find_word(word);
  if(!word_num) {
    return boost::none;
  }
  return find_vector(*word_num);
}

Value yann::word2vec::Word2Vec::distance(const MatrixSize & word_num1, const MatrixSize & word_num2) const
{
  auto vv1 = find_vector(word_num1);
  auto vv2 = find_vector(word_num2);
  return (vv1 && vv2) ? cosine(*vv1, *vv2) : 0;
}

Value yann::word2vec::Word2Vec::distance(const std::string word1, const std::string & word2) const
{
  auto vv1 = find_vector(word1);
  auto vv2 = find_vector(word2);
  return (vv1 && vv2) ? cosine(*vv1, *vv2) : 0;
}

vector<pair<const string &, Value>> yann::word2vec::Word2Vec::find_closest(const string & word, const size_t & num) const
{
  YANN_CHECK_GT(num, 0);

  auto word_num = find_word(word);
  if(!word_num) {
    return vector<pair<const string &, Value>>(); // not found
  }
  auto vv = find_vector(*word_num);
  YANN_CHECK(vv);

  // compare by the vector distance
  auto cmp = [](const auto & aa, const auto & bb) { return aa.second < bb.second; };
  set<pair<MatrixSize, Value>, decltype(cmp)> ordered(cmp);
  for(MatrixSize ii = 0; ii < _vectors.rows(); ++ii) {
    if(ii == *word_num) continue; // ignore itself

    auto distance = cosine(*vv, _vectors.row(ii));
    ordered.insert(make_pair(ii, distance));
    if(ordered.size() > num) {
      ordered.erase(ordered.begin());
    }
  }

  vector<pair<const string &, Value>> res;
  res.reserve(ordered.size());
  for(auto it = ordered.rbegin(); it != ordered.rend(); ++it) {
    // can't use make_pair() because of "const string &"
    res.push_back(pair<const string &, Value>(_dictionary.get_word(it->first), it->second));
  }
  return res;
}

vector<pair<const string &, Value>> yann::word2vec::Word2Vec::find_farthest(const string & word, const size_t & num) const
{
  YANN_CHECK_GT(num, 0);

  auto word_num = find_word(word);
  if(!word_num) {
    return vector<pair<const string &, Value>>(); // not found
  }
  auto vv = find_vector(*word_num);
  YANN_CHECK(vv);

  // compare by the vector distance
  auto cmp = [](const auto & aa, const auto & bb) { return aa.second > bb.second; };
  set<pair<MatrixSize, Value>, decltype(cmp)> ordered(cmp);
  for(MatrixSize ii = 0; ii < _vectors.rows(); ++ii) {
    if(ii == *word_num) continue; // ignore itself

    auto distance = cosine(*vv, _vectors.row(ii));
    ordered.insert(make_pair(ii, distance));
    if(ordered.size() > num) {
      ordered.erase(ordered.begin());
    }
  }

  vector<pair<const string &, Value>> res;
  res.reserve(ordered.size());
  for(auto it = ordered.rbegin(); it != ordered.rend(); ++it) {
    // can't use make_pair() because of "const string &"
    res.push_back(pair<const string &, Value>(_dictionary.get_word(it->first), it->second));
  }
  return res;
}

void yann::word2vec::Word2Vec::save(const string & filename) const
{
  ofstream ofs(filename, ofstream::out | ofstream::trunc);
  if(!ofs || ofs.fail()) {
    throw runtime_error("can't open file " + filename);
  }
  ofs << (*this);
  if(!ofs || ofs.fail()) {
    throw runtime_error("can't write to file " + filename);
  }
  ofs.close();
}

void yann::word2vec::Word2Vec::load(const std::string & filename)
{
  ifstream ifs(filename, ifstream::in);
  if(!ifs || ifs.fail()) {
    throw runtime_error("can't open file " + filename);
  }
  ifs >> (*this);
  if(!ifs || ifs.fail()) {
    throw runtime_error("can't read file " + filename);
  }
  ifs.close();
}

template<typename DataSourceType>
std::unique_ptr<Word2Vec> yann::word2vec::Word2Vec::train(
    const Text & text,
    const TrainingParams & params,
    DataSourceType & data_source)
{
  auto w2v = make_unique<Word2Vec>();
  YANN_CHECK(w2v);

  // create network
  auto nn = make_unique<Network>();
  YANN_CHECK(nn);
  nn->set_cost_function(make_unique<CrossEntropyCost>());

  auto fc_layer1 = make_unique<FullyConnectedLayer>(
      text.get_dictionary_size(), params._dimensions);
  fc_layer1->set_fixed_bias(0.0);
  fc_layer1->set_activation_function(make_unique<IdentityFunction>());
  if(0 < params._training_sampling_rate && params._training_sampling_rate < 1.0) {
    fc_layer1->set_sampling_rate(params._training_sampling_rate);
  }
  nn->append_layer(std::move(fc_layer1));

  auto fc_layer2 = make_unique<FullyConnectedLayer>(
      params._dimensions, text.get_dictionary_size());
  fc_layer2->set_fixed_bias(0.0);
  fc_layer2->set_activation_function(make_unique<FastSigmoidFunction>());
  if(0 < params._training_sampling_rate && params._training_sampling_rate < 1.0) {
    fc_layer2->set_sampling_rate(params._training_sampling_rate);
  }
  nn->append_layer(std::move(fc_layer2));
  nn->init(Layer::InitMode_Random, params._training_init_context);

  // train the network
  Trainer trainer(params._updater);
  trainer.set_batch_progress_callback(params._batch_callback);
  trainer.set_epochs_progress_callback(params._epochs_callback);
  trainer.train(*nn, data_source, params._training_batch_size,  params._epochs);

  // extract the weights
  auto fc_layer = dynamic_cast<const FullyConnectedLayer*>(nn->get_layer(0));
  YANN_CHECK(fc_layer);
  w2v->_vectors = fc_layer->get_weights();
  w2v->_dictionary = text.dictionary();

  // done
  return w2v;
}

unique_ptr<Word2Vec> yann::word2vec::Word2Vec::train_skip_gram(const Text & text, const TrainingParams & params)
{
  DataSource_SkipGram data_source(
        text,
        params._window_size,
        params._training_batch_size);
  return train(text, params, data_source);
}

std::unique_ptr<Word2Vec> yann::word2vec::Word2Vec::train_cbow(const Text & text, const TrainingParams & params)
{
  DataSource_CBOW data_source(
        text,
        params._window_size,
        params._training_batch_size);
  return train(text, params, data_source);
}


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/21/2020
Allrights reserved by David Ge

base class for time advancement estimation module
********************************************************************/
#include "TimeTssBase.h"
#include "../FileUtil/fileutil.h"
#include "../ProcessMonitor/ProcessMonitor.h"
#include "../ProcessMonitor/workProcess.h"

#include <thread>

#include <boost/multiprecision/cpp_dec_float.hpp>

bool voidReport(){ return false; }
TimeTssBase::TimeTssBase()
{
	GehP = GhhP = GeeP = GheP = NULL;
	space = NULL;
	H = E = curlH = curlE = NULL; // workH = workE = NULL;
	currentTime = 0.0;
	tIndex = 0;
	reporter = NULL;
	operationCanceled = voidReport;
}

TimeTssBase::~TimeTssBase()
{
	cleanup();
}
void TimeTssBase::cleanup()
{
	//
	if (GehP != NULL)
	{
		free(GehP); GehP = NULL;
	}
	if (GhhP != NULL)
	{
		free(GhhP); GhhP = NULL;
	}
	if (GeeP != NULL)
	{
		free(GeeP); GeeP = NULL;
	}
	if (GheP != NULL)
	{
		free(GheP); GheP = NULL;
	}
	//
	if (H != NULL)
	{
		FreeMemory(H); H = NULL;
	}
	if (E != NULL)
	{
		FreeMemory(E); E = NULL;
	}
	if (curlH != NULL)
	{
		FreeMemory(curlH); curlH = NULL;
	}
	if (curlE != NULL)
	{
		FreeMemory(curlE); curlE = NULL;
	}
}
void TimeTssBase::onSettingSimParams()
{
	cellCount = (pams->nx + 1)*(pams->ny + 1)*(pams->nz + 1);
}
int TimeTssBase::initializeTimeModule(Space *s, SimStruct *pams0, FieldSourceTss *src)
{
	using namespace boost::multiprecision;
	int ret = ERR_OK;
	puts("\r\nCalculating update coefficients...");
	pams = pams0;
	onSettingSimParams();
	fieldMemorySize = cellCount*sizeof(Point3Dstruct);
	if (src != NULL)
	{
		if (!src->Initialized())
		{
			return ERR_SRC_NOT_INIT;
		}
	}
	space = s;
	source = src;
	cpp_dec_float_100 *G_ehP = (cpp_dec_float_100 *)malloc((pams->kmax + 1)*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *G_hhP = (cpp_dec_float_100 *)malloc((pams->kmax + 1)*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *G_eeP = (cpp_dec_float_100 *)malloc((pams->kmax + 1)*sizeof(cpp_dec_float_100));
	cpp_dec_float_100 *G_heP = (cpp_dec_float_100 *)malloc((pams->kmax + 1)*sizeof(cpp_dec_float_100));
	//
	GehP = (double*)malloc((pams->kmax + 1)*sizeof(double));
	GhhP = (double*)malloc((pams->kmax + 1)*sizeof(double));
	GeeP = (double*)malloc((pams->kmax + 1)*sizeof(double));
	GheP = (double*)malloc((pams->kmax + 1)*sizeof(double));
	if (G_ehP == NULL || G_hhP == NULL || G_eeP == NULL || G_heP == NULL || GehP == NULL || GhhP == NULL || GeeP == NULL || GheP == NULL)
	{
		ret = ERR_OUTOFMEMORY;
	}
	else
	{
		printf("\r\nUpdate coefficients allocated. Array size:%u ", pams->kmax + 1);
		for (unsigned int k = 0; k <= pams->kmax; k++)
		{
			G_ehP[k] = G_hhP[k] = G_eeP[k] = G_heP[k] = 0.0;
			GehP[k]  = GhhP[k]  = GeeP[k]  = GheP[k]  = 0.0;
		}
	}
	if (ret == ERR_OK)
	{
		/*
		p[h=0..2k_max+2,i=0..2k_max+2]=p_i^{h}
		q[h=0..2k_max+2,i=0..2k_max+2]=q_i^{h}
		*/
		size_t pMax = 2 * pams->kmax + 3; // 0 ... 2kmax+2
		size_t pMax2 = pMax * pMax;
		//p_i^{h}
#define Idx_ih(i, h) ((h)*pMax+(i))
		cpp_dec_float_100 *p = (cpp_dec_float_100 *)malloc(pMax2*sizeof(cpp_dec_float_100));
		cpp_dec_float_100 *q = (cpp_dec_float_100 *)malloc(pMax2*sizeof(cpp_dec_float_100));
		cpp_dec_float_100 *dtR = (cpp_dec_float_100 *)malloc(pMax*sizeof(cpp_dec_float_100));
		cpp_dec_float_100 *dsR = (cpp_dec_float_100 *)malloc(pMax*sizeof(cpp_dec_float_100));
		if (p == NULL || q == NULL || dtR == NULL || dsR == NULL)
		{
			ret = ERR_OUTOFMEMORY;
		}
		if (ret == ERR_OK)
		{
			cpp_dec_float_100 gde = pams->sie;
			cpp_dec_float_100 geps = pams->eps;
			cpp_dec_float_100 gmu = pams->mu;
			cpp_dec_float_100 gdm = pams->sim;
			cpp_dec_float_100 g1 = 1.0;
			cpp_dec_float_100 f;
			//values for k=0
			cpp_dec_float_100 dt2 = 1.0;
			cpp_dec_float_100 XN = 0.0;
			cpp_dec_float_100 X = 1.0;
			cpp_dec_float_100 ds2 = 1.0;
			for (unsigned int k = 0; k < pMax2; k++)
			{
				p[k] = 0.0; q[k] = 0.0;
			}
			//p_0^0=1, q_0^0 = 1
			p[0] = 1.0; q[0] = 1.0;
			for (unsigned int k = 0; k < pMax; k++)
			{
				f = dt2 / X;
				dtR[k] = f; //dt^k/k!
				dsR[k] = ds2; //ds^k
				//prepare for k+1
				XN = XN + 1.0;
				X = X * XN;
				dt2 = dt2 * pams->dt;
				ds2 = ds2 * pams->ds;
			}
			/*
			p_0^{1} =1/ε,p_1^{1} =-σ/ε
			q_0^{1} =-1/μ,q_1^{1} =-σ_m/μ
			*/
			p[Idx_ih(0, 1)] = g1 / geps; p[Idx_ih(1, 1)] = -gde / geps;
			q[Idx_ih(0, 1)] = -g1 / gmu; q[Idx_ih(1, 1)] = -gdm / gmu;
			/*
			p_0^{2k}  =q_0^{1}  p_0^{2k-1}
			p_2k^{2k} =p_(2k-1)^{2k-1}  p_1^{1}
			q_0^{2k}  =p_0^{1}  q_0^{2k-1}
			q_2k^{2k} =q_(2k-1)^{2k-1}  q_1^{1}

			p_0^{2k+1}      =p_0^{1}  p_0^{2k}
			p_(2k+1)^{2k+1} =p_2k^{2k}  p_1^{1}
			q_0^{2k+1}      =q_0^{1}  q_0^{2k}
			q_(2k+1)^{2k+1} =q_2k^{2k}  q_1^{1}
			*/
			//from (0,1), (1,1) get
			//(0,2)...(0,2kmax+2); (2,2) ... (2kmax+2,2kmax+2)
			//(0,3)...(0,2kmax+1); (3,3) ... (2kmax+1,2kmax+1)
			for (unsigned int k = 1; k <= pams->kmax + 1; k++)
			{
				p[Idx_ih(0, 2 * k)] = q[Idx_ih(0, 1)] * p[Idx_ih(0, 2 * k - 1)];
				q[Idx_ih(0, 2 * k)] = p[Idx_ih(0, 1)] * q[Idx_ih(0, 2 * k - 1)];
				p[Idx_ih(2 * k, 2 * k)] = p[Idx_ih(2 * k - 1, 2 * k - 1)] * p[Idx_ih(1, 1)];
				q[Idx_ih(2 * k, 2 * k)] = q[Idx_ih(2 * k - 1, 2 * k - 1)] * q[Idx_ih(1, 1)];
				if (k <= pams->kmax)
				{
					p[Idx_ih(0, 2 * k + 1)] = p[Idx_ih(0, 1)] * p[Idx_ih(0, 2 * k)];
					q[Idx_ih(0, 2 * k + 1)] = q[Idx_ih(0, 1)] * q[Idx_ih(0, 2 * k)];
					p[Idx_ih(2 * k + 1, 2 * k + 1)] = p[Idx_ih(2 * k, 2 * k)] * p[Idx_ih(1, 1)];
					q[Idx_ih(2 * k + 1, 2 * k + 1)] = q[Idx_ih(2 * k, 2 * k)] * q[Idx_ih(1, 1)];
				}
			}
			/*
			p_(2i+1)^{2k} =q_1^{1}  p_2i^{2k-1} +p_0^{1}  p_(2i+1)^{2k-1} ;    i=0,1,…,k-1;k_max+1≥k>0
			q_(2i+1)^{2k} =p_1^{1}  q_2i^{2k-1} +q_0^{1}  q_(2i+1)^{2k-1} ;    i=0,1,…,k-1;k_max+1≥k>0
			p_(2i+2)^{2k} =p_1^{1}  p_(2i+1)^{2k-1} +q_0^{1}  p_(2i+2)^{2k-1} ;i=0,1,…,k-2;k_max+1≥k>1
			q_(2i+2)^{2k} =q_1^{1}  q_(2i+1)^{2k-1} +p_0^{1}  q_(2i+2)^{2k-1} ;i=0,1,…,k-2;k_max+1≥k>1
			*/
			for (unsigned int k = 1; k <= pams->kmax + 1; k++)
			{
				for (unsigned int i = 0; i < k; i++)
				{
					p[Idx_ih(2 * i + 1, 2 * k)] = q[Idx_ih(1, 1)] * p[Idx_ih(2 * i, 2 * k - 1)] + p[Idx_ih(0, 1)] * p[Idx_ih(2 * i + 1, 2 * k - 1)];
					q[Idx_ih(2 * i + 1, 2 * k)] = p[Idx_ih(1, 1)] * q[Idx_ih(2 * i, 2 * k - 1)] + q[Idx_ih(0, 1)] * q[Idx_ih(2 * i + 1, 2 * k - 1)];
					if (k>1 && i < k - 1)
					{
						p[Idx_ih(2 * i + 2, 2 * k)] = p[Idx_ih(1, 1)] * p[Idx_ih(2 * i + 1, 2 * k - 1)] + q[Idx_ih(0, 1)] * p[Idx_ih(2 * i + 2, 2 * k - 1)];
						q[Idx_ih(2 * i + 2, 2 * k)] = q[Idx_ih(1, 1)] * q[Idx_ih(2 * i + 1, 2 * k - 1)] + p[Idx_ih(0, 1)] * q[Idx_ih(2 * i + 2, 2 * k - 1)];
					}
				}
				if (k <= pams->kmax)
				{
					/*
					p_(2i-1)^{2k+1} =p_1^{1}  p_(2i-2)^{2k} +q_0^{1}  p_(2i-1)^{2k}
					p_2i    ^{2k+1} =q_1^{1}  p_(2i-1)^{2k} +p_0^{1}  p_2i^{2k}
					q_(2i-1)^{2k+1} =q_1^{1}  q_(2i-2)^{2k} +p_0^{1}  q_(2i-1)^{2k}
					q_2i    ^{2k+1} =p_1^{1}  q_(2i-1)^{2k} +q_0^{1}  q_2i^{2k}
					i=1,2,…,k;k≥1
					*/
					for (unsigned int i = 1; i <= k; i++)
					{
						p[Idx_ih(2 * i - 1, 2 * k + 1)] = p[Idx_ih(1, 1)] * p[Idx_ih(2 * i - 2, 2 * k)] + q[Idx_ih(0, 1)] * p[Idx_ih(2 * i - 1, 2 * k)];
						q[Idx_ih(2 * i - 1, 2 * k + 1)] = q[Idx_ih(1, 1)] * q[Idx_ih(2 * i - 2, 2 * k)] + p[Idx_ih(0, 1)] * q[Idx_ih(2 * i - 1, 2 * k)];
						p[Idx_ih(2 * i, 2 * k + 1)] = q[Idx_ih(1, 1)] * p[Idx_ih(2 * i - 1, 2 * k)] + p[Idx_ih(0, 1)] * p[Idx_ih(2 * i, 2 * k)];
						q[Idx_ih(2 * i, 2 * k + 1)] = p[Idx_ih(1, 1)] * q[Idx_ih(2 * i - 1, 2 * k)] + q[Idx_ih(0, 1)] * q[Idx_ih(2 * i, 2 * k)];
					}
				}
			}
			////calculate G_e^h, G_h^h, G_h^e, G_e^e///////////////////////////////
			cpp_dec_float_100 a, b, c;
			/*
			i=0,1,...,kmax
			G_e^h (i,∆_t)=∑_(k=i)^(k_max)〖C_e^h (k,i,∆_t ) 〗
			G_h^h (i,∆_t)=∑_(k=i)^(k_max)〖C_h^h (k,i,∆_t ) 〗
			G_h^e (i,∆_t)=∑_(k=i)^(k_max)〖C_h^e (k,i,∆_t ) 〗
			G_e^e (i,∆_t)=∑_(k=i)^(k_max)〖C_e^e (k,i,∆_t ) 〗
			//
			sum k=i,i+1,...,kmax
			start from (∆_t^(2i+1))/(2i+1)! go up
			-------------------------------------
			C_e^h (k,i,∆_t )= ((∆_t^(2k+1))/(2k+1)! q_0^{2k+1}                                        ,i=k
							  (∆_t^2k)/(2k)! q_(2(k-i)-1)^{2k} + (∆_t^(2k+1))/(2k+1)! q_2(k-i)^{2k+1} ,0≤i<k
			//
			C_h^e (k,i,∆_t )= ((∆_t^(2k+1))/(2k+1)! p_0^{2k+1}                                        ,i=k
							  (∆_t^2k)/(2k)! p_(2(k-i)-1)^{2k} + (∆_t^(2k+1))/(2k+1)! p_2(k-i)^{2k+1} ,0≤i<k
			====================
			C_h^h (k,i,∆_t )=(∆_t^2k)/(2k)! q_2(k-i)^{2k} +(∆_t^(2k+1))/(2k+1)! q_(2(k-i)+1)^{2k+1}
			C_e^e (k,i,∆_t )=(∆_t^2k)/(2k)! p_2(k-i)^{2k} +(∆_t^(2k+1))/(2k+1)! p_(2(k-i)+1)^{2k+1}
			--------------------------------
			*/
			for (unsigned int i = 0; i <= pams->kmax; i++)
			{
				G_ehP[i] = 0.0; G_hhP[i] = 0.0; G_eeP[i] = 0.0; G_heP[i] = 0.0;
				for (unsigned int k = i; k <= pams->kmax; k++)
				{
					a = q[Idx_ih(2 * (k - i), 2 * k)];
					b = p[Idx_ih(2 * (k - i), 2 * k)];
					G_hhP[i] += dtR[2*k] * a;
					G_eeP[i] += dtR[2*k] * b;
					//
					a = q[Idx_ih(2 * (k - i)+1, 2 * k+1)];
					b = p[Idx_ih(2 * (k - i)+1, 2 * k+1)];
					G_hhP[i] += dtR[2 * k+1] * a;
					G_eeP[i] += dtR[2 * k+1] * b;
					//
					if (k == i)
					{
						a = q[Idx_ih(0, 2 * k + 1)];
						b = p[Idx_ih(0, 2 * k + 1)];
						G_ehP[i] += dtR[2 * k + 1] * a;
						G_heP[i] += dtR[2 * k + 1] * b;
					}
					else
					{
						a = q[Idx_ih(2 * (k - i) - 1, 2 * k)];
						b = p[Idx_ih(2 * (k - i) - 1, 2 * k)];
						G_ehP[i] += dtR[2 * k] * a;
						G_heP[i] += dtR[2 * k] * b;
						//
						a = q[Idx_ih(2 * (k - i), 2 * k + 1)];
						b = p[Idx_ih(2 * (k - i), 2 * k + 1)];
						G_ehP[i] += dtR[2 * k+1] * a;
						G_heP[i] += dtR[2 * k+1] * b;
					}
				}
			}
			/*
			G_h^h (i,∆_t ) ∇ ̅^{2i} ×H+G_e^h (i,∆_t ) ∇ ̅^{2i+1} ×E
			G_e^e (i,∆_t ) ∇ ̅^{2i} ×E+G_h^e (i,∆_t ) ∇ ̅^{2i+1} ×H
			*/
			for (unsigned int i = 0; i <= pams->kmax; i++)
			{
				G_ehP[i] = G_ehP[i] / dsR[2 * i + 1];
				G_heP[i] = G_heP[i] / dsR[2 * i + 1];
				G_hhP[i] = G_hhP[i] / dsR[2 * i];
				G_eeP[i] = G_eeP[i] / dsR[2 * i];
			}
			for (unsigned int i = 0; i <= pams->kmax; i++)
			{
				GehP[i] = G_ehP[i].convert_to<double>();
				GheP[i] = G_heP[i].convert_to<double>();
				GhhP[i] = G_hhP[i].convert_to<double>();
				GeeP[i] = G_eeP[i].convert_to<double>();
			}
			free(G_ehP); free(G_heP); free(G_hhP); free(G_eeP);
			if (source != NULL)
			{
				if (ret == ERR_OK)
				{
					unsigned int emMax = source->GetemMax(); //2 * kmax + 3;
					size_t emMax2 = emMax*emMax;
					cpp_dec_float_100 *e = (cpp_dec_float_100 *)malloc(emMax2*sizeof(cpp_dec_float_100));
					cpp_dec_float_100 *m = (cpp_dec_float_100 *)malloc(emMax2*sizeof(cpp_dec_float_100));
					size_t t,t1;
					for (t = 0; t < emMax2; t++)
					{
						e[t] = 0.0; m[t] = 0.0;
					}
					/*
					Eq. (16), (17)
					e_0^{0} =-1/ε
					m_0^{0} =-1/μ
					*/
					e[I_ih(0, 0)] = -1.0 / pams->eps;
					m[I_ih(0, 0)] = -1.0 / pams->mu;
					/*
					Eq. (18)
					e_2(k-i)^{2i} =p_(2(k-i))^{2k}  e_0^{0} ,
					m_2(k-i)^{2i} =q_(2(k-i))^{2k}  m_0^{0} ,
					i=0,1,2,…,k, k>0
					*/
					for (unsigned int k = 1; k <= pams->kmax + 1; k++)
					{
						for (unsigned int i = 0; i <= k; i++)
						{
							//0...2kmax+2
							t = I_ih(2 * (k - i), 2 * i); //[2kmax+2, (2kmax), ..., 0], {0, 2, ..., 2kmax+2}
							e[t] = p[Idx_ih(2 * (k - i), 2 * k)] * e[I_ih(0, 0)];
							m[t] = q[Idx_ih(2 * (k - i), 2 * k)] * m[I_ih(0, 0)];
						}
					}
					/*
					Eq. (19)
					m_(2(k-i)+1)^{2i-1} =p_(2(k-i)+1)^{2k}  m_0^{0} ,
					e_(2(k-i)+1)^{2i-1} =q_(2(k-i)+1)^{2k}  e_0^{0} ,
					i=1,2,…,k,k>0
					*/
					for (unsigned int k = 1; k <= pams->kmax + 1; k++)
					{
						for (unsigned int i = 1; i <= k; i++)
						{
							t = I_ih(2 * (k - i) + 1, 2 * i - 1);//[2kmax+1,2kmax-1,...,1], {1,3,...,2kmax+1}
							m[t] = p[Idx_ih(2 * (k - i) + 1, 2 * k)] * m[I_ih(0, 0)];
							e[t] = q[Idx_ih(2 * (k - i) + 1, 2 * k)] * e[I_ih(0, 0)];
						}
					}
					/*
					Eq. (26)
					e_(2(k-i)-1)^{2i} =p_(2(k-i)-1)^{2k-1}  e_0^{0} ,
					m_(2(k-i)-1)^{2i} =q_(2(k-i)-1)^{2k-1}  m_0^{0} ,
					i=0,1,2,…,k-1, k>0
					Eq. (27)
					m_(2(k-i-1))^{2i+1} =p_(2(k-i-1))^{2k-1}  m_0^{0} ,
					e_(2(k-i-1))^{2i+1} =q_(2(k-i-1))^{2k-1}  e_0^{0} ,
					i=0,1,2,…,k-1, k>0
					*/
					for (unsigned int k = 1; k <= pams->kmax + 1; k++)
					{
						for (unsigned int i = 0; i < k; i++)
						{
							t = I_ih(2 * (k - i) - 1, 2 * i);//[2kmax+1,2kmax-1,...,1],{0,2,...,2kmax+2}
							t1 = Idx_ih(2 * (k - i) - 1, 2 * k - 1);
							e[t] = p[t1] * e[I_ih(0, 0)];
							m[t] = q[t1] * m[I_ih(0, 0)];
							t = I_ih(2 * (k - i - 1), 2 * i + 1);//[2kmax,2kmax-2,...,0],{1,2,...,2kmax+1}
							t1 = Idx_ih(2 * (k - i - 1), 2 * k - 1);
							m[t] = p[t1] * m[I_ih(0, 0)];
							e[t] = q[t1] * e[I_ih(0, 0)];
						}
					}
					ret = source->onInitialized(e, m);
					free(e); free(m);
				}
			}
		}
		if (p != NULL) { free(p); p = NULL; }
		if (q != NULL) { free(q); q = NULL; }
	}
	if (ret == ERR_OK)
	{
		startThreads();
		ret = pml.initialize(pams);
	}
	if (ret == ERR_OK)
	{
		puts("\r\nFinished calculating update coefficients.");
	}
	return ret;
}
int TimeTssBase::initFields(FieldsSetter* f0)
{
	int ret = ERR_OK;
	puts("\r\nAllocating field memories...");
	//
	//all field memories used
	if (H != NULL)
	{
		FreeMemory(H); H = NULL;
	}
	if (E != NULL)
	{
		FreeMemory(E); E = NULL;
	}
	if (curlH != NULL)
	{
		FreeMemory(curlH); curlH = NULL;
	}
	if (curlE != NULL)
	{
		FreeMemory(curlE); curlE = NULL;
	}
	H = (Point3Dstruct *)AllocateMemory(fieldMemorySize);
	if (H == NULL)
	{
		ret = MEMRETLAST;
		if (ret == ERR_OK)
		{
			ret = ERR_OUTOFMEMORY;
		}
	}
	if (ret == ERR_OK)
	{
		E = (Point3Dstruct *)AllocateMemory(fieldMemorySize);
		if (E == NULL)
		{
			ret = MEMRETLAST;
			if (ret == ERR_OK)
			{
				ret = ERR_OUTOFMEMORY;
			}
		}
	}
	if (ret == ERR_OK)
	{
		curlH = (Point3Dstruct *)AllocateMemory(fieldMemorySize);
		if (curlH == NULL)
		{
			ret = MEMRETLAST;
			if (ret == ERR_OK)
			{
				ret = ERR_OUTOFMEMORY;
			}
		}
	}
	if (ret == ERR_OK)
	{
		curlE = (Point3Dstruct *)AllocateMemory(fieldMemorySize);
		if (curlE == NULL)
		{
			ret = MEMRETLAST;
			if (ret == ERR_OK)
			{
				ret = ERR_OUTOFMEMORY;
			}
		}
	}
	if (ret == ERR_OK)
	{
		size_t w = 0;
		double x = pams->xmin, y, z;
		puts("\r\nInitializing fields...");
		for (unsigned int i = 0; i <= pams->nx; i++)
		{
			y = pams->ymin;
			for (unsigned int j = 0; j <= pams->ny; j++)
			{
				z = pams->zmin;
				for (unsigned int k = 0; k <= pams->nz; k++)
				{
					if (f0 != NULL)
					{
						f0->SetFields(x, y, z, &(E[w]), &(H[w]));
					}
					else
					{
						E[w].x = 0.0;
						E[w].y = 0.0;
						E[w].z = 0.0;
						H[w].x = 0.0;
						H[w].y = 0.0;
						H[w].z = 0.0;
					}
					z += pams->ds;
					w++;
				}
				y += pams->ds;
			}
			x += pams->ds;
		}
	}
	return ret;
}

int TimeTssBase::writeCoefficientsToFile(char *file, unsigned int maxTimeSteps, double courantFactor, unsigned int dataFileSteps)
{
	int ret = ERR_OK;
	FILE *retFileHandle = 0;
	if (file != NULL)
	{
		ret = deleteFile(file);
		if (ret == ERR_OK)
		{
			ret = openTextfileWrite(file, &retFileHandle);
		}
	}
	else
	{
		ret = ERR_FILENAME_MISSING;
	}
	if (retFileHandle != 0)
	{
		unsigned int size = 1024;
		char buff[1024];
		sprintf_1(buff, size, "kmax=%u, time estimation order = 2(kmax+1)=%u \nds=%g, dt=%g, courant factor=%g, total time=%g, data file interval=%g\n", pams->kmax, 2 * (pams->kmax + 1), pams->ds, pams->dt, courantFactor, pams->dt*maxTimeSteps, pams->dt*dataFileSteps);
		writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		sprintf_1(buff, size, "material parameters: eps=%g, mu=%g, sigma_e=%g, sigma_h=%g\r\n", pams->eps, pams->mu, pams->sie, pams->sim);
		writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		////////////H/////////////////////////////////////////////
		sprintf_1(buff, size, "H=C.F.dJm+C.G.dJe+");
		writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		//Curl X H -> H
		for (unsigned int k = 0; k <= pams->kmax; k++)
		{
			if (k == 0)
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, "[%g]C2xH+", GhhP[k]);
				}
				else
				{
					sprintf_1(buff, size, "[%g,", GhhP[k]);
				}
			}
			else
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, " %g]C2xH+", GhhP[k]);
				}
				else
				{
					sprintf_1(buff, size, " %g,", GhhP[k]);
				}
			}
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		}
		//CE->H
		for (unsigned int k = 0; k <= pams->kmax; k++)
		{
			if (k == 0)
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, "[%g]C1xE\r\n", GehP[k]);
				}
				else
				{
					sprintf_1(buff, size, "[%g,", GehP[k]);
				}
			}
			else
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, " %g]C1xE\n", GehP[k]);
				}
				else
				{
					sprintf_1(buff, size, " %g,", GehP[k]);
				}
			}
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		}
		///////////////E///////////
		sprintf_1(buff, size, "\nE=C.U.dJe+C.W.dJm+");
		writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		//Curl X E -> E
		for (unsigned int k = 0; k <= pams->kmax; k++)
		{
			if (k == 0)
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, "[%g]C2xE+", GeeP[k]);
				}
				else
				{
					sprintf_1(buff, size, "[%g,", GeeP[k]);
				}
			}
			else
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, " %g]C2xE+", GeeP[k]);
				}
				else
				{
					sprintf_1(buff, size, " %g,", GeeP[k]);
				}
			}
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		}
		//CH->E
		for (unsigned int k = 0; k <= pams->kmax; k++)
		{
			if (k == 0)
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, "[%g]C1xH\n", GheP[k]);
				}
				else
				{
					sprintf_1(buff, size, "[%g,", GheP[k]);
				}
			}
			else
			{
				if (k == pams->kmax)
				{
					sprintf_1(buff, size, " %g]C1xH\n", GheP[k]);
				}
				else
				{
					sprintf_1(buff, size, " %g,", GheP[k]);
				}
			}
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
		}
		if (source != NULL)
		{
			double *f = source->GetJmH();
			double *g = source->GetJeH();
			double *u = source->GetJeE();
			double *w = source->GetJmE();
			unsigned int srcDim = source->GetsrcDim();
			unsigned int t = 0;
			//SH=============================================
			sprintf_1(buff, size, "\nF[ %u x %u ]=\n", srcDim, srcDim);
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
			t = 0;
			for (unsigned int i = 0; i < srcDim; i++)
			{
				for (unsigned int j = 0; j < srcDim; j++)
				{
					if (j == srcDim - 1)
					{
						sprintf_1(buff, size, " %g\n", f[t]);
					}
					else
					{
						sprintf_1(buff, size, " %g,\t", f[t]);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					t++;
				}
			}
			sprintf_1(buff, size, " \nG[ %u x %u ]=\n", srcDim, srcDim);
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
			t = 0;
			for (unsigned int i = 0; i < srcDim; i++)
			{
				for (unsigned int j = 0; j < srcDim; j++)
				{
					if (j == srcDim - 1)
					{
						sprintf_1(buff, size, " %g\n", g[t]);
					}
					else
					{
						sprintf_1(buff, size, " %g,\t", g[t]);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					t++;
				}
			}
			//SE=============================================
			sprintf_1(buff, size, "\nU[ %u x %u ]=\n", srcDim, srcDim);
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
			t = 0;
			for (unsigned int i = 0; i < srcDim; i++)
			{
				for (unsigned int j = 0; j < srcDim; j++)
				{
					if (j == srcDim - 1)
					{
						sprintf_1(buff, size, " %g\n", u[t]);
					}
					else
					{
						sprintf_1(buff, size, " %g,\t", u[t]);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					t++;
				}
			}
			sprintf_1(buff, size, "\nW[ %u x %u ]=\n", srcDim, srcDim);
			writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
			t = 0;
			for (unsigned int i = 0; i < srcDim; i++)
			{
				for (unsigned int j = 0; j < srcDim; j++)
				{
					if (j == srcDim - 1)
					{
						sprintf_1(buff, size, " %g\n", w[t]);
					}
					else
					{
						sprintf_1(buff, size, " %g,\t", w[t]);
					}
					writefile(retFileHandle, buff, (unsigned int)strnlen_0(buff, size));
					t++;
				}
			}
		}//source
		if (pml.usePML())
		{
			pml.writeCoefficientsToFile(retFileHandle);
		}
	}
	if (retFileHandle != 0)
	{
		closefile(retFileHandle);
	}
	return ret;
}

int TimeTssBase::saveFieldToFile(char *filename, FIELD_EMTYPE fieldToSave)
{
	int ret = ERR_OK;
	FILE *fp = NULL;
	ret = openfileWrite(filename, &fp);
	if (ret != ERR_OK || fp == NULL)
	{
		if (ret == ERR_OK)
		ret = ERR_FILE_OPEN_WRIT_EACCES;
	}
	else
	{
		size_t sizew;
		if (fieldToSave == Field_E)
		{
			sizew = fwrite(getRawMemoryE(), sizeof(Point3Dstruct), cellCount, fp);
		}
		else
		{
			sizew = fwrite(getRawMemoryH(), sizeof(Point3Dstruct), cellCount, fp);
		}
		fclose(fp);
		if (sizew != cellCount)
		{
			ret = ERR_FILE_WRITE_LESS;
		}
	}
	return ret;
}

int TimeTssBase::loadFieldFromFile(char *filename, FIELD_EMTYPE fieldToLoad)
{
	int ret = ERR_OK;
	size_t fSize = 0;
	Point3Dstruct *f = (Point3Dstruct *)ReadFileIntoMemory(filename, &fSize, &ret);
	if (ret == ERR_OK)
	{
		if (fieldMemorySize != fSize)
		{
			ret = ERR_FILESIZE_MISMATCH;
		}
		else
		{
			Point3Dstruct *field;
			if (fieldToLoad == Field_E)
			{
				field = getRawMemoryE();
			}
			else
			{
				field = getRawMemoryH();
			}
			for (size_t w = 0; w < cellCount; w++)
			{
				field[w].x = f[w].x;
				field[w].y = f[w].y;
				field[w].z = f[w].z;
			}
		}
		FreeMemory(f);
	}
	return ret;
}


//
// $Id: filter.hpp 5313 2013-12-17 18:06:54Z chambm $
//
//
// Original author: Witold Wolski <wewolski@gmail.com>
//
// Copyright : ETH Zurich
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

#ifndef FILTERUTILS_H
#define FILTERUTILS_H
#include <math.h>
#include <algorithm>
#include <vector>
#include <functional>
#include <numeric>
#include <limits>

#include <boost/iterator/reverse_iterator.hpp>
#include <boost/bind.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/cstdint.hpp>

#include "pwiz/utility/findmf/base/filter/utilities/preparedata.hpp"

namespace ralab
{
  namespace base
  {
    namespace filter
    {

      typedef boost::uint32_t uint32_t;
      template <typename TIterator, typename TFilterIterator, typename TOutputIterator>
      void filter_sequence(
          TIterator  dataBeg,   //!<[in] a univariate time series.
          TIterator  dataEnd,
          TFilterIterator filterBeg, //!<[in] a vector of filter coefficients in reverse time order (as for AR or MA coefficients). Lenght of filter must be odd.
          size_t fsize,
          TOutputIterator resBeg, //!<[out] result
          bool circular = false, //!<[in]  If TRUE, wrap the filter around the ends of the series, otherwise assume external values are missing (NA).
          uint32_t sides = 2  //!<[in] currently only sides 2 supported....
          )
      {
        typedef typename std::iterator_traits<TOutputIterator>::value_type TReal;
        if((fsize-1) % 2)
          {
            throw std::logic_error("filter size must be odd");
          }
        if(!circular)
          {
            //result.assign(data.size(), std::numeric_limits<TReal>::quiet_NaN() );

            size_t offset = static_cast<size_t>(fsize/2);
            for(std::size_t i = 0 ; i< offset; ++i, ++resBeg)
              {
                *resBeg = std::numeric_limits<TReal>::quiet_NaN();
              }

            for( ; dataBeg != dataEnd - (fsize -1) ; ++dataBeg, ++resBeg )
              {
                *resBeg = (std::inner_product(dataBeg , dataBeg + fsize, filterBeg ,0. ));
              }
          }
        else
          {

            std::vector<typename std::iterator_traits<TIterator>::value_type> tmp;
            typename std::vector<typename std::iterator_traits<TIterator>::value_type>::iterator it;
            it = utilities::prepareData( dataBeg, dataEnd, fsize , tmp );

            TIterator tbegin = tmp.begin();
            TIterator tend = it;

            for( ; tbegin != tend - (fsize-1 ) ; ++tbegin, ++resBeg )
              {
                *resBeg = std::inner_product(tbegin , tbegin + fsize, filterBeg ,0. );
              }
          }
      }// filter end

      /*! \brief Applies linear convolution (filtering) to a univariate time series

            The convolution filter is  \f$ y[i] = f[1]*x[i+o] + ... + f[p]*x[i+o-(p-1)] \f$
            where o is the offset: see sides for how it is determined.

            \param sides for convolution filters only.
                        If sides=1 the filter coefficients are for past values only;
                        if sides=2 they are centred around lag 0.
                        In this case the length of the filter should be odd,
                        but if it is even, more of the filter is forward in time than backward

            */
      template <typename TContainer>
      void filter(
          const TContainer & data,   //!<[in] a univariate time series.
          const TContainer & filter, //!<[in] a vector of filter coefficients in reverse time order (as for AR or MA coefficients). Lenght of filter must be odd.
          TContainer & result, //!<[out] result
          bool circular = false, //!<[in]  If TRUE, wrap the filter around the ends of the series, otherwise assume external values are missing (NA).
          uint32_t sides = 2  //!<[in] currently only sides 2 supported....
          )
      {
        result.resize(data.size());
        filter_sequence
            (
              data.begin(),
              data.end(),
              filter.begin(),
              filter.size(),
              result.begin(),
              circular,
              sides
              );
      }// filter end
    }//filter
  }//base
}//ralab

#endif