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

// Copyright (c) 2017-2021 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifdef BOOST_GEOMETRY_TEST_DEBUG
#include <iostream>
#endif

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_geographic_pointlike_areal
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_geo_common.hpp"
#include "test_empty_geometry.hpp"

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps
>
void test_distance_point_ring(Strategy_pp const& strategy_pp,
                              Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/ring distance tests" << std::endl;
#endif
    typedef bg::model::ring<Point> ring_type;
    typedef test_distance_of_geometries<Point, ring_type> tester;

    std::string const ring = "POLYGON((10 10,0 20, 15 30, 20 15, 15 10, 10 10))";

    tester::apply("pr1", "POINT(0 10)", ring,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("pr2", "POINT(10 0)", ring,
                  pp_distance<Point>("POINT(10 10)", "POINT(10 0)", strategy_pp),
                  strategy_ps, true, true, false);
    tester::apply("pr3", "POINT(0 20)", ring,
                  0, strategy_ps, true, true, false);
    tester::apply("pr4", "POINT(10 10)", ring,
                  0, strategy_ps, true, true, false);
    tester::apply("pr4", "POINT(10 11)", ring,
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template
<
        typename Point,
        typename Strategy_ps
>
void test_distance_multipoint_ring(Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/ring distance tests" << std::endl;
#endif
    typedef bg::model::ring<Point> ring_type;
    typedef bg::model::multi_point<Point> multi_point_type;
    typedef test_distance_of_geometries<multi_point_type, ring_type> tester;

    std::string const ring = "POLYGON((10 10,0 20, 15 30, 20 15, 15 10, 10 10))";

    tester::apply("pr1", "MULTIPOINT(0 10,10 0,0 0)", ring,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);
}

//===========================================================================

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps
>
void test_distance_point_polygon(Strategy_pp const& strategy_pp,
                                 Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/polygon distance tests" << std::endl;
#endif
    typedef bg::model::polygon<Point> polygon_type;
    typedef test_distance_of_geometries<Point, polygon_type> tester;

    std::string const polygon =
            "POLYGON((10 10,0 20, 15 30, 20 15, 15 10, 10 10))";

    tester::apply("pr1", "POINT(0 10)", polygon,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);
    tester::apply("pr2", "POINT(10 0)", polygon,
                  pp_distance<Point>("POINT(10 10)", "POINT(10 0)", strategy_pp),
                  strategy_ps, true, true, false);
    tester::apply("pr3", "POINT(0 20)", polygon,
                  0, strategy_ps, true, true, false);
    tester::apply("pr4", "POINT(10 10)", polygon,
                  0, strategy_ps, true, true, false);
    tester::apply("pr5", "POINT(10 10.1)", polygon,
                  0, strategy_ps, false, false, false);
}

//===========================================================================

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps
>
void test_distance_multipoint_polygon(Strategy_pp const& strategy_pp,
                                      Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/polygon distance tests" << std::endl;
#endif
    typedef bg::model::polygon<Point> polygon_type;
    typedef bg::model::multi_point<Point> multi_point_type;
    typedef test_distance_of_geometries<multi_point_type, polygon_type> tester;

    std::string const polygon = "POLYGON((10 10,0 20,15 30,20 15,15 10,10 10))";

    tester::apply("mpp1", "MULTIPOINT(0 10,10 0,0 0)", polygon,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);

    tester::apply("mpp2", "MULTIPOINT(0 10,10 0,0 0,20.1 15)", polygon,
                  pp_distance<Point>("POINT(20 15)",
                                     "POINT(20.1 15)", strategy_pp),
                  strategy_ps, true, true, false);

    tester::apply("mpp3", "MULTIPOINT(0 10,10 0,0 0,20.1 15,10 10)", polygon,
                  0, strategy_ps, true, true, false);

    tester::apply("mpp4", "MULTIPOINT(0 10,10 0,0 0,20.1 15,10 11)", polygon,
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps
>
void test_distance_point_multipolygon(Strategy_pp const& strategy_pp,
                                      Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/multipolygon distance tests" << std::endl;
#endif
    typedef bg::model::polygon<Point> polygon_type;
    typedef bg::model::multi_polygon<polygon_type> multipolygon_type;
    typedef test_distance_of_geometries<Point, multipolygon_type> tester;

    std::string const mpoly = "MULTIPOLYGON(((20 20, 20 30, 30 40, 20 20)),\
                                     ((10 10,0 20, 15 30, 20 15, 15 10, 10 10)))";

    tester::apply("pmp1", "POINT(0 10)", mpoly,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);

    tester::apply("pmp2", "POINT(10 9.9)", mpoly,
                  pp_distance<Point>("POINT(10 9.9)",
                                     "POINT(10 10)", strategy_pp),
                  strategy_ps, true, true, false);

    tester::apply("pmp3", "POINT(10 11)", mpoly,
                  0, strategy_ps, true, true, false);
}

//===========================================================================

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps
>
void test_distance_multipoint_multipolygon(Strategy_pp const& strategy_pp,
                                           Strategy_ps const& strategy_ps)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/multipolygon distance tests" << std::endl;
#endif
    typedef bg::model::polygon<Point> polygon_type;
    typedef bg::model::multi_polygon<polygon_type> multipolygon_type;
    typedef bg::model::multi_point<Point> multi_point_type;
    typedef test_distance_of_geometries<multi_point_type, multipolygon_type> tester;

    std::string const mpoly = "MULTIPOLYGON(((20 20, 20 30, 30 40, 20 20)),\
                                  ((10 10,0 20, 15 30, 20 15, 15 10, 10 10)))";

    tester::apply("pr1", "MULTIPOINT(0 10,10 0,0 0)", mpoly,
                  ps_distance<Point>("POINT(0 10)",
                                     "SEGMENT(0 20, 10 10)", strategy_ps),
                  strategy_ps, true, true, false);

    tester::apply("pmp2", "MULTIPOINT(0 10,10 0,0 0,10 9.9)", mpoly,
                  pp_distance<Point>("POINT(10 9.9)",
                                     "POINT(10 10)", strategy_pp),
                  strategy_ps, true, true, false);

    tester::apply("pmp3", "MULTIPOINT(0 10,10 0,0 0,10 9.9,10 11)", mpoly,
                  0, strategy_ps, true, true, false);
}

//===========================================================================
// Cases for relative location of a point wrt to a box
//
//           |         |
//           |    3    |
//           |         |
//           +---------+
//           |         |
//       1   |    5    |   2
//           |         |
//           +---------+
//           |         |
//           |    4    |
//           |         |
//
// and also the following cases
//
//           |         |
//           A         B
//           |         |
//           +----C----+
//           |         |
//           D         E
//           |         |
//           +----F----+
//           |         |
//           G         H
//           |         |
//
// and finally we have the corners
//
//           |         |
//           |         |
//           |         |
//           a---------b
//           |         |
//           |         |
//           |         |
//           c---------d
//           |         |
//           |         |
//           |         |
//
// for each relative position we also have to test the shifted point
// (this is due to the fact that boxes have longitudes in the
// range [-180, 540)
//===========================================================================

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps,
        typename Strategy_pb
>
void test_distance_point_box(Strategy_pp const& strategy_pp,
                             Strategy_ps const& strategy_ps,
                             Strategy_pb const& strategy_pb)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/box distance tests" << std::endl;
#endif
    typedef bg::model::box<Point> box_type;
    typedef test_distance_of_geometries<Point, box_type> tester;

    std::string const box1 = "BOX(10 10,20 20)";

    // case 1
    tester::apply("pb1-1a", "POINT(5 25)", box1,
                  pp_distance<Point>("POINT(5 25)", "POINT(10 20)", strategy_pp),
                  strategy_pb);

    // case 1
    tester::apply("pb1-1b", "POINT(3 12)", box1,
                  ps_distance<Point>("POINT(3 12)",
                                     "SEGMENT(10 10,10 20)", strategy_ps),
                  strategy_pb);

    // case 1
    tester::apply("pb1-1c", "POINT(3 17)", box1,
                  ps_distance<Point>("POINT(3 17)",
                                     "SEGMENT(10 10,10 20)", strategy_ps),
                  strategy_pb);

    // case 1
    tester::apply("pb1-1d", "POINT(5 4)", box1,
                  pp_distance<Point>("POINT(5 4)", "POINT(10 10)", strategy_pp),
                  strategy_pb);

    // case 1
    tester::apply("pb1-1e", "POINT(-100 20)", box1,
                  pp_distance<Point>("POINT(-100 20)",
                                     "POINT(10 20)", strategy_pp),
                  strategy_pb);

    // case 1
    tester::apply("pb1-1g", "POINT(-100 10)", box1,
                  ps_distance<Point>("POINT(-100 10)",
                                     "SEGMENT(10 10,10 20)", strategy_ps),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2a", "POINT(31 25)", box1,
                  pp_distance<Point>("POINT(31 25)",
                                     "POINT(20 20)", strategy_pp),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2b", "POINT(23 17)", box1,
                  ps_distance<Point>("POINT(23 17)",
                                     "SEGMENT(20 10,20 20)", strategy_ps),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2c", "POINT(29 3)", box1,
                  pp_distance<Point>("POINT(29 3)",
                                     "POINT(20 10)", strategy_pp),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2d", "POINT(131 65)", box1,
                  pp_distance<Point>("POINT(131 65)",
                                     "POINT(20 20)", strategy_pp),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2e", "POINT(110 10)", box1,
                  ps_distance<Point>("POINT(110 10)",
                                     "SEGMENT(20 10,20 20)", strategy_ps),
                  strategy_pb);

    // case 2
    tester::apply("pb1-2f", "POINT(150 20)", box1,
                  pp_distance<Point>("POINT(150 20)",
                                     "POINT(20 20)", strategy_pp),
                  strategy_pb);

    // case 3
    tester::apply("pb1-3a", "POINT(11 25)", box1,
                  pp_distance<Point>("POINT(11 25)",
                                     "POINT(11 20)", strategy_pp),
                  strategy_pb);

    // case 3
    tester::apply("pb1-3b", "POINT(15 25)", box1,
                  pp_distance<Point>("POINT(15 25)",
                                     "POINT(15 20)", strategy_pp),
                  strategy_pb);

    // case 3
    tester::apply("pb1-3c", "POINT(18 25)", box1,
                  pp_distance<Point>("POINT(18 25)",
                                     "POINT(18 20)", strategy_pp),
                  strategy_pb);

    // case 4
    tester::apply("pb1-4a", "POINT(13 4)", box1,
                  pp_distance<Point>("POINT(13 4)",
                                     "POINT(13 10)", strategy_pp),
                  strategy_pb);

    // case 4
    tester::apply("pb1-4b", "POINT(19 4)", box1,
                  pp_distance<Point>("POINT(19 4)",
                                     "POINT(19 10)", strategy_pp),
                  strategy_pb);

    // case 5
    tester::apply("pb1-5", "POINT(15 14)", box1, 0, strategy_pb);

    // case A
    tester::apply("pb1-A", "POINT(10 28)", box1,
                  pp_distance<Point>("POINT(10 28)",
                                     "POINT(10 20)", strategy_pp),
                  strategy_pb);

    // case B
    tester::apply("pb1-B", "POINT(20 28)", box1,
                  pp_distance<Point>("POINT(20 28)",
                                     "POINT(20 20)", strategy_pp),
                  strategy_pb);

    // case C
    tester::apply("pb1-C", "POINT(14 20)", box1, 0, strategy_pb);

    // case D
    tester::apply("pb1-D", "POINT(10 17)", box1, 0, strategy_pb);

    // case E
    tester::apply("pb1-E", "POINT(20 11)", box1, 0, strategy_pb);

    // case F
    tester::apply("pb1-F", "POINT(19 10)", box1, 0, strategy_pb);

    // case G
    tester::apply("pb1-G", "POINT(10 -40)", box1,
                  pp_distance<Point>("POINT(10 -40)",
                                     "POINT(10 10)", strategy_pp),
                  strategy_pb);

    // case H
    tester::apply("pb1-H", "POINT(20 -50)", box1,
                  pp_distance<Point>("POINT(20 -50)",
                                     "POINT(20 10)", strategy_pp),
                  strategy_pb);

    // case a
    tester::apply("pb1-a", "POINT(10 20)", box1, 0, strategy_pb);
    // case b
    tester::apply("pb1-b", "POINT(20 20)", box1, 0, strategy_pb);
    // case c
    tester::apply("pb1-c", "POINT(10 10)", box1, 0, strategy_pb);
    // case d
    tester::apply("pb1-d", "POINT(20 10)", box1, 0, strategy_pb);

    std::string const box2 = "BOX(170 -60,400 80)";

    // case 1 - point is closer to western meridian
    tester::apply("pb2-1a", "POINT(160 0)", box2,
                  ps_distance<Point>("POINT(160 0)",
                                     "SEGMENT(170 -60,170 80)", strategy_ps),
                  strategy_pb);

    // case 1 - point is closer to eastern meridian
    tester::apply("pb2-1b", "POINT(50 0)", box2,
                  ps_distance<Point>("POINT(50 0)",
                                     "SEGMENT(40 -60,40 80)", strategy_ps),
                  strategy_pb);

    // case 3 - equivalent point POINT(390 85) is above the box
    tester::apply("pb2-3", "POINT(30 85)", box2,
                  pp_distance<Point>("POINT(30 85)",
                                     "POINT(30 80)", strategy_pp),
                  strategy_pb);

    // case 4 - equivalent point POINT(390 -75) is below the box
    tester::apply("pb2-4", "POINT(30 -75)", box2,
                  pp_distance<Point>("POINT(30 -75)",
                                     "POINT(30 -60)", strategy_pp),
                  strategy_pb);

    // case 5 - equivalent point POINT(390 0) is inside box
    tester::apply("pb2-5", "POINT(30 0)", box2, 0, strategy_pb);

    std::string const box3 = "BOX(-150 -50,-40 70)";

    // case 1 - point is closer to western meridian
    tester::apply("pb3-1a", "POINT(-170 10)", box3,
                  ps_distance<Point>("POINT(-170 10)",
                                     "SEGMENT(-150 -50,-150 70)", strategy_ps),
                  strategy_pb);

    // case 2 - point is closer to eastern meridian
    tester::apply("pb3-2a", "POINT(5 10)", box3,
                  ps_distance<Point>("POINT(5 10)",
                                     "SEGMENT(-40 -50,-40 70)", strategy_ps),
                  strategy_pb);

    // case 2 - point is closer to western meridian
    tester::apply("pb3-2a", "POINT(160 10)", box3,
                  ps_distance<Point>("POINT(160 10)",
                                     "SEGMENT(-150 -50,-150 70)", strategy_ps),
                  strategy_pb);

    // case 2 - point is at equal distance from eastern and western meridian
    tester::apply("pb3-2c1", "POINT(85 20)", box3,
                  ps_distance<Point>("POINT(85 20)",
                                     "SEGMENT(-150 -50,-150 70)", strategy_ps),
                  strategy_pb);

    // case 2 - point is at equal distance from eastern and western meridian
    tester::apply("pb3-2c2", "POINT(85 20)", box3,
                  ps_distance<Point>("POINT(85 20)",
                                     "SEGMENT(-40 -50,-40 70)", strategy_ps),
                  strategy_pb);

    // box that is symmetric wrt the prime meridian
    std::string const box4 = "BOX(-75 -45,75 65)";

    // case 1 - point is closer to western meridian
    tester::apply("pb4-1a", "POINT(-100 10)", box4,
                  ps_distance<Point>("POINT(-100 10)",
                                     "SEGMENT(-75 -45,-75 65)", strategy_ps),
                  strategy_pb);

    // case 2 - point is closer to eastern meridian
    tester::apply("pb4-2a", "POINT(90 15)", box4,
                  ps_distance<Point>("POINT(90 15)",
                                     "SEGMENT(75 -45,75 65)", strategy_ps),
                  strategy_pb);

    // case 2 - point is at equal distance from eastern and western meridian
    tester::apply("pb4-2c1", "POINT(-180 20)", box4,
                  ps_distance<Point>("POINT(-180 20)",
                                     "SEGMENT(-75 -45,-75 65)", strategy_ps),
                  strategy_pb);

    // case 2 - point is at equal distance from eastern and western meridian
    tester::apply("pb4-2c2", "POINT(-180 20)", box4,
                  ps_distance<Point>("POINT(-180 20)",
                                     "SEGMENT(75 -45,75 65)", strategy_ps),
                  strategy_pb);

    //box degenerates to a meridian segment
    std::string const boxdeg1 = "BOX(0 10,0 20)";

    tester::apply("pbd1", "POINT(1 10)", boxdeg1,
                  ps_distance<Point>("POINT(1 10)",
                                     "SEGMENT(0 10, 0 20)", strategy_ps),
                  strategy_pb);
    tester::apply("pbd2", "POINT(1 5)", boxdeg1,
                  ps_distance<Point>("POINT(1 5)",
                                     "SEGMENT(0 10, 0 20)", strategy_ps),
                  strategy_pb);
    tester::apply("pbd3", "POINT(1 15)", boxdeg1,
                  ps_distance<Point>("POINT(1 15)",
                                     "SEGMENT(0 10, 0 20)", strategy_ps),
                  strategy_pb);
    tester::apply("pbd4", "POINT(1 25)", boxdeg1,
                  ps_distance<Point>("POINT(1 25)",
                                     "SEGMENT(0 10, 0 20)", strategy_ps),
                  strategy_pb);

    //box degenerates to a horizontal line; that is not a geodesic segment
    std::string const boxdeg2 = "BOX(10 10,20 10)";

    tester::apply("pbd5", "POINT(15 15)", boxdeg2,
                  pp_distance<Point>("POINT(15 15)",
                                     "POINT(15 10)", strategy_pp),
                  strategy_pb);
    tester::apply("pbd6", "POINT(5 15)", boxdeg2,
                  pp_distance<Point>("POINT(5 15)",
                                     "POINT(10 10)", strategy_pp),
                  strategy_pb);
    tester::apply("pbd7", "POINT(25 15)", boxdeg2,
                  pp_distance<Point>("POINT(25 15)",
                                     "POINT(20 10)", strategy_pp),
                  strategy_pb);

    //box degenerates to a point
    std::string const boxdeg3 = "BOX(0 10,0 10)";

    tester::apply("pbd8", "POINT(1 11)", boxdeg3,
                  pp_distance<Point>("POINT(1 11)",
                                     "POINT(0 10)", strategy_pp),
                  strategy_pb);
}

template
<
        typename Point,
        typename Strategy_pp,
        typename Strategy_ps,
        typename Strategy_pb
>
void test_distance_multipoint_box(Strategy_pp const& strategy_pp,
                                  Strategy_ps const& strategy_ps,
                                  Strategy_pb const& strategy_pb)
{

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/box distance tests" << std::endl;
#endif
    typedef bg::model::box<Point> box_type;
    typedef bg::model::multi_point<Point> multi_point_type;
    typedef test_distance_of_geometries<multi_point_type, box_type> tester;

    std::string const box1 = "BOX(10 10,20 20)";

    tester::apply("mpb1", "MULTIPOINT(5 25,25 26)", box1,
                  pp_distance<Point>("POINT(5 25)", "POINT(10 20)", strategy_pp),
                  strategy_pb, true, false, false);

    tester::apply("mpb2", "MULTIPOINT(110 10,110 9,110 0)", box1,
                  ps_distance<Point>("POINT(110 10)",
                                     "SEGMENT(20 10,20 20)", strategy_ps),
                  strategy_pb, true, false, false);

    tester::apply("mpb3", "MULTIPOINT(110 10,110 9,110 0,10 20)", box1,
                  0, strategy_pb, true, false, false);

    tester::apply("mpb3", "MULTIPOINT(110 10,110 9,110 0,15 15)", box1,
                  0, strategy_pb, true, false, false);
}

//===========================================================================
//===========================================================================
//===========================================================================

template
<
    typename Point,
    typename Strategy_pp,
    typename Strategy_ps,
    typename Strategy_pb
>
void test_all_pl_ar(Strategy_pp pp_strategy,
                    Strategy_ps ps_strategy,
                    Strategy_pb pb_strategy)
{
    test_distance_point_ring<Point>(pp_strategy, ps_strategy);
    test_distance_multipoint_ring<Point>(ps_strategy);

    test_distance_point_polygon<Point>(pp_strategy, ps_strategy);
    test_distance_multipoint_polygon<Point>(pp_strategy, ps_strategy);

    test_distance_point_multipolygon<Point>(pp_strategy, ps_strategy);
    test_distance_multipoint_multipolygon<Point>(pp_strategy, ps_strategy);

    test_distance_point_box<Point>(pp_strategy, ps_strategy, pb_strategy);
    test_distance_multipoint_box<Point>(pp_strategy, ps_strategy, pb_strategy);

    test_more_empty_input_pointlike_areal<Point>(ps_strategy);
}

BOOST_AUTO_TEST_CASE( test_all_pointlike_areal )
{
    typedef bg::model::point
            <
                double, 2,
                bg::cs::spherical_equatorial<bg::degree>
            > sph_point;

    test_all_pl_ar<sph_point>(spherical_pp(), spherical_ps(), spherical_pb());

    typedef bg::model::point
            <
                double, 2,
                bg::cs::geographic<bg::degree>
            > geo_point;

    test_all_pl_ar<geo_point>(vincenty_pp(), vincenty_ps(), vincenty_pb());
    test_all_pl_ar<geo_point>(thomas_pp(), thomas_ps(), thomas_pb());
    test_all_pl_ar<geo_point>(andoyer_pp(), andoyer_ps(), andoyer_pb());
    //test_all_pl_ar<geo_point>(karney_pp(), karney_ps(), karney_pb());

    // test with different spheroid
    stype spheroid(6372000, 6370000);
    test_all_pl_ar<geo_point>(andoyer_pp(spheroid), andoyer_ps(spheroid),  andoyer_pb(spheroid));
}


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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_SELECTED_HPP
#define BOOST_GEOMETRY_ALGORITHMS_SELECTED_HPP

#include <cmath>
#include <cstddef>

#include <boost/range.hpp>

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

#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/topological_dimension.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

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

namespace boost { namespace geometry
{

/*!
    \ingroup impl
 */
#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace selected
{

/*!
\details Checks, per dimension, if d[I] not larger than search distance. If true for all
dimensions then returns true. If larger stops immediately and returns false.
Calculate during this process the sum, which is only valid if returning true
*/
template <typename P1, typename P2, typename T, std::size_t D, std::size_t N>
struct differences_loop
{
    static inline bool apply(P1 const& p1, P2 const& p2, T const& distance, T& sum)
    {
        typedef typename select_coordinate_type<P1, P2>::type coordinate_type;

        coordinate_type const c1 = boost::numeric_cast<coordinate_type>(get<D>(p1));
        coordinate_type const c2 = boost::numeric_cast<coordinate_type>(get<D>(p2));

        T const d = geometry::math::abs(c1 - c2);
        if (d > distance)
        {
            return false;
        }
        sum += d * d;
        return differences_loop<P1, P2, T, D + 1, N>::apply(p1, p2, distance, sum);
    }
};

template <typename P1, typename P2, typename T, std::size_t N>
struct differences_loop<P1, P2, T, N, N>
{
    static inline bool apply(P1 const&, P2 const&, T const&, T&)
    {
        return true;
    }
};

template <typename PS, typename P, typename T, std::size_t D, std::size_t N>
struct outside_loop
{
    static inline bool apply(PS const& seg1, PS const& seg2, P const& point, T const& distance)
    {
        typedef typename select_coordinate_type<PS, P>::type coordinate_type;

        coordinate_type const v = boost::numeric_cast<coordinate_type>(get<D>(point));
        coordinate_type const s1 = get<D>(seg1);
        coordinate_type const s2 = get<D>(seg2);

        // Out of reach if left/bottom or right/top of both points making up the segment
        // I know and currently accept that these comparisons/calculations are done twice per point

        if ((v < s1 - distance && v < s2 - distance) || (v > s1 + distance && v > s2 + distance))
        {
            return true;
        }
        return outside_loop<PS, P, T, D + 1, N>::apply(seg1, seg2, point, distance);
    }
};

template <typename PS, typename P, typename T, std::size_t N>
struct outside_loop<PS, P, T, N, N>
{
    static inline bool apply(PS const&, PS const&, P const&, T const&)
    {
        return false;
    }
};

template <typename P1, typename P2, typename T>
struct close_to_point
{
    static inline bool apply(P1 const& point, P1 const& selection_point, T const& search_radius)
    {
        assert_dimension_equal<P1, P2>();

        T sum = 0;
        if (differences_loop
                <
                    P1, P2, T, 0, dimension<P1>::type::value
                >::apply(point, selection_point, search_radius, sum))
        {
            return sum <= search_radius * search_radius;
        }

        return false;
    }
};

template <typename PS, typename P, typename T>
struct close_to_segment
{
    static inline bool apply(PS const& seg1, PS const& seg2, P const& selection_point, T const& search_radius)
    {
        assert_dimension_equal<PS, P>();

        if (! outside_loop
                <
                    PS, P, T, 0, dimension<P>::type::value
                >::apply(seg1, seg2, selection_point, search_radius))
        {
            // Not outside, calculate dot product/square distance to segment.
            // Call corresponding strategy
            typedef typename strategy::distance::services::default_strategy
                <
                    segment_tag, P, PS
                >::type strategy_type;
            typedef typename strategy::distance::services::return_type<strategy_type>::type return_type;

            strategy_type strategy;
            return_type result = strategy.apply(selection_point, seg1, seg2);
            return result < search_radius;
        }

        return false;
    }
};

template <typename R, typename P, typename T>
struct close_to_range
{
    static inline bool apply(R const& range, P const& selection_point, T const& search_radius)
    {
        assert_dimension_equal<R, P>();

        std::size_t const n = boost::size(range);
        if (n == 0)
        {
            // Line with zero points, never close
            return false;
        }

        typedef typename point_type<R>::type point_type;
        typedef typename boost::range_iterator<R const>::type iterator_type;

        iterator_type it = boost::begin(range);
        if (n == 1)
        {
            // Line with one point ==> close to point
            return close_to_point<P, point_type, T>::apply(*it, selection_point, search_radius);
        }

        iterator_type previous = it++;
        while(it != boost::end(range))
        {
            //typedef segment<point_type const> segment_type;
            //segment_type s(*previous, *it);
            if (close_to_segment
                    <
                        point_type, P, T
                    >::apply(*previous, *it, selection_point, search_radius))
            {
                return true;
            }
            previous = it++;
        }

        return false;
    }
};

template <typename Tag, typename G, typename P, typename T>
struct use_within
{
    static inline bool apply(G const& geometry, P const& selection_point, T const& search_radius)
    {
        return geometry::within(selection_point, geometry);
    }
};

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

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

/*!
    \tparam TD topological dimension
 */
template <typename Tag, typename G, std::size_t D, typename P, typename T>
struct selected
{
};

template <typename P1, typename P2, typename T>
struct selected<point_tag, P1, 0, P2, T> : detail::selected::close_to_point<P1, P2, T> { };

// SEGMENT, TODO HERE (close_to_segment)

template <typename L, typename P, typename T>
struct selected<linestring_tag, L, 1, P, T> : detail::selected::close_to_range<L, P, T> { };

template <typename Tag, typename G, typename P, typename T>
struct selected<Tag, G, 2, P, T> : detail::selected::use_within<Tag, G, P, T> { };

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
    \brief Checks if one geometry is selected by a point lying within or in the neighborhood of that geometry
    \ingroup selected
    \tparam Geometry type of geometry to check
    \tparam Point type of point to check
    \tparam T type of search radius
    \param geometry geometry which might be located in the neighborhood
    \param selection_point point to select the geometry
    \param search_radius for points/linestrings: defines radius of "neighborhood" to find things in
    \return true if point is within or close to the other geometry

 */
template<typename Geometry, typename Point, typename RadiusType>
inline bool selected(Geometry const& geometry,
        Point const& selection_point,
        RadiusType const& search_radius)
{
    concept::check<Geometry const>();
    concept::check<Point const>();

    typedef dispatch::selected
        <
            typename tag<Geometry>::type,
            Geometry,
            topological_dimension<Geometry>::value,
            Point,
            RadiusType
        > selector_type;

    return selector_type::apply(geometry, selection_point, search_radius);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_SELECTED_HPP


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-ieee
    This function object returns  the largest of two floating point arguments, treating NaNs
    as missing data (between a NaN and a numeric value, the numeric value is chosen).

    @par Header <boost/simd/function/maxnum.hpp>

    @par Decorators

     - std_ for floating entries calls the stdlibc++ function std::fmax.

    @see max, maxnummag,  maxmag

    @par Example:

      @snippet maxnum.cpp maxnum

    @par Possible output:

      @snippet maxnum.txt maxnum

  **/
  Value maxnum(Value const& x, Value const& y);
} }
#endif

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

#endif


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/SparseCOO.h++>

typedef boost::mpl::list<
	float,
	double
> scalar_list;

template <typename Scalar>
using MatrixType = mla::matrix::SparseCOO<Scalar>;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);

	BOOST_CHECK_EQUAL(m.rows(), 3);
	BOOST_CHECK_EQUAL(m.columns(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), 0.0);
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Assigning, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(1,1) = 1.0;

	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);

	BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Permute_rows, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	m.permuteRows(2,3);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Permute_columns, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	m.permuteColumns(2,3);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( symmetric_reorder_identity_matrix, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	std::vector<size_t> new_order{0,1,2,3,4};

	m.symmetricReorder(new_order);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( symmetric_reorder_identity_matrix_2, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	std::vector<size_t> new_order{0,2,1,3,4};

	m.symmetricReorder(new_order);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( splitOutBottom_diagonal_matrix, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m.setValue(0,0, 0.0);
	m.setValue(1,1, 1.0);
	m.setValue(2,2, 2.0);
	m.setValue(3,3, 3.0);
	m.setValue(4,4, 4.0);

	MatrixType<Scalar> m2 = m.splitOutBottom(2);

	// test NNZ size
	BOOST_CHECK_EQUAL(m.nnz(), 2);
	BOOST_CHECK_EQUAL(m2.nnz(), 3);

	// test matrix size
	BOOST_CHECK_EQUAL(m.rows(), 2);
	BOOST_CHECK_EQUAL(m.columns(), 5);
	BOOST_CHECK_EQUAL(m2.rows(), 3);
	BOOST_CHECK_EQUAL(m2.columns(), 5);

	// test matrix elements
	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m2.getValue(0, 2), 2.0);
	BOOST_CHECK_EQUAL(m2.getValue(1, 3), 3.0);
	BOOST_CHECK_EQUAL(m2.getValue(2, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( splitOutRight_diagonal_matrix, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m.setValue(0,0, 0.0);
	m.setValue(1,1, 1.0);
	m.setValue(2,2, 2.0);
	m.setValue(3,3, 3.0);
	m.setValue(4,4, 4.0);

	MatrixType<Scalar> m2 = m.splitOutRight(2);

	// test NNZ size
	BOOST_CHECK_EQUAL(m.nnz(), 2);
	BOOST_CHECK_EQUAL(m2.nnz(), 3);

	// test matrix size
	BOOST_CHECK_EQUAL(m.rows(), 5);
	BOOST_CHECK_EQUAL(m.columns(), 2);
	BOOST_CHECK_EQUAL(m2.rows(), 5);
	BOOST_CHECK_EQUAL(m2.columns(), 3);

	// test matrix elements
	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m2.getValue(2, 0), 2.0);
	BOOST_CHECK_EQUAL(m2.getValue(3, 1), 3.0);
	BOOST_CHECK_EQUAL(m2.getValue(4, 2), 4.0);
}

BOOST_AUTO_TEST_SUITE_END()


#include "mm/ug_bitext.h"
#include <boost/format.hpp>
// #include <unicode/stringpiece.h>
#include <unicode/translit.h>
#include <unicode/utypes.h>
#include <unicode/unistr.h>
#include <unicode/uchar.h>
#include <unicode/utf8.h>
#include "moses/TranslationModel/UG/generic/stringdist/ug_stringdist.h"

using namespace std;
using namespace Moses;
using namespace ugdiss;
using namespace Moses::bitext;

typedef L2R_Token<SimpleWordId> Token;
typedef mmTtrack<Token> ttrack_t;
typedef mmTSA<Token>     tsa_t;
typedef vector<Moses::bitext::PhrasePair<Token> > pplist_t;
typedef pair<ushort,ushort> span_t;

TokenIndex V1,V2;
boost::shared_ptr<ttrack_t> T1,T2;
tsa_t I1,I2;
mmBitext<Token> BT;

float lbop_level = .05;
#define smooth 1
namespace stats
{
  using namespace Moses::bitext;
  float
  pmi(size_t j,size_t m1, size_t m2, size_t N)
  {
#if smooth
    float p1 = lbop(N,m1,lbop_level);
    float p2 = lbop(N,m2,lbop_level);
    float p12 = lbop(N,j,lbop_level);
    return log(p12) - log(p1) - log(p2);
#else
    return log(j) + log(N) - log(m1) - log(m2);
#endif
  }

  float
  npmi(size_t j,size_t m1, size_t m2, size_t N)
  {
#if smooth
    // cout << j << " " << m1 << " " << m2 << " " << N << endl;
    float p1 = lbop(N,m1,lbop_level);
    float p2 = lbop(N,m2,lbop_level);
    float p12 = lbop(N,j,lbop_level);
    return (log(p12) - log(p1) - log(p2)) / -log(p12);
#else
    return pmi(j,m1,m2,N) / (log(N) - log(j));
#endif
  }

  float
  mi(size_t j,size_t m1, size_t m2, size_t N)
  {
    float ret = 0;
    if (j)    ret += float(j)/N * pmi(j,m1,m2,N);
    if (m1>j) ret += float(m1-j)/N * pmi(m1-j,m1,N-m2,N);
    if (m2>j) ret += float(m2-j)/N * pmi(m2-j,N-m1,m2,N);
    if (N>m1+m2-j) ret += float(N-m1-m2+j)/N * pmi(N-m1-m2+j,N-m1,N-m2,N);
    return ret;
  }
}

struct SinglePhrase
{
  typedef map<uint64_t,SPTR<SinglePhrase> > cache_t;
  uint64_t pid; // phrase id
  vector<ttrack::Position> occs; // occurrences
};

struct PhrasePair2
{
  struct score_t;
  uint64_t p1,p2;
  ushort s1,e1,s2,e2;
  int parent;

  struct stats_t
  {
    typedef map<pair<uint64_t,uint64_t>, SPTR<stats_t> > cache_t;
    size_t m1,m2,j;
    float  npmi; // normalized point-wise mutual information
    float   pmi; // point-wise mutual information
    float    mi; // mutual information
    float score;

    void
    set(vector<ttrack::Position> const& o1,
	vector<ttrack::Position> const& o2,
	size_t const N)
    {
      m1 = m2 = j = 0;
      size_t i1=0,i2=0;
      while (i1 < o1.size() && i2 < o2.size())
	{
	  if (i1 && o1[i1].sid == o1[i1-1].sid) { ++i1; continue; }
	  if (i2 && o2[i2].sid == o2[i2-1].sid) { ++i2; continue; }

	  if (o1[i1].sid == o2[i2].sid) { ++j; ++i1; ++i2; ++m1; ++m2; }
	  else if (o1[i1].sid < o2[i2].sid) { ++i1; ++m1; }
	  else { ++i2; ++m2; }
	}
      // for (++i1; i1 < o1.size(); ++i1)
      // 	if (o1[i1-1].sid != o1[i1].sid) ++m1;
      // for (++i2; i2 < o2.size(); ++i2)
      // 	if (o2[i2-1].sid != o2[i2].sid) ++m2;

      m1 = 1; m2 = 1;
      for (i1=1; i1 < o1.size(); ++i1)
       	if (o1[i1-1].sid != o1[i1].sid) ++m1;
      for (i2=1; i2 < o2.size(); ++i2)
	if (o2[i2-1].sid != o2[i2].sid) ++m2;

      this->mi = stats::mi(j,m1,m2,N);
      this->pmi = stats::pmi(j,m1,m2,N);
      this->npmi = stats::npmi(j,m1,m2,N);
      // float z = float(m1)/N * float(m2)/N;
      // float hmean = 2.*j/(m1+m2);
      this->score = npmi; // npmi; // hmean; // /sqrt(z);
    }
  } stats;

  PhrasePair2(ushort s1_=0, ushort e1_=0, ushort s2_=0, ushort e2_=0)
    : s1(s1_), e1(e1_), s2(s2_), e2(e2_), parent(-1) { }

  bool
  operator<(PhrasePair2 const& other) const
  {
    return (this->stats.score == other.stats.score
	    ? (e1-s1 + e2-s2 > other.e1-other.s1 + other.e2-other.s2)
	    : (this->stats.score > other.stats.score));
  }

  size_t len1() const { return e1 - s1; }
  size_t len2() const { return e2 - s2; }
  bool includes(PhrasePair2 const& o) const
  {
    return s1 <= o.s1 && e1 >= o.e1 && s2 <= o.s2 && e2 >= o.e2;
  }

};

SinglePhrase::cache_t cache1,cache2;
PhrasePair2::stats_t::cache_t ppcache;

struct SortByPositionInCorpus
{
  bool
  operator()(ttrack::Position const& a,
	     ttrack::Position const& b) const
  {
    return a.sid != b.sid ? a.sid < b.sid : a.offset < b.offset;
  }
};

void
getoccs(tsa_t::tree_iterator const& m,
	vector<ttrack::Position>& occs)
{
  occs.clear();
  occs.reserve(m.approxOccurrenceCount()+10);
  tsa::ArrayEntry I(m.lower_bound(-1));
  char const* stop = m.upper_bound(-1);
  do {
    m.root->readEntry(I.next,I);
    occs.push_back(I);
  } while (I.next != stop);
  sort(occs.begin(),occs.end(),SortByPositionInCorpus());
}

void
lookup_phrases(vector<id_type> const& snt,
	       TokenIndex& V, ttrack_t const& T,
	       tsa_t const& I, SinglePhrase::cache_t& cache,
	       vector<vector<SPTR<SinglePhrase> > >& dest)
{
  dest.resize(snt.size());
  for (size_t i = 0; i < snt.size(); ++i)
    {
      tsa_t::tree_iterator m(&I);
      dest[i].clear();
      for (size_t k = i; k < snt.size() && m.extend(snt[k]); ++k)
	{
	  if (m.approxOccurrenceCount() < 3) break;
	  // if (k - i > 0) break;
	  SPTR<SinglePhrase>& o = cache[m.getPid()];
	  if (!o)
	    {
	      o.reset(new SinglePhrase());
	      o->pid = m.getPid();
	      getoccs(m,o->occs);
	    }
	  dest[i].push_back(o);
	}
    }
}

struct
RowIndexSorter
{
  vector<vector<float> > const& M;
  size_t const my_col;
  RowIndexSorter(vector<vector<float> > const& m, size_t const c)
    : M(m), my_col(c) { }

  template<typename T>
  bool
  operator()(T const& a, T const& b) const
  {
    return M.at(a).at(my_col) > M.at(b).at(my_col);
  }
};

struct
ColIndexSorter
{
  vector<vector<float> > const& M;
  size_t const my_row;
  ColIndexSorter(vector<vector<float> > const& m, size_t const r)
    : M(m), my_row(r) { }

  template<typename T>
  bool
  operator()(T const& a, T const& b) const
  {
    return M.at(my_row).at(a) > M[my_row].at(b);
  }

};

template<typename Token>
class
npmi_scorer1 : public Moses::bitext::PhrasePair<Token>::Scorer
{
public:
  float operator()(PhrasePair<Token>& pp) const
  {
#if 0
    cout << pp.raw1 << " " << pp.sample1 << " " << pp.good1 << " "
	 << pp.raw2 << " " << pp.sample2 << " " << pp.good2 << " "
	 << pp.joint << " " << __FILE__ << ":" << __LINE__ << endl;
#endif
    pp.good2 = ceil(pp.raw2 * float(pp.good1)/pp.raw1);
    size_t N  = ceil(BT.T1->numTokens() * float(pp.good1)/pp.raw1);
    return pp.score = stats::npmi(pp.joint,pp.good1,pp.good2,N);
  }
};

class Alnhyp
{
  ushort s1,s2,e1,e2;
  float score;
};

size_t
lcs(string const a, string const b)
{
  using namespace stringdist;
  if (a == b) return a.size();
  StringDiff diff(a,b);
  size_t ret = 0;
  size_t len = 0;
  // size_t th = min(size_t(4),min(a.size(),b.size()));
  for (size_t i = 0; i < diff.size(); ++i)
    {
      StringDiff::Segment const& s = diff[i];
      if (s.match != StringDiff::same && s.match != StringDiff::cap)
	{
	  if (len > ret) ret = len;
	  len = 0;
	  continue;
	}
      len += s.end_a - s.start_a;
    }
  if (len > ret) ret = len;
  return ret;
}

size_t
mapstring(string const& utf8,
	  UnicodeString& U,
	  vector<int>& c2w,
	  vector<int>* wlen=NULL)
{
  static UChar space = UnicodeString(" ")[0];
  assert(utf8.at(0) != ' ');
  U = UnicodeString(utf8.c_str()).toLower();
  stringdist::strip_accents(U);
  c2w.assign(U.length(),-1);
  size_t k = 0;
  size_t z = 0;
  for (int i = 0; i < U.length(); ++i)
    if (U[i] == space) { if (wlen) wlen->push_back(i-z); z = ++k; }
    else c2w[i] = k;
  assert(c2w.back() >= 0);
  if (wlen) wlen->push_back(U.length()-z);
  return k+1;
}

void
align_letters(UnicodeString const& A, vector<int> const& a2p,
	      UnicodeString const& B, vector<int> const& b2p,
	      vector<vector<int> >& W)
{
  vector<vector<int> > M(A.length(),vector<int>(B.length(),0));
  for (int a = 0; a < A.length(); ++a)
    {
      for (int b = 0; b < B.length(); ++b)
	{
	  if (A[a] != B[b] || a2p[a] < 0 || b2p[b] < 0)
	    continue;
	  M[a][b] = (a && b) ? M[a-1][b-1] + 1 : 1;
	  int& x = W[a2p[a]][b2p[b]];
	  x = max(x,M[a][b]);
	}
    }
  // string astring; A.toUTF8String(astring);
  // string bstring; B.toUTF8String(bstring);
  // cout << astring << "\n" << bstring << endl;
  // for (size_t r = 0; r < W.size(); ++r)
  //   {
  //     BOOST_FOREACH(int x, W[r]) cout << setw(3) << x;
  //     cout << endl;
  //   }
}

void
map_back(vector<vector<int> > const& W,
	 vector<vector<int> > & X,
	 vector<uchar> const & aln)
{
  for (size_t i = 0; i < aln.size(); i += 2)
    {
      vector<int> const& w = W.at(aln[i+1]);
      vector<int>& x = X.at(aln[i]);
      assert(x.size() == w.size());
      for (size_t k = 0; k < x.size(); ++k)
	x[k] = max(w[k],x[k]);
    }
}

void trymatch3(vector<PhrasePair<Token> > const& tcands,
	       UnicodeString const& T, size_t const tlen,
	       vector<int> const& t2p,
	       TokenIndex const& V2, vector<vector<int> >&X)
{
  BOOST_FOREACH(PhrasePair<Token> const& pp, tcands)
    {
      UnicodeString H; vector<int> h2p;
      string hstring = toString(V2, pp.start2, pp.len2);
      size_t hlen = mapstring(hstring,H,h2p);
      vector<vector<int> > W(hlen,vector<int>(tlen,0));
      align_letters(H, h2p, T, t2p, W);
#if 0
      string s; S.toUTF8String(s);
      string h; H.toUTF8String(h);
      string t; T.toUTF8String(t);
      cout << s << endl << h << endl << t << endl;
      cout << slen << " " << tlen << endl;
      cout << "W: " << W.size() << " rows; " << W[0].size() << " cols" << endl;
      cout << "X: " << X.size() << " rows; " << X[0].size() << " cols" << endl;
      cout << "aln: ";
      for (size_t a = 0; a < pp.aln.size(); a +=2)
	cout  << int(pp.aln[a]) << "-" << int(pp.aln[a+1]) << " ";
      cout << endl;
#endif
      map_back(W,X,pp.aln);
    }
}

void minmatch_filter(vector<vector<int> > & X,
		     vector<int> const& len1,
		     vector<int> const& len2)
{
  // compute marginals
  vector<int> m1(X.size(),0), m2(X.at(0).size(),0);
  for (size_t r = 0;  r < X.size(); ++r)
    for (size_t c = 0; c < X[r].size(); ++c)
      {
	if (X[r][c] == 0) continue;
	m1[r] += X[r][c];
	m2[c] += X[r][c];
      }

  bool sth_changed = true;
  while (sth_changed)
    {
      sth_changed = false;
      for (size_t r = 0;  r < m1.size(); ++r)
	{
	  if (m1[r] && m1[r] < max(2,min(5,len1[r]/2)))
	    {
	      sth_changed = true;
	      for (size_t c = 0; c < X[r].size(); ++c)
		{
		  m2[c] -= X[r][c];
		  X[r][c] = 0;
		}
	      m1[r] = 0;
	    }
	}

      for (size_t c = 0; c < m2.size(); ++c)
	{
	  if (m2[c] && m2[c] < max(2,min(5,len2[c]/2)))
	    {
	      sth_changed = true;
	      for (size_t r = 0; r < m1.size(); ++r)
		{
		  m1[r] -= X[r][c];
		  X[r][c] = 0;
		}
	      m2[c] = 0;
	    }
	}
    }
}

void
trymatch2(TokenIndex& V1, // source language vocab
	  TokenIndex& V2, // target language vocab
	  string const& source, // source phrase
	  string const& target, // observed target candidate
 	  vector<PhrasePair<Token> > const* const tcands,
	  vector<vector<int> >& X) // destination alignment matrix
	  // tcands: translations for source
{
  UnicodeString S,T;
  vector<int> t2p, s2p; // maps from character position in string to word pos.
  vector<int> wlen_t, wlen_s; // individual word lengths
  size_t slen = mapstring(source, S, s2p, &wlen_s);
  size_t tlen = mapstring(target, T, t2p, &wlen_t);

  X.assign(slen,vector<int>(tlen,0));
  if (slen == 1 && tlen ==1 && S == T)
    X[0][0] = S.length();
  else
    {
      align_letters(S,s2p,T,t2p,X);
      if (tcands) trymatch3(*tcands, T, tlen, t2p, V2, X);
    }

  minmatch_filter(X, wlen_s, wlen_t);
  bool hit = false;
  for (size_t r = 0; !hit && r < X.size(); ++r)
    for (size_t c = 0; !hit && c < X[r].size(); ++c)
      hit = X[r][c] > min(S.length(),T.length())/2;

  // if (hit)
  //   {
  //     cout << source << " ::: " << target;
  //     for (size_t r = 0; r < X.size(); ++r)
  // 	for (size_t c = 0; c < X[r].size(); ++c)
  // 	  cout << boost::format(" %u-%u:%d") %  r % c % X[r][c];
  //     cout << endl;
  //   }
}

// float
// trymatch(string const a, string const b,
// 	 vector<PhrasePair<Token> > const* atrans,
// 	 vector<PhrasePair<Token> > const* btrans)
// {
//   if (a == b) return a.size();
//   float score = 0;
//   float bar = lcs(a,b);
//   // score = max(bar/min(a.size(),b.size()),score);
//   score = max(bar,score);
//   // cout << "\n[" << bar << "] " << a << " ::: " << b << endl;
//   if (atrans)
//     {
//       BOOST_FOREACH(PhrasePair<Token> const& pp, *atrans)
// 	{
// 	  // if (!pp.aln.size()) continue;
// 	  ushort L = pp.aln[1], R = pp.aln[1];
// 	  for (size_t k = 3; k < pp.aln.size(); k += 2)
// 	    {
// 	      if (L > pp.aln[k]) L = pp.aln[k];
// 	      if (R < pp.aln[k]) R = pp.aln[k];
// 	    }
// 	  if (L || R+1U < pp.len2) continue;
// 	  string foo = toString(*BT.V2,pp.start2,pp.len2);
// 	  // float bar = float(lcs(foo,b))/min(foo.size(),b.size());
// 	  float bar = float(lcs(foo,b));

// 	  if (bar > .5)
// 	    {
// 	      // score = max(pp.score * bar,score);
// 	      score = max(bar,score);
// 	      // cout << "[" << bar << "] " << foo << " ::: " << b
// 	      // << " (" << a << ") " << pp.score << endl;
// 	    }
// 	}
//     }
//   if (btrans)
//     {
//       BOOST_FOREACH(PhrasePair<Token> const& pp, *btrans)
// 	{
// 	  // if (!pp.aln.size()) continue;
// 	  ushort L = pp.aln[1], R = pp.aln[1];
// 	  for (size_t k = 3; k < pp.aln.size(); k += 2)
// 	    {
// 	      if (L > pp.aln[k]) L = pp.aln[k];
// 	      if (R < pp.aln[k]) R = pp.aln[k];
// 	    }
// 	  if (L || R+1U < pp.len2) continue;
// 	  string foo = toString(*BT.V1,pp.start2,pp.len2);
// 	  // float bar = float(lcs(a,foo))/min(a.size(),foo.size());
// 	  float bar = float(lcs(a,foo));
// 	  if (bar > .5)
// 	    {
// 	      score = max(bar,score);
// 	      // cout << "[" << bar<< "] " << a << " ::: " << foo
// 	      // << " (" << b << ") " << pp.score << endl;
// 	    }
// 	}
//     }
//   return score;
// }

struct ahyp
{
  ushort s1,s2,e1,e2;
  float score;
  bool operator<(ahyp const& o) const { return score < o.score; }
  bool operator>(ahyp const& o) const { return score > o.score; }
};

struct AlnPoint
{
  enum status { no = 0, yes = 1, maybe = -1, undef = -7 };
  float    score;
  status   state;
  AlnPoint() : score(0), state(undef) {}
};

bool overlap(span_t const& a, span_t const& b)
{
  return !(a.second <= b.first || b.second <= a.first);
}

class AlnMatrix
{
  vector<bitvector> A1,A2;  // final alignment matrix
  vector<bitvector> S1,S2;  // shadow alignment matrix
public:
  vector<bitvector*> m1,m2; // margins
  AlnMatrix(size_t const rows, size_t const cols);
  bitvector const&
  operator[](size_t const r) const
  { return A1.at(r); }

  bool
  incorporate(span_t const& rspan, span_t const& cspan,
	      vector<uchar> const& aln, bool const flip);

  size_t size() const { return A1.size(); }
};

AlnMatrix::
AlnMatrix(size_t const rows, size_t const cols)
{
  A1.assign(rows,bitvector(cols));
  S1.assign(rows,bitvector(cols));
  A2.assign(cols,bitvector(rows));
  S2.assign(cols,bitvector(rows));
  m1.assign(rows,NULL);
  m2.assign(cols,NULL);
}

bool
AlnMatrix::
incorporate(span_t const& rspan,
	    span_t const& cspan,
	    vector<uchar> const& aln,
	    bool const flip)
{
  for (size_t r = rspan.first; r < rspan.second; ++r)
    S1[r].reset();
  for (size_t c = cspan.first; c < cspan.second; ++c)
    S2[c].reset();
  if (flip)
    {
      for (size_t i = 0; i < aln.size(); i += 2)
	{
	  size_t r = rspan.first + aln[i];
	  size_t c = cspan.first + aln[i+1];
	  S1[r].set(c);
	  S2[c].set(r);
	}
    }
  else
    {
      for (size_t i = 0; i < aln.size(); i += 2)
	{
	  size_t r = rspan.first + aln[i+1];
	  size_t c = cspan.first + aln[i];
	  S1[r].set(c);
	  S2[c].set(r);
	}
    }
  // check compatibility with existing alignment
  for (size_t r = rspan.first; r < rspan.second; ++r)
    if (m1[r] && (*m1[r]) != S1[r]) return false;
  for (size_t c = cspan.first; c < cspan.second; ++c)
    if (m2[c] && (*m2[c]) != S2[c]) return false;

  // all good, add new points
  for (size_t r = rspan.first; r < rspan.second; ++r)
    if (!m1[r]) { A1[r] = S1[r]; m1[r] = &A1[r]; }
  for (size_t c = cspan.first; c < cspan.second; ++c)
    if (!m2[c]) { A2[c] = S2[c]; m2[c] = &A2[c]; }

  return true;
}

struct alink
{
  size_t r,c,m;
  bool operator<(alink const& o) const { return m < o.m; }
  bool operator>(alink const& o) const { return m > o.m; }
};

int main(int argc, char* argv[])
{
  string base = argc > 1 ? argv[1] : "crp/trn/mm/";
  string L1   = argc > 1 ? argv[2] : "de";
  string L2   = argc > 1 ? argv[3] : "en";
  BT.open(base,L1,L2);
  BT.V1->setDynamic(true);
  BT.V2->setDynamic(true);
  string line1, line2;
  npmi_scorer1<Token> scorer;
  while (getline(cin,line1) and getline(cin,line2))
    {
      cout << "\n" << line1 << "\n" << line2 << endl;
      vector<Token> snt1,snt2;
      fill_token_seq(*BT.V1,line1,snt1);
      fill_token_seq(*BT.V2,line2,snt2);
      vector<vector<SPTR<vector<PhrasePair<Token> > > > > pt1,pt2;
      vector<vector<uint64_t> > pm1,pm2;
      BT.lookup(snt1,*BT.I1,pt1,&pm1,&scorer);
      BT.lookup(snt2,*BT.I2,pt2,&pm2,&scorer);

      // build map from phrases to positions
      typedef boost::unordered_map<uint64_t, vector<span_t> >
	p2s_map_t;
      typedef p2s_map_t::iterator p2s_iter;
      p2s_map_t p2s1,p2s2;
      for (ushort i = 0; i < pm1.size(); ++i)
	for (ushort k = 0; k < pm1[i].size(); ++k)
	  p2s1[pm1[i][k]].push_back(make_pair(i,i+k+1));
      for (ushort i = 0; i < pm2.size(); ++i)
	for (ushort k = 0; k < pm2[i].size(); ++k)
	  p2s2[pm2[i][k]].push_back(make_pair(i,i+k+1));

      boost::unordered_map<uint64_t,SPTR<vector<PhrasePair<Token> > > > all1,all2;
      vector<PhrasePair<Token> > pp_all;
      for (size_t i = 0; i < pt2.size(); ++i)
	for (size_t k = 0; k < pt2[i].size(); ++k)
	  all2[pm2[i][k]] = pt2[i][k];
      for (size_t i = 0; i < pt1.size(); ++i)
	for (size_t k = 0; k < pt1[i].size(); ++k)
	  {
	    all1[pm1[i][k]] = pt1[i][k];
	    BOOST_FOREACH(PhrasePair<Token> const& pp, *pt1[i][k])
	      {
		if (pp.score < 0) break;
		if (p2s2.find(pp.p2) != p2s2.end())
		  pp_all.push_back(pp);
	      }
	  }
      sort(pp_all.begin(), pp_all.end(), greater<PhrasePair<Token> >());
      vector<int> a1(snt1.size(),-1), a2(snt2.size(),-1);

      vector<bitvector> R(snt1.size(),bitvector(snt2.size()));
      vector<bitvector> C(snt2.size(),bitvector(snt1.size()));
      vector<bitvector> myR(snt1.size(),bitvector(snt2.size()));
      vector<bitvector> myC(snt2.size(),bitvector(snt1.size()));
      vector<bitvector*> m1(snt1.size(),NULL);
      vector<bitvector*> m2(snt2.size(),NULL);

      // vector<vector<AlnPoint> > M(snt1.size(),vector<AlnPoint>(snt2.size()));
      AlnMatrix A(snt1.size(),snt2.size());
      for (size_t p = 0; p < pp_all.size(); ++p)
	{
	  PhrasePair<Token> const& pp = pp_all[p];
#if 0
	  cout << (boost::format("%30s ::: %-30s ")
		   % BT.toString(pp.p1,0).c_str()
		   % BT.toString(pp.p2,1).c_str());
	  cout << (boost::format("%.4f [%d/%d/%d]")
		   % pp.score % pp.good1 % pp.joint % pp.good2);
	  for (size_t a = 0; a < pp.aln.size(); a += 2)
	    cout << " " << int(pp.aln[a]) << "-" << int(pp.aln[a+1]);
	  cout << endl;
#endif

	  vector<span_t>& v1 = p2s1[pp.p1];
	  vector<span_t>& v2 = p2s2[pp.p2];
	  if (v1.size() == 1)
	    for (size_t i = v1[0].first; i < v1[0].second; ++i)
	      if (a1[i] < 0) a1[i] = p;
	  if (v2.size() == 1)
	    for (size_t i = v2[0].first; i < v2[0].second; ++i)
	      if (a2[i] < 0) a2[i] = p;

	  if (v1.size() == 1 && v2.size() == 1)
	    A.incorporate(v1[0],v2[0],pp.aln,pp.inverse);
	}

      for (size_t i = 0; i < A.size(); ++i)
	{
	  cout << (*BT.V1)[snt1[i].id()] << ": ";
	  for (size_t k=A[i].find_first(); k < A[i].size(); k=A[i].find_next(k))
	    cout << boost::format(" %d:%s") % k % (*BT.V2)[snt2[k].id()];
	  cout << endl;
	}

      vector<PhrasePair<Token> > const* atrans, *btrans;
      ahyp h;
      vector<ahyp> hyps;
      vector<vector<int> > L(snt1.size(),vector<int>(snt2.size(),0));
      // L: matches by letter overlap

      for (h.s1 = 0; h.s1 < a1.size(); ++h.s1)
	{
	  if (a1[h.s1] >= 0) continue;
	  ostringstream buf1;
	  for (h.e1 = h.s1; h.e1 < a1.size() && a1[h.e1] < 0; ++h.e1)
	    {
	      if (h.e1 > h.s1)
		{
		  if (pt1[h.s1].size() + h.s1 <= h.e1) break;
		  buf1 << " ";
		}
	      buf1 << (*BT.V1)[snt1[h.e1].id()];
	      atrans = pt1[h.s1].size() ? pt1[h.s1].at(h.e1-h.s1).get() : NULL;
	      for (h.s2 = 0; h.s2 < a2.size(); ++h.s2)
		{
		  ostringstream buf2;
		  if (a2[h.s2] >= 0) continue;
		  for (h.e2 = h.s2; h.e2 < a2.size() && a2[h.e2] < 0; ++h.e2)
		    {
		      if (h.e2 > h.s2)
			{
			  if (pt2[h.s2].size() + h.s2 <= h.e2) break;
			  buf2 << " ";
			}
		      buf2 << (*BT.V2)[snt2[h.e2].id()];
		      btrans = (pt2[h.s2].size()
				? pt2[h.s2].at(h.e2-h.s2).get()
				: NULL);

		      vector<vector<int> > aln;
		      trymatch2(*BT.V1, *BT.V2, buf1.str(),buf2.str(),
				atrans,aln);
		      for (size_t i = 0; i < aln.size(); ++i)
			for (size_t k = 0; k < aln[i].size(); ++k)
			  L[h.s1+i][h.s2+k] = max(L[h.s1+i][h.s2+k],aln[i][k]);
		      trymatch2(*BT.V2, *BT.V1, buf2.str(),buf1.str(),
				btrans,aln);
		      for (size_t i = 0; i < aln[0].size(); ++i)
			for (size_t k = 0; k < aln.size(); ++k)
			  L[h.s1+i][h.s2+k] = max(L[h.s1+i][h.s2+k],aln[k][i]);
		      // h.score = trymatch(buf1.str(), buf2.str(), atrans, btrans);
		      // hyps.push_back(h);
		    }
		}
	    }
	}

      vector<alink> links;

      alink x;
      for (x.r = 0; x.r < L.size(); ++x.r)
	{

	for (x.c = 0; x.c < L[x.r].size(); ++x.c)
	  {
	    x.m = L[x.r][x.c];
	    if (x.m) links.push_back(x);
	  }
	}

      sort(links.begin(),links.end(),greater<alink>());

      BOOST_FOREACH(alink& x, links)
	{
	  if (L[x.r][x.c])
	    {
	      cout << (*BT.V1)[snt1[x.r].id()] << " ::: "
		   << (*BT.V2)[snt2[x.c].id()] << " ::: "
		   << L[x.r][x.c] << endl;
	    }
	}

      // sort(hyps.begin(),hyps.end(),greater<ahyp>());
      // BOOST_FOREACH(ahyp const& h, hyps)
      // 	{
      // 	  if (h.score < .5) break;
      // 	  for (size_t i = h.s1; i <= h.e1; ++i)
      // 	    cout << i << ":" << (*BT.V1)[snt1[i].id()] << " ";
      // 	  cout << " ::: ";
      // 	  for (size_t i = h.s2; i <= h.e2; ++i)
      // 	    cout << i << ":" << (*BT.V2)[snt2[i].id()] << " ";
      // 	  cout << h.score << endl;
      // 	}

    }
}

//       for (size_t i = 0; i < pt1.size(); ++i)
// 	{
// 	  for (size_t k = 0; k < pt1[i].size(); ++k)
// 	    {
// 	      size_t d1 = 0;
// 	      bool first = true;
// 	      BOOST_FOREACH(PhrasePair<Token> const& pt, *pt1[i][k])
// 		{
// 		  TSA<Token>::tree_iterator m(BT.I2.get(),pt.start2,pt.len2);
// 		  if (pt.score < 0) break;
// 		  int left = pt.aln[1], right = pt.aln[1];
// 		  bool match = p2s2.find(m.getPid()) != p2s2.end();
// 		  if (!match)
// 		    {
// 		      for (size_t a = 3; a < pt.aln.size(); a += 2)
// 			{
// 			  if (left  > pt.aln[a]) left  = pt.aln[a];
// 			  if (right < pt.aln[a]) right = pt.aln[a];
// 			}
// 		    }
// #if 0
// 		  if (match)
// 		    {
// 		      if (first)
// 			{
// 			  cout << BT.toString(pm1[i][k],0) << endl;
// 			  first = false;
// 			}
// 		      cout << boost::format("%.4f") % pt.score << " "
// 			   << setw(5) << d1 << " " << (match ? "* " : "  ")
// 			   << toString(*BT.V2, pt.start2, pt.len2) << " ["
// 			   << pt.good1 << "/" << pt.joint << "/"
// 			   << pt.good2 << "]";
// 		      for (size_t a = 0; a < pt.aln.size(); a += 2)
// 			cout << " " << int(pt.aln[a]) << "-" << int(pt.aln[a+1]);
// 		      cout << " [" << left << ":" << right << "]" << endl;
// 		    }
// #endif
// 		  if (!match)
// 		    {
// 		      if (left == 0 && pt.len2 - right == 1)
// 			d1 += pt.joint;
// 		    }
// 		  else
// 		    {
// 		      pp_all.push_back(pt);
// 		      // pp_all.back().m1 -= d1;
// 		    }

// 		}
// 	      if (!first) cout << endl;
// 	    }


#include <string>
#include <boost/shared_ptr.hpp>

#include <sophus/se3.h>
#include <Eigen/Eigen>
#include <opencv2/highgui/highgui.hpp>

#include "param_reader.h"
#include "pinhole_camera.h"
#include "detect_features.h"
#include "align_image.h"
#include "generate_point_cloud.h"
#include "map_point.h"
#include "map.h"
#include "image_reader.h"
#include "visual_odometry.h"

VisualOdometry::VisualOdometry(ParameterReader::Ptr param_reader):
state_(INITIALIZING),
local_map_(new Map),
cam_(new PinHoleCamera(param_reader)),
detector_(new DetectFeatures(param_reader)),
align_image_(new AlignImage(cam_, param_reader)),
point_cloud_generator_(new PointCloudGenerator(cam_, param_reader)),
min_keyframe_rotate_(param_reader->getParam<double>("min_keyframe_rotate")),
min_keyframe_translate_(param_reader->getParam<double>("min_keyframe_translate")),
map_point_erase_ratio_(param_reader->getParam<double>("map_point_erase_ratio")),
max_num_lost_(param_reader->getParam<double>("max_num_lost"))
{

}

VisualOdometry::~VisualOdometry()
{
	// 因为使用智能指针,所以析构函数中不需要做内存释放工作
}

bool VisualOdometry::addImage(cv::Mat& rgb_img, cv::Mat& depth_img)
{
	switch (state_)
	{
		case INITIALIZING: {
			curr_frame_ = Frame::createFrame(cam_, rgb_img.clone(), depth_img.clone());
			curr_frame_->T_c2w_ = Sophus::SE3( Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero() );
			ref_frame_ = ref_keyframe_  = curr_frame_;
			detector_->detectFeaturePoints(curr_frame_);		// 计算关键点和描述子
			local_map_->insertKeyFrame(curr_frame_);		// 第一帧必然是关键帧
			addMapPoint();								// 向地图中添加3D点
			point_cloud_generator_->joinPointCloud(curr_frame_);
			state_ = TRACKING;
			break;
		}
		case TRACKING: {
			curr_frame_ = Frame::createFrame(cam_, rgb_img.clone(), depth_img.clone());
			curr_frame_->T_c2w_ = ref_frame_->T_c2w_;	// 用相邻帧的位姿来近似当前帧位姿
			detector_->detectFeaturePoints(curr_frame_);
			align_image_->alignMapFrame(local_map_, curr_frame_, ref_frame_);
			if (true == checkEstimatedPose()) {

				 num_lost_ = 0;

				 optimizeMap();

				if (true == checkKeyFrame()) {
					local_map_->insertKeyFrame(curr_frame_);
					ref_keyframe_ = curr_frame_;
				}
				point_cloud_generator_->joinPointCloud(curr_frame_);
				ref_frame_ = curr_frame_;
			}
			else {
				 num_lost_++;
				 if ( num_lost_ > max_num_lost_ ) {
					state_ = LOST;
				}

				curr_frame_->T_c2w_ = ref_frame_->T_c2w_;	// 若估计结果出错,则不更新当前帧的位姿
			}
			break;
		}
		case LOST: {
			cout<<"vo has lost."<<endl;
			break;
		}
	}
}

void VisualOdometry::addMapPoint()
{
	int add_num = 0;

	if (INITIALIZING == state_) {
		// 初始化阶段,将第一帧的所有特征点都插入到地图中
		for (size_t i=0; i<curr_frame_->keypoints_.size(); i++ ) {
			double d = curr_frame_->findDepth(curr_frame_->keypoints_[i]);
			if(d < 0)
				continue;
			Eigen::Vector2d pt_img(curr_frame_->keypoints_[i].pt.x, curr_frame_->keypoints_[i].pt.y);
			Eigen::Vector3d pt_world = curr_frame_->cam_->pixel2world(pt_img, curr_frame_->T_c2w_.inverse(), d);
			Eigen::Vector3d n = pt_world - ref_frame_->getCameraCenter();
			n.normalize();
			MapPoint::Ptr map_point = MapPoint::createMapPoint(pt_world, n, curr_frame_->descrip_.row(i).clone());
			local_map_->insertMapPoint(map_point);
			add_num++;
		}
	}
	else if (TRACKING == state_) {
		// 正常的tracking阶段,仅将没有匹配的点添加到地图中
		vector<bool> matched(curr_frame_->keypoints_.size(), false);
		for ( int index : align_image_->match_2dkp_index_ )
			matched[index] = true;
		for ( int i=0; i<curr_frame_->keypoints_.size(); i++ )
		{
			// 如果在匹配过程中,存在良好的匹配,则认为已经存在于地图中,不在重复添加
			if ( matched[i] == true )
				continue;
			double d = curr_frame_->findDepth ( curr_frame_->keypoints_[i]);
			if ( d<0 )
				continue;
			Eigen::Vector2d pt_img(curr_frame_->keypoints_[i].pt.x, curr_frame_->keypoints_[i].pt.y);
			Eigen::Vector3d pt_world = curr_frame_->cam_->pixel2world(pt_img, curr_frame_->T_c2w_.inverse(), d);

			Eigen::Vector3d n = pt_world - curr_frame_->getCameraCenter();
			n.normalize();
			MapPoint::Ptr map_point = MapPoint::createMapPoint(pt_world, n, curr_frame_->descrip_.row(i).clone());
			local_map_->insertMapPoint(map_point);
			add_num++;
		}
	}
	else {
		// TODO
	}

	cout << "add " << add_num << " points to map" << endl;
	return;
}

// 检查当前帧与上一关键帧之间的运动量来确定是否将其作为关键帧
bool VisualOdometry::checkKeyFrame()
{
	Sophus::SE3 T_c2r = ref_keyframe_->T_c2w_.inverse() * curr_frame_->T_c2w_;

	Sophus::Vector6d d = T_c2r.log();
	Eigen::Vector3d trans = d.head<3>();
	Eigen::Vector3d rot = d.tail<3>();
	if ( rot.norm() >min_keyframe_rotate_ || trans.norm() >min_keyframe_translate_ )
		return true;

	return false;
}

bool VisualOdometry::checkEstimatedPose()
{
    // 检查运动估计过程是否出错
    if (false == align_image_->checkAlignQuality()) {
        cout<<"reject because bad align: "<<endl;
        return false;
    }

    // 检查估计结果是否正常,太大则认为出错
    Sophus::SE3 T_c2r = ref_frame_->T_c2w_.inverse() * curr_frame_->T_c2w_;
    Sophus::Vector6d d = T_c2r.log();
    if ( d.norm() > 5.0 ) {
        cout<<"reject because motion is too large: "<<d.norm() <<endl;
        return false;
    }

    return true;
}

void VisualOdometry::optimizeMap()
{
	int i(0), j(0), k(0);
	// 遍历所有地图点,去掉不合格的点
	for(auto iter = local_map_->map_points_.begin(); iter != local_map_->map_points_.end(); ) {
		// 删除未出现在当前帧中的地图点
		if (!curr_frame_->isInFrame(iter->second->pose_)){
			iter = local_map_->map_points_.erase(iter);
			i++;
			continue;
		}
		// 删除匹配率过低的点
		float match_ratio = float(iter->second->matched_times_) / iter->second->observed_times_;
		if ( match_ratio < map_point_erase_ratio_ )
		{
			iter = local_map_->map_points_.erase(iter);
			j++;
			continue;
		}
		// 删除当前观测角度与初始观测角度变化太大的地图点
		// (因为视角变化太大会造成特征点外观变化明显,不利于匹配?)
		double angle = getViewAngle( curr_frame_, iter->second );
		if ( angle > M_PI/6. )
		{
			iter = local_map_->map_points_.erase(iter);
			k++;
			continue;
		}
// 		if ( iter->second->good_ == false )
// 		{
// 			// TODO try triangulate this map point
// 		}
		iter++;
	}
	cout << "i: " << i << "j: " << j <<" k: " << k <<endl;

	// 地图点与当前帧匹配过少时,将当前帧中未匹配的点加到地图中
	if (align_image_->match_2dkp_index_.size() < 30 || align_image_->inliers_num_ < 20) {
		addMapPoint();
	}

	// 地图点过多时,降低筛选门限,去除更多的点
	if (local_map_->map_points_.size() > 800) {
		map_point_erase_ratio_ += 0.05;
	}
	else {
		map_point_erase_ratio_ = 0.1;
	}
	cout << "map_point_erase_ratio: " << map_point_erase_ratio_ << endl;
	cout << "map size: " << local_map_->map_points_.size() << endl;

	return;
}

double VisualOdometry::getViewAngle ( Frame::Ptr frame, MapPoint::Ptr map_point )
{
	Eigen::Vector3d n = map_point->pose_ - frame->getCameraCenter();
	n.normalize();
	return acos( n.transpose() * map_point->norm_observe_direct_ );
}


//
//  cal_fe_global_const_ke_bimat.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/7/18.
//
//

#ifndef cal_fe_global_const_ke_bimat_hpp
#define cal_fe_global_const_ke_bimat_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "maplocal.hpp"
#include "mapglobal.hpp"

using namespace Eigen;

typedef Eigen::Matrix<int, -1, -1,RowMajor> MatrixXi_rm;

void cal_fe_global_const_ke_bimat(MatrixXd &Node, MatrixXi_rm &Element,int n_nodes, int n_el, MatrixXi &index_store, double q, VectorXd &u_n, VectorXd &v_n, int Ndofn, MatrixXd ke_1, MatrixXd ke_2, VectorXd &fe_global);

#endif /* cal_fe_global_const_ke_bimat_hpp */


#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_typedefs.hpp>
#include <map>
#include <vector>

#ifndef UR_10_FK_FAST_HPP
#define UR_10_FK_FAST_HPP

namespace UR_10_FK_FAST {
const size_t UR_10_NUM_ACTIVE_JOINTS = 6;
const size_t UR_10_NUM_LINKS = 7;

const std::string UR_10_ACTIVE_JOINT_1_NAME = "shoulder_pan_joint";
const std::string UR_10_ACTIVE_JOINT_2_NAME = "shoulder_lift_joint";
const std::string UR_10_ACTIVE_JOINT_3_NAME = "elbow_joint";
const std::string UR_10_ACTIVE_JOINT_4_NAME = "wrist_1_joint";
const std::string UR_10_ACTIVE_JOINT_5_NAME = "wrist_2_joint";
const std::string UR_10_ACTIVE_JOINT_6_NAME = "wrist_3_joint";

const std::string UR_10_LINK_1_NAME = "base_link";
const std::string UR_10_LINK_2_NAME = "shoulder_link";
const std::string UR_10_LINK_3_NAME = "upper_arm_link";
const std::string UR_10_LINK_4_NAME = "forearm_link";
const std::string UR_10_LINK_5_NAME = "wrist_1_link";
const std::string UR_10_LINK_6_NAME = "wrist_2_link";
const std::string UR_10_LINK_7_NAME = "wrist_3_link";

inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1273);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.220941, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, M_PI_2, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, -0.1719, 0.612);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.5723);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, M_PI_2, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.1149, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1157);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  assert(configuration.size() == UR_10_NUM_ACTIVE_JOINTS);
  EigenHelpers::VectorIsometry3d link_transforms(UR_10_NUM_LINKS);
  link_transforms[0] = base_transform;
  link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
  link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
  link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
  link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
  link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
  link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
  return link_transforms;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  std::vector<double> configuration_vector(UR_10_NUM_ACTIVE_JOINTS);
  configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_1_NAME, 0.0);
  configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_2_NAME, 0.0);
  configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_3_NAME, 0.0);
  configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_4_NAME, 0.0);
  configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_5_NAME, 0.0);
  configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, UR_10_ACTIVE_JOINT_6_NAME, 0.0);
  return GetLinkTransforms(configuration_vector, base_transform);
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[UR_10_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[UR_10_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[UR_10_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[UR_10_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[UR_10_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[UR_10_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[UR_10_LINK_7_NAME] = link_transforms[6];
  return link_transforms_map;
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[UR_10_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[UR_10_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[UR_10_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[UR_10_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[UR_10_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[UR_10_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[UR_10_LINK_7_NAME] = link_transforms[6];
  return link_transforms_map;
}
}  // namespace UR_10_FK_FAST

#endif  // UR_10_FK_FAST_HPP


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/bitfloating.hpp>
#include <boost/simd/function/bitinteger.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/is_nan.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( successor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
       return saturated_(inc)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( successor_
                          , (typename A0)
                             , bd::cpu_
                                    , bd::scalar_< bd::floating_<A0> >
                                    )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      if (is_nan(a0)) return a0;
      return bitfloating(saturated_(inc)(bitinteger(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( successor_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      if (Valmax<A0>()-a1 < a0) return Valmax<A0>();
      return a0+a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( successor_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      using itype = bd::as_integer_t<A0, signed>;
      if (is_nan(a0)) return a0;
      return bitfloating(saturated_(plus)(bitinteger(a0), itype(a1)));
    }
  };
} } }

#endif