#include "utils.h"

#include <fstream>
#include <iostream>
#include <ctime>

#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>

auto random_engine = std::mt19937(std::time(0));

void train_with_features(std::string data_file_path,
                         std::string features_file_path,
                         std::string noise_file_path,
                         int n_steps, double eta_0, double iter_power,
                         size_t l_dimensions, size_t k_dimensions,
                         std::string output_file_path,
                         std::string objective_output_file_path) {
    // Data loading
    std::fstream data_file_input;
    data_file_input.open(data_file_path, std::ios::in);
    std::string line;
    std::vector<std::string> tokens;
    getline(data_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t data_size = stoul(tokens[0]);
    size_t n_samples = stoul(tokens[1]);

    std::vector<size_t> start_indexes(n_samples);
    std::vector<size_t> permutation(n_samples);
    std::vector<int> data(data_size + n_samples);
    long n_data = 0;
    long n_noise = 0;
    for (size_t i = 0, data_index = 0; i < n_samples; ++i) {
        getline(data_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        start_indexes[i] = data_index;
        permutation[i] = i;
        for (size_t j = 0; j < tokens.size(); ++j) {
            data[data_index + j] = stoi(tokens[j]);
        }
        if (data[data_index] == 0) {
            ++n_noise;
        } else {
            ++n_data;
        }
        data[data_index + tokens.size()] = -1;
        data_index += tokens.size() + 1;
    }

    size_t n_items;
    size_t m_features;
    std::vector<double> features;
    auto index_features = [&m_features](size_t i, size_t j) -> size_t {
        return (i * m_features) + j;
    };
    masterthesis::readFeatureFile(
            features_file_path, &n_items, &m_features, &features);

    std::fstream noise_file_input;
    noise_file_input.open(noise_file_path, std::ios::in);
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    std::vector<double> a_weights(m_features);
    for (size_t i = 0; i < m_features; ++i) {
        a_weights[i] = stod(tokens[i]);
    }
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    std::vector<double> noise_utilities(n_items);
    for (size_t i = 0; i < n_items; ++i) {
        noise_utilities[i] = stod(tokens[i]);
    }

    // Calculate constant quantities.
    double nu = n_noise / n_data;
    double log_nu = log(nu);
    double logz_noise = 0;
    for (size_t i = 0; i < n_items; ++i) {
        logz_noise += masterthesis::log1exp(noise_utilities[i]);
    }

    // Initialize parameters.
    std::uniform_real_distribution<double_t> udouble_dist(0, 1e-3);
    std::vector<double> b_weights(m_features * l_dimensions);
    std::vector<double> c_weights(m_features * k_dimensions);
    double n_logz = 0;

    auto index_b_weights = [&l_dimensions](size_t i, size_t j) -> size_t {
        return (i * l_dimensions) + j;
    };

    auto index_c_weights = [&k_dimensions](size_t i, size_t j) -> size_t {
        return (i * k_dimensions) + j;
    };

    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions; ++j) {
            b_weights[index_b_weights(i, j)] = udouble_dist(random_engine);
        }
        for (size_t j = 0; j < k_dimensions; ++j) {
            c_weights[index_c_weights(i, j)] = udouble_dist(random_engine);
        }
    }
    for (size_t i = 0; i < n_items; ++i) {
        double item_util = 0;
        for (size_t j = 0; j < m_features; ++j) {
            item_util += a_weights[j] * features[index_features(i, j)];
        }
        n_logz -= masterthesis::log1exp(item_util);
    }
    std::cout << logz_noise << " " << n_logz << std::endl;

    std::vector<double> a_gradient(m_features);
    std::vector<double> objectives(n_steps);
    for (size_t iter = 0; iter < n_steps; ++iter) {
        double objective = 0;
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            size_t start_idx = start_indexes[sub_iter];
            size_t end_idx;
            if (sub_iter == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[sub_iter + 1] - 1;
            }
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;
            for (size_t j = 0; j < m_features; ++j) {
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    p_model += features[index_features(data[i], j)] *
                               a_weights[j];
                }
            }
            for (size_t i = start_idx + 1; i < end_idx; ++i) {
                p_noise += noise_utilities[data[i]];
            }
            for (size_t j = 0; j < l_dimensions; ++j) {
                double max_b_weight = -1;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    double weight = 0;
                    for (size_t k = 0; k < m_features; ++k) {
                        weight += features[index_features(data[i], k)] *
                                  b_weights[index_b_weights(k, j)];
                    }
                    if (weight > max_b_weight) {
                        max_b_weight = weight;
                    }
                    p_model -= weight;
                }
                if (max_b_weight >= 0) {
                    p_model += max_b_weight;
                }
            }
            for (size_t j = 0; j < k_dimensions; ++j) {
                double max_c_weight = -1;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    double weight = 0;
                    for (size_t k = 0; k < m_features; ++k) {
                        weight += features[index_features(data[i], k)] *
                                  c_weights[index_c_weights(k, j)];
                    }
                    if (weight > max_c_weight) {
                        max_c_weight = weight;
                    }
                    p_model += weight;
                }
                if (max_c_weight >= 0) {
                    p_model -= max_c_weight;
                }
            }
            if (label == 1) {
                objective -= masterthesis::log1exp(log_nu + p_noise - p_model);
            } else {
                objective -= masterthesis::log1exp(p_model - log_nu - p_noise);
            }
        }
        std::cout << objective << std::endl;
        objectives[iter] = objective;
        shuffle(begin(permutation), end(permutation), random_engine);
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            size_t start_idx = start_indexes[permutation[sub_iter]];
            size_t end_idx;
            if (permutation[sub_iter] == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[permutation[sub_iter] + 1] - 1;
            }
            bool isEmpty = (start_idx == end_idx - 1);
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;
            for (size_t j = 0; j < m_features; ++j) {
                a_gradient[j] = 0;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    a_gradient[j] += features[index_features(data[i], j)];
                    p_model += features[index_features(data[i], j)] *
                               a_weights[j];
                }
            }
            for (size_t i = start_idx + 1; i < end_idx; ++i) {
                p_noise += noise_utilities[data[i]];
            }

            std::vector<double> max_b_weights(l_dimensions);
            std::vector<int> max_weight_b_indexes(l_dimensions);
            std::vector<double> max_c_weights(k_dimensions);
            std::vector<int> max_weight_c_indexes(k_dimensions);
            if (!isEmpty) {
                for (size_t j = 0; j < l_dimensions; ++j) {
                    max_b_weights[j] = -1;
                    max_weight_b_indexes[j] = -1;
                    for (size_t i = start_idx + 1; i < end_idx; ++i) {
                        double weight = 0;
                        for (size_t k = 0; k < m_features; ++k) {
                            weight += features[index_features(data[i], k)] *
                                      b_weights[index_b_weights(k, j)];
                        }
                        if (weight > max_b_weights[j]) {
                            max_b_weights[j] = weight;
                            max_weight_b_indexes[j] = data[i];
                        }
                        p_model -= weight;
                    }
                    p_model += max_b_weights[j];
                }

                for (size_t j = 0; j < k_dimensions; ++j) {
                    max_c_weights[j] = -1;
                    max_weight_c_indexes[j] = -1;
                    for (size_t i = start_idx + 1; i < end_idx; ++i) {
                        double weight = 0;
                        for (size_t k = 0; k < m_features; ++k) {
                            weight += features[index_features(data[i], k)] *
                                      c_weights[index_c_weights(k, j)];
                        }
                        if (weight > max_c_weights[j]) {
                            max_c_weights[j] = weight;
                            max_weight_c_indexes[j] = data[i];
                        }
                        p_model += weight;
                    }
                    p_model -= max_c_weights[j];
                }
            }

            double learning_rate = eta_0 *
                                   pow((iter * n_samples) + sub_iter + 1, -iter_power);
            double factor = learning_rate *
                            (label - masterthesis::expit(p_model - p_noise - log_nu));
            if (!isEmpty) {
                for (size_t i = 0; i < m_features; ++i) {
                    a_weights[i] += factor * a_gradient[i];
                    for (size_t j = 0; j < l_dimensions; ++j) {
                        b_weights[index_b_weights(i, j)] += factor *
                                                            (features[index_features(
                                                                    max_weight_b_indexes[j],
                                                                    i)] -
                                                             a_gradient[i]);
                        if (b_weights[index_b_weights(i, j)] <= 0) {
                            b_weights[index_b_weights(i, j)] = udouble_dist(random_engine);
                        }
                    }
                    for (size_t j = 0; j < k_dimensions; ++j) {
                        c_weights[index_c_weights(i, j)] += factor *
                                                            (a_gradient[i] -
                                                             features[index_features(
                                                                     max_weight_c_indexes[j],
                                                                     i)]);
                        if (c_weights[index_c_weights(i, j)] <= 0) {
                            c_weights[index_c_weights(i, j)] = udouble_dist(random_engine);
                        }
                    }
                }
            }
            n_logz += factor;
        }
    }

    std::fstream output_file(output_file_path, std::ios::out);

    output_file << n_logz << std::endl;
    for (size_t i = 0; i < m_features - 1; ++i) {
        output_file << a_weights[i] << ",";
    }
    output_file << a_weights[m_features - 1] << std::endl;

    for (size_t i = 0; i < m_features; ++i) {
        if (l_dimensions > 0) {
            for (size_t j = 0; j < l_dimensions - 1; ++j) {
                output_file << b_weights[index_b_weights(i, j)] << ",";
            }
            output_file << b_weights[index_b_weights(i, l_dimensions - 1)] << std::endl;
        }
    }

    for (size_t i = 0; i < m_features; ++i) {
        if (k_dimensions > 0) {
            for (size_t j = 0; j < k_dimensions - 1; ++j) {
                output_file << c_weights[index_c_weights(i, j)] << ",";
            }
            output_file << c_weights[index_c_weights(i, k_dimensions - 1)] << std::endl;
        }
    }

    output_file.close();

    std::fstream objective_output_file(
            objective_output_file_path, std::ios::out);

    for (size_t i = 0; i < n_steps; ++i) {
        objective_output_file << objectives[i] << std::endl;
    }

    objective_output_file.close();
}

int main(int argc, char* argv[]) {
    int fold_number = std::stoi(argv[1]);
    int l_dimensions = std::stoi(argv[2]);
    int k_dimensions = std::stoi(argv[3]);
    char* feature_set = argv[4];
    char* dataset_name = argv[5];
    int iterations = std::stoi(argv[6]);
    std::vector<double> times(fold_number);
    for (int i = 1; i <= fold_number; ++i) {
        time_t start = time(0);
        train_with_features(
                (boost::format(
                        "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_data_features_%2%_fold_%3%.csv") %
                 dataset_name % feature_set % i).str(),
                (boost::format(
                        "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_features_%2%.csv") %
                 dataset_name % feature_set).str(),
                (boost::format(
                        "/home/diegob/workspace/master-thesis-2015/data/path_set_%1%_nce_noise_features_%2%_fold_%3%.csv") %
                 dataset_name % feature_set % i).str(),
                iterations, 0.00005, 0.1,
                static_cast<size_t>(l_dimensions),
                static_cast<size_t>(k_dimensions),
                (boost::format(
                        "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_out_features_%2%_l_dim_%3%_k_dim_%4%_fold_%5%.csv") %
                 dataset_name % feature_set % l_dimensions % k_dimensions % i).str(),
                (boost::format(
                        "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_objective_features_%2%_l_dim_%3%_k_dim_%4%_fold_%5%.csv") %
                 dataset_name % feature_set % l_dimensions % k_dimensions % i).str()
        );
        time_t end = time(0);
        double time = std::difftime(end, start);
        times[i-1] = time;
        std::cout << "Fold finished." << std::endl;
        std::cout << "Fold took: " << times[i-1] << "s" << std::endl;
    }
    std::fstream times_output_file(
            (boost::format(
                    "/home/diegob/workspace/master-thesis-2015/data/models/path_set_%1%_nce_timing_features_%2%_l_dim_%3%_k_dim_%4%.csv") %
             dataset_name % feature_set % l_dimensions % k_dimensions).str(),
            std::ios::out);

    for (size_t i = 0; i < fold_number; ++i) {
        times_output_file << times[i] << std::endl;
    }
    times_output_file.close();
}


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

// Copyright (c) 2007-2012 Barend Gehrels, 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_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP

#include <vector>

#include <boost/range.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/union.hpp>

#include <boost/geometry/extensions/algorithms/dissolve.hpp>
#include <boost/geometry/extensions/algorithms/detail/overlay/dissolver.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace dissolve
{

template <typename Multi, typename GeometryOut>
struct dissolve_multi
{
    template <typename RescalePolicy, typename OutputIterator>
    static inline OutputIterator apply(Multi const& multi, RescalePolicy const& rescale_policy, OutputIterator out)
    {
        typedef typename boost::range_value<Multi>::type polygon_type;
        typedef typename boost::range_iterator<Multi const>::type iterator_type;

        // Step 1: dissolve all polygons in the multi-polygon, independantly
        std::vector<GeometryOut> step1;
        for (iterator_type it = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            dissolve_ring_or_polygon
                <
                    polygon_type,
                    GeometryOut
                >::apply(*it, rescale_policy, std::back_inserter(step1));
        }

        // Step 2: remove mutual overlap
        {
            std::vector<GeometryOut> step2; // TODO avoid this, output to "out", if possible
            detail::dissolver::dissolver_generic<detail::dissolver::plusmin_policy>::apply(step1, rescale_policy, step2);
            for (typename std::vector<GeometryOut>::const_iterator it = step2.begin();
                it != step2.end(); ++it)
            {
                *out++ = *it;
            }
        }

        return out;
    }
};

// Dissolving multi-linestring is currently moved to extensions/algorithms/connect,
// because it is actually different from dissolving of polygons.
// To be decided what the final behaviour/name is.

}} // namespace detail::dissolve
#endif

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template<typename Multi, typename GeometryOut>
struct dissolve<multi_polygon_tag, polygon_tag, Multi, GeometryOut>
    : detail::dissolve::dissolve_multi<Multi, GeometryOut>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_MULTI_ALGORITHMS_DISSOLVE_HPP


#include "radial_wave.hpp"
#include <boost/numeric/conversion/cast.hpp>

using namespace init_cond;

// ---------------------------------------------------------------------------------------------------------------------
//                                                  radial wave 2d
// ---------------------------------------------------------------------------------------------------------------------

RadialWave2D::RadialWave2D(std::size_t dim) :
    InitialConditionGenerator(dim, 1, "radial"),
    mStartingPos(dim)
{
    if(dim < 2)
        THROW_EXCEPTION(std::logic_error, "Radial two dimensional initial condition requires at least two dimensional "
                "world, got %1%", dim);

    /// \todo this should depend on the world support.
    for(unsigned i = 0; i < mWorldDimension; ++i)
        mStartingPos[i] = 0.5;
}

void RadialWave2D::generate(gen_vect& ray_position, gen_vect& ray_velocity, const manifold_pos& position) const
{
    double param = position[0];

    // set starting pos
    ray_position = mStartingPos;

    // set starting velocity
    ray_velocity[0] = std::sin(param * 2*pi);
    ray_velocity[1] = std::cos(param * 2*pi);
}

void RadialWave2D::setOrigin(const gen_vect& origin)
{
    if(origin.size() != mWorldDimension)
        THROW_EXCEPTION( std::invalid_argument, "%1% dimensional origin supplied for %2% dimensional world",
                         origin.size(), getWorldDimension() );
    mStartingPos = origin;
}

const gen_vect& RadialWave2D::getOrigin() const {
    return mStartingPos;
}

// ---------------------------------------------------------------------------------------------------------------------
//                                                  radial wave 3d
// ---------------------------------------------------------------------------------------------------------------------

RadialWave3D::RadialWave3D(std::size_t dim) :
    InitialConditionGenerator(dim, 2, "radial_3d")
{
    // does not work in 2d
    if(dim != 3)
        THROW_EXCEPTION( std::invalid_argument, "RadialWave3D three dimensions, got %1%", dim );

    for(unsigned i = 0; i < mWorldDimension; ++i)
        mStartingPos[i] = 0.5;
}

void RadialWave3D::init_generator(MultiIndex& manifold_index)
{
    // whole surface: 4pi -> "area per particle" 4 pi / N
    // if small -> assume square: edge length: sqrt( 4 pi / N )
    mStepSize = std::sqrt( 4 * pi / getParticleCount() );

    // half circle: pi / s => NUM OF STEPS [pi/2, because -RNG ... RNG]
    auto range = boost::numeric_cast<int>(std::ceil(pi / mStepSize));

    manifold_index.setUpperBoundAt(0, range);
    manifold_index.setUpperBoundAt(1, 1);

    std::cout << "azimuth range: " << range << "\n";
}

/// generate next IC
void RadialWave3D::generate(gen_vect& ray_position, gen_vect& ray_velocity, const manifold_pos& params) const
{
    double theta = (2*params[0] - 1) * pi / 2;
    // azimuth: always full great circle
    double phi = params[1] * 2 * pi;

    // set starting pos
    ray_position = mStartingPos;

    ray_velocity[0] = std::cos( theta ) * std::sin( phi );
    ray_velocity[1] = std::cos( theta ) * std::cos( phi );
    ray_velocity[2] = std::sin( theta );
}

void RadialWave3D::setOrigin(const gen_vect& origin)
{
    if(origin.size() != mWorldDimension)
        THROW_EXCEPTION( std::invalid_argument, "%1% dimensional origin supplied for %2% dimensional world",
                         origin.size(), getWorldDimension() );
    mStartingPos = origin;
}

void RadialWave3D::next_trajectory(const manifold_pos& pos, MultiIndex& index)
{
    double theta = (2 * pos[0] - 1) * pi / 2;
    double circumference = std::cos(theta) * 2 * pi;

    // for the start of each new row, recalculate the upper iteration bound.
    if(index[getManifoldDimension()-1] == 0)
    {
        index.setUpperBoundDynamic(1, boost::numeric_cast<int>(std::ceil(circumference / mStepSize)));
    }
}

const gen_vect& RadialWave3D::getOrigin() const {
    return mStartingPos;
}

// ---------------------------------------------------------------------------------------------------------------------
//                                             random radial wave
// ---------------------------------------------------------------------------------------------------------------------
RandomRadial::RandomRadial(std::size_t dim) :
        InitialConditionGenerator(dim, dim-1, "random_radial"),
        initial_position(dim),
        initial_angle(dim-1),
        fixed_angle(dim-1)
{
    //! \todo Get Value from c'tor
    for(auto& val : fixed_angle)
        val = -1;
}

void RandomRadial::next_trajectory(const manifold_pos& pos, MultiIndex&)
{
    manifold_start = pos;
    // position
    for(unsigned i = 0; i < mWorldDimension; ++i)
    {
        initial_position[i] = distribution( random_engine );
    }

    // angle
    if( mWorldDimension == 2 )
    {
        initial_angle[0] = distribution( random_engine ) * 2 * pi;
    }
    else if (mWorldDimension == 3 )
    {
        // http://mathworld.wolfram.com/SpherePointPicking.html
        double u = distribution( random_engine );
        double v = distribution( random_engine );
        initial_angle[0] = u * 2 * pi;
        initial_angle[1] = std::acos(2*v - 1)    ;
    }

    // set fixed angle
    for(unsigned i = 0; i < fixed_angle.size(); ++i)
        if( fixed_angle[i] >= 0)
            initial_angle[i] = fixed_angle[i];
}

void
RandomRadial::generate(gen_vect& ray_position, gen_vect& ray_velocity, const manifold_pos& manifold_position) const
{
    ray_position = initial_position;
    // angle to direction
    if( mWorldDimension == 3 )
    {
        double phi = initial_angle[0] + (manifold_position[1] - manifold_start[1]) * 2 * pi;
        ray_velocity[0] = std::cos(phi);
        ray_velocity[1] = std::sin(phi);

        double theta = initial_angle[1] + (manifold_position[0] - manifold_start[0]) * pi;
        ray_velocity[0] *= std::sin(theta);
        ray_velocity[1] *= std::sin(theta);
        ray_velocity[2]  = std::cos(theta);
    }
    else if( mWorldDimension == 2 )
    {
        double phi = initial_angle[0] + (manifold_position[0] - manifold_start[0]) * 2 * pi;
        ray_velocity[0] = std::cos(phi);
        ray_velocity[1] = std::sin(phi);
    }
}


// The contents of this file are licensed under the MIT license.
// See LICENSE.txt for more information.

/*
This program takes a set of face chips and trains a network for face
embedding.

This program is based on the following example from dlib:

https://github.com/davisking/dlib/blob/master/examples/dnn_metric_learning_ex.cpp

The program had 3 usage modes:
1. train: using a set of face chips, train an embedding network
2. test:  using a set of face chips, test the network for accuracy
3. embed: use the trained network to create embeddings fro each provided
face chip.
*/

#include <iostream>
#include <ctime>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/misc_api.h>
#include <dlib/cmd_line_parser.h>
#include <dlib/matrix.h>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/posix_time/posix_time_io.hpp>
#include "boost/date_time/local_time_adjustor.hpp"
#include "boost/date_time/c_local_time_adjustor.hpp"
#include <map>

using namespace dlib;
using namespace std;
using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace boost::algorithm;
using boost::property_tree::ptree;

// ----------------------------------------------------------------------------------------
//  program functionality:
//		list of training set & test set
//     	network produced
//     	results
//		any parameters used as input (size of vector)
//
// log error rate(s) through iterations.
// ----------------------------------------------------------------------------------------

// We will need to create some functions for loading data.  This program will
// expect to be given a directory structured as follows:
//    top_level_directory/
//        person1/
//            image1.jpg
//            image2.jpg
//            image3.jpg
//        person2/
//            image4.jpg
//            image5.jpg
//            image6.jpg
//        person3/
//            image7.jpg
//            image8.jpg
//            image9.jpg
//
// The specific folder and image names don't matter, nor does the number of folders or
// images.  What does matter is that there is a top level folder, which contains
// subfolders, and each subfolder contains images of a single person.

ptree g_xml_tree;
std::string g_mode; // one of {train,test,embed}
std::vector <ptree> g_chips;

//--------------------------------------------------
// initialize xml
//--------------------------------------------------
int xml_add_headers ()
{
  g_xml_tree.add("dataset.name", "bearid dataset");
  g_xml_tree.add("dataset.comment", "Created by bearembed");
  return 0;
}

//--------------------------------------------------
// This function spiders the top level directory and obtains a list of all the
// image files.
std::vector<std::vector<string>> load_objects_list (
  const string& dir
)
{
  std::vector<std::vector<string>> objects;
  for (auto subdir : directory(dir).get_dirs())
  {
    std::vector<string> imgs;
    for (auto img : subdir.get_files())
    imgs.push_back(img);

    if (imgs.size() != 0)
    objects.push_back(imgs);
  }
  return objects;
}

/*!
    jitter image for augmentation.

    requires
        - image_type == an image object that implements the interface defined in
          dlib/image_processing/generic_image.h
        - pixel_traits<typename image_traits<image_type>::pixel_type>::has_alpha == false
        - img.size() > 0
        - img.nr() == img.nc()
    ensures
        - Randomly jitters the image a little bit and returns this new jittered image.
          To be specific, the returned image has the same size as img and will look
          generally similar.  The difference is that the returned image will have been
          slightly rotated, zoomed, and translated.  There is also a 50% chance it will
          be mirrored left to right.
!*/
template <
    typename image_type
    >
image_type my_jitter_image(
    const image_type& img,
    dlib::rand& rnd
)
{
    DLIB_CASSERT(num_rows(img)*num_columns(img) != 0);
    DLIB_CASSERT(num_rows(img)==num_columns(img));

    const double max_rotation_degrees = 3;
    const double min_object_height = 0.97;
    const double max_object_height = 0.99999;
    const double translate_amount = 0.02;

    const auto rect = shrink_rect(get_rect(img),3);

    // perturb the location of the crop by a small fraction of the object's size.
    const point rand_translate = dpoint(rnd.get_double_in_range(-translate_amount,translate_amount)*rect.width(),
        rnd.get_double_in_range(-translate_amount,translate_amount)*rect.height());

    // perturb the scale of the crop by a fraction of the object's size
    const double rand_scale_perturb = rnd.get_double_in_range(min_object_height, max_object_height);

    const long box_size = rect.height()/rand_scale_perturb;
    const auto crop_rect = centered_rect(center(rect)+rand_translate, box_size, box_size);
    const double angle = rnd.get_double_in_range(-max_rotation_degrees, max_rotation_degrees)*pi/180;
    image_type crop;
    extract_image_chip(img, chip_details(crop_rect, chip_dims(num_rows(img),num_columns(img)), angle), crop);
    if (rnd.get_random_double() > 0.5)
        flip_image_left_right(crop);

    return crop;
}

//-----------------------------------------------------------------
// Grab matched and unmatched pairs of chip files and labels
//-----------------------------------------------------------------
std::vector<std::vector<string>> load_pairs_map (
  const std::string& xml_file)
  {
    //std::map<string,std::vector<std::string>> chips_map;
    ptree tree;
    boost::property_tree::read_xml (xml_file, tree,
			boost::property_tree::xml_parser::trim_whitespace);

    cout << "load_pairs_map" << endl;

    std::vector<std::vector<string>> objects;		// return object
    std::vector<string> matched_chips;
    std::vector<string> unmatched_chips;
    std::vector<string> matched_labels;
    std::vector<string> unmatched_labels;
    int count = 0;

    BOOST_FOREACH(ptree::value_type& child, tree.get_child("dataset.pairs"))
    {
      std::string child_name = child.first;
      std::vector<string> pairFiles;
      std::vector<string> pairLabels;

      //cout << child_name << " " << count << endl;

      if ((child_name == "pair_matched") || (child_name == "pair_unmatched"))
      {
        ptree pairtree = (ptree) child.second;
        int ccount = 0;

        BOOST_FOREACH(ptree::value_type& pchild, pairtree)
        {
          //cout << pchild.first << endl;
          ccount += 1;
          std::string chipfile = pchild.second.get<std::string>("<xmlattr>.file");
          pairFiles.push_back (chipfile);
          std::string bearID = pchild.second.get<std::string>("label");
          pairLabels.push_back (bearID);
          //cout << "ID: " << bearID << " File: " << chipfile << endl;
        }
        if (ccount == 2)
        {
          // Add pair to map
          if (child_name == "pair_matched")
          {
            matched_chips.push_back (pairFiles[0]);
            matched_chips.push_back (pairFiles[1]);
            matched_labels.push_back (pairLabels[0]);
            matched_labels.push_back (pairLabels[1]);
          }
          else{
            unmatched_chips.push_back (pairFiles[0]);
            unmatched_chips.push_back (pairFiles[1]);
            unmatched_labels.push_back (pairLabels[0]);
            unmatched_labels.push_back (pairLabels[1]);
          }
        }
        else
        {
          cout << "BAD PAIR: " << child_name << " " << count << " has " << ccount << " chip(s)" << endl;
        }
      }

      count += 1;
      //if (count >= 3) break;  //TODO Remove ME
    }

    objects.push_back (matched_chips);
    objects.push_back (unmatched_chips);
    objects.push_back (matched_labels);
    objects.push_back (unmatched_labels);

    return objects;
}

//-----------------------------------------------------------------
// Grab all the chip files from xml and store each under its label.
// When doing infer/embed, chips will not have label so return
//   vector of 1 list and empty string.
//   Generate warning for chips with no label when test and train.
//-----------------------------------------------------------------
std::vector<std::vector<string>> load_chips_map (
  const std::string& xml_file, std::vector<std::string>& obj_labels)
  {
    std::map<string,std::vector<std::string>> chips_map;
    ptree tree;
    boost::property_tree::read_xml (xml_file, tree,
			boost::property_tree::xml_parser::trim_whitespace);
		std::string mode = g_mode;

    std::vector<std::vector<string>> objects;		// return object

    // for traing and test, add all chip filenames to map by bearID
    BOOST_FOREACH(ptree::value_type& child, tree.get_child("dataset.chips"))
    {
      std::string child_name = child.first;

      if (child_name == "chip")
      {
        ptree chip = child.second;
        std::string chipfile = child.second.get<std::string>("<xmlattr>.file");
        std::string bearID = child.second.get<std::string>("label");
        if (bearID.empty() && g_mode != "embed")
        {
			std::cout << "Error: ignoring chipfile " << chipfile << " wwith no bearID.\n" << endl;
			continue;
        }
		if (bearID.empty())
		{
			bearID = " ";
			// TODO:  need to support for unknown images
		}
		g_chips.push_back (chip);
        chips_map[bearID].push_back (chipfile);
      }
    }
    // massage map of vector to return vector of vector
    std::string key;
    std::vector<std::string> value;

	obj_labels.clear ();
    std::map<std::string, std::vector<std::string>>::iterator it;
    for ( it = chips_map.begin(); it != chips_map.end(); it++ )
    {
      objects.push_back (it->second);
      obj_labels.push_back (it->first);
    }
    return objects;
  }

//-----------------------------------------------------------------
//-----------------------------------------------------------------
std::vector<std::vector<string>> load_chips_xml (
  const std::string& xml_file, bool pair, std::vector<std::string>& obj_labels)
  {
    std::vector<std::vector<string>> objects;		// return object

    if (pair)
    {
      cout << "Pair file" << endl;
      objects = load_pairs_map(xml_file);
    }
    else
    {
      cout << "Normal file" << endl;
      objects = load_chips_map(xml_file, obj_labels);
    }
    return objects;
  }

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

  // This function takes the output of load_objects_list() as input and randomly
  // selects images for training.  It should also be pointed out that it's really
  // important that each mini-batch contain multiple images of each person.  This
  // is because the metric learning algorithm needs to consider pairs of images
  // that should be close (i.e. images of the same person) as well as pairs of
  // images that should be far apart (i.e. images of different people) during each
  // training step.
  void load_mini_batch (
    const size_t num_people,     // how many different people to include
    const size_t samples_per_id, // how many images per person to select.
    dlib::rand& rnd,
    const std::vector<std::vector<string>>& objs,
    std::vector<matrix<rgb_pixel>>& images,
    std::vector<unsigned long>& labels
  )
  {
    images.clear();
    labels.clear();
    DLIB_CASSERT(num_people <= objs.size(), "The dataset doesn't have that many people in it.");

    std::vector<bool> already_selected(objs.size(), false);
    matrix<rgb_pixel> image;
    for (size_t i = 0; i < num_people; ++i)
    {
      size_t id = rnd.get_random_32bit_number()%objs.size();
      // don't pick a person we already added to the mini-batch
      while(already_selected[id])
      id = rnd.get_random_32bit_number()%objs.size();
      already_selected[id] = true;
      //cout << "Rnd ID: " << id << endl;

      for (size_t j = 0; j < samples_per_id; ++j)
      {
        const auto& obj = objs[id][rnd.get_random_32bit_number()%objs[id].size()];
        load_image(image, obj);
        images.push_back(std::move(image));
        labels.push_back(id);
      }
    }

    // You might want to do some data augmentation at this point.  Here we do some simple
    // color augmentation.
    for (auto&& crop : images)
    {
        disturb_colors(crop,rnd);
        // Jitter most crops
        if (rnd.get_random_double() > 0.1)
            crop = my_jitter_image(crop,rnd);
    }

    // All the images going into a mini-batch have to be the same size.  And really, all
    // the images in your entire training dataset should be the same size for what we are
    // doing to make the most sense.
    DLIB_CASSERT(images.size() > 0);
    for (auto&& img : images)
    {
      DLIB_CASSERT(img.nr() == images[0].nr() && img.nc() == images[0].nc(),
      "All the images in a single mini-batch must be the same size.");
    }
  }

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

  // The next page of code defines a ResNet network.  It's basically copied
  // and pasted from the dnn_imagenet_ex.cpp example, except we replaced the loss
  // layer with loss_metric and make the network somewhat smaller.

  template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
  using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

  template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
  using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

  template <int N, template <typename> class BN, int stride, typename SUBNET>
  using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

  template <int N, typename SUBNET> using res       = relu<residual<block,N,bn_con,SUBNET>>;
  template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
  template <int N, typename SUBNET> using res_down  = relu<residual_down<block,N,bn_con,SUBNET>>;
  template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

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

  template <typename SUBNET> using level0 = res_down<256,SUBNET>;
  template <typename SUBNET> using level1 = res<256,res<256,res_down<256,SUBNET>>>;
  template <typename SUBNET> using level2 = res<128,res<128,res_down<128,SUBNET>>>;
  template <typename SUBNET> using level3 = res<64,res<64,res<64,res_down<64,SUBNET>>>>;
  template <typename SUBNET> using level4 = res<32,res<32,res<32,SUBNET>>>;

  template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
  template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
  template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
  template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
  template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

  // training network type
  using net_type = loss_metric<fc_no_bias<128,avg_pool_everything<
  level0<
  level1<
  level2<
  level3<
  level4<
  max_pool<3,3,2,2,relu<bn_con<con<32,7,7,2,2,
  input_rgb_image
  >>>>>>>>>>>>;

  // training network type of size 150
  using net_type_150 = loss_metric<fc_no_bias<128,avg_pool_everything<
  level0<
  level1<
  level2<
  level3<
  level4<
  max_pool<3,3,2,2,relu<bn_con<con<32,7,7,2,2,
  input_rgb_image_sized<150>
  >>>>>>>>>>>>;

  // testing network type (replaced batch normalization with fixed affine transforms)
  using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
  alevel0<
  alevel1<
  alevel2<
  alevel3<
  alevel4<
  max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
  input_rgb_image
  >>>>>>>>>>>>;

  // testing network type of size 150
  using anet_type_150 = loss_metric<fc_no_bias<128,avg_pool_everything<
  alevel0<
  alevel1<
  alevel2<
  alevel3<
  alevel4<
  max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
  input_rgb_image_sized<150>
  >>>>>>>>>>>>;

int find_chip_index (
std::vector <ptree> chips, std::string chip_name)
{
	for (int i=0 ; i < chips.size (); ++i)
	{
        std::string chipfile = chips[i].get<std::string>("<xmlattr>.file");
		if (chip_name == chipfile)
			return i;
	}
	return -1;
}

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

  int main(int argc, char** argv)
  {
    try
    {
      time_t timeStart = time(NULL);
      command_line_parser parser;

      cout << "Start time: " << timeStart << endl;

      parser.add_option("h","Display this help message.");
      parser.add_option("train","Train the face embedding network. Writes to network file.");
      // --test <network>
      parser.add_option("test","Test the face embedding network. Takes trained network", 1);
      parser.add_option("pair","Interpret xml_file as a pair file, used for testing");
      // --embed <network>
      parser.add_option("embed","Create the face embedding for each image.", 1);
      // --pretrain only used with --train
      parser.add_option("pretrain","Specifies data to initialize the network.",1);
      // --output: <trained_network> with --train; <embed_directory> with --embed
      parser.add_option("output","Used with train, specifies trained weights file.  Use with -embed, specifies directory to put embeddings. Defaults to local.",1);
      // --bn not used with --pretrain
      parser.add_option("bn","Use batch norm, not affine net");
      // --network_steps  <num_iterations>.  defaults to 2000
      parser.add_option("network_steps","Steps before writing to new network file [default = 2000]",1);
      // --threshold <num_iterations>.  defaults to 10000
      parser.add_option("threshold","Iterations without progess before stopping [default = 10000]",1);
      // --numid <count_per_minibatch> . defaults to 5
      parser.add_option("numid","Number if IDs used per mini batch [default = 5]",1);
      parser.add_option("numface","Number of face images used per ID [default = 5]",1);
	  // --root <chip_root_path> . defaults to /home/data/bears/faceDogHip
      parser.add_option("root","Root of chip files, used in extracting embed file path.", 1);
      parser.add_option("roc","Generate ROC Curve data");

      parser.parse(argc, argv);

      // Now we do a little command line validation.  Each of the following functions
      // checks something and throws an exception if the test fails.
      const char* one_time_opts[] = {"h", "train", "test", "embed"};
      parser.check_one_time_options(one_time_opts); // Can't give an option more than once

      if (parser.option("h"))
      {
        cout << "Usage: bearembed [options] <xml_file>\n";
        parser.print_options();

        return EXIT_SUCCESS;
      }

      if (parser.number_of_arguments() == 0)
      {
        cout << "Give a xml chip file  as input.  It should list chips and labels to be" << endl;
        cout << "used to learn to distinguish between these labels with metric learning.  " << endl;
        cout << "For example:" << endl;
        cout << "   ./bearembed <xml_file>" << endl;
        return 1;
      }

      cout << "\nXML chip file.... : " << parser[0] << endl;

      std::vector<string> obj_labels;

			// if train or test, load chips with labels
			// if infer, has no labels, load list of chips
			if (parser.option ("embed"))
				g_mode = "embed";
			else if (parser.option ("test"))
				g_mode = "test";
			else if (parser.option ("train"))
				g_mode = "train";
      auto objs = load_chips_xml (parser[0], parser.option("pair"), obj_labels);
      cout << "objs.size(): "<< objs.size() << endl;

      // auto objs = load_objects_list(parser[0]);
      // cout << "objs.size(): "<< objs.size() << endl;

      // Id x Face parameters: example 5x5, face recognition 35x15
      int numid = get_option (parser, "numid", 5);     // how many different people to include
      int numface = get_option (parser, "numface", 5); // how many images per person to select
      cout << "numid: " << numid << endl;
      cout << "numface: " << numface << endl;

      std::vector<matrix<rgb_pixel>> images;
      std::vector<unsigned long> labels;

      // Training
      if (parser.option("train"))
      {
        anet_type_150 anet_150;
        net_type_150 net_150;
        net_type net;
        cout << "Start training..." << endl;
        std::string sync_file;
        std::string trained_network;
        if (parser.option ("output"))
        {
          trained_network = parser.option("output").argument();
        }
        else
        {
          trained_network = "bears.dat";
        }
        cout << "Output: " << trained_network << endl;

        int threshold = get_option (parser, "threshold", 10000);
        cout << "Threshold: " << threshold << endl;

        if (parser.option("pretrain")) // assumes size 150
        {
          cout << ".. using pretrain weights ..." << endl;
          std::string pretrained_file = parser.option("pretrain").argument();
          cout << "Load network: " << pretrained_file << endl;
          if (parser.option("bn"))
          {     // ------- BATCHNORM, NO -ANET ---------------
            deserialize(pretrained_file) >> net_150;
            dnn_trainer<net_type_150> trainer(net_150, sgd(0.0001, 0.9));
            cout << "...... no anet ......." << endl;
            trainer.set_learning_rate(0.001);
            sync_file= "pretrained_no_anet_sync";
            trainer.be_verbose();
            trainer.set_synchronization_file(sync_file, std::chrono::minutes(5));
            trainer.set_iterations_without_progress_threshold(threshold);
            dlib::pipe<std::vector<matrix<rgb_pixel>>> qimages(4);
            dlib::pipe<std::vector<unsigned long>> qlabels(4);
            auto data_loader = [&numid, &numface, &qimages, &qlabels, &objs](time_t seed)
            {
              dlib::rand rnd(time(0)+seed);
              std::vector<matrix<rgb_pixel>> images;
              std::vector<unsigned long> labels;
              while(qimages.is_enabled())
              {
                try
                {
                  load_mini_batch(numid, numface, rnd, objs, images, labels);
                  qimages.enqueue(images);
                  qlabels.enqueue(labels);
                }
                catch(std::exception& e)
                {
                  cout << "EXCEPTION IN LOADING DATA" << endl;
                  cout << e.what() << endl;
                }
              }
            };
            std::thread data_loader1([data_loader](){ data_loader(1); });
            std::thread data_loader2([data_loader](){ data_loader(2); });
            std::thread data_loader3([data_loader](){ data_loader(3); });
            std::thread data_loader4([data_loader](){ data_loader(4); });
            std::thread data_loader5([data_loader](){ data_loader(5); });
            while(trainer.get_learning_rate() >= 1e-4)
            {
              qimages.dequeue(images);
              qlabels.dequeue(labels);
              trainer.train_one_step(images, labels);
              cout << "Step: " << trainer.get_train_one_step_calls() << " Loss: " << trainer.get_average_loss() << endl;
            }
            trainer.get_net();
            cout << "done training" << endl;
            net_150.clean();
            anet_150 = net_150;
            serialize(trained_network) << anet_150;
            qimages.disable();
            qlabels.disable();
            data_loader1.join();
            data_loader2.join();
            data_loader3.join();
            data_loader4.join();
            data_loader5.join();
          }   // ------- BATCHNORM, NO -ANET ---------------
          else
          {     // ------- ANET ---------------
            cout << "...... using anet ......." << endl;
            deserialize(pretrained_file) >> anet_150;
            dnn_trainer<anet_type_150> trainer(anet_150, sgd(0.0001, 0.9));
            trainer.set_learning_rate(0.001);
            sync_file= "pretrained_anet_sync";
            trainer.be_verbose();
            trainer.set_synchronization_file(sync_file, std::chrono::minutes(5));
            trainer.set_iterations_without_progress_threshold(threshold);
            dlib::pipe<std::vector<matrix<rgb_pixel>>> qimages(4);
            dlib::pipe<std::vector<unsigned long>> qlabels(4);
            auto data_loader = [&numid, &numface, &qimages, &qlabels, &objs](time_t seed)
            {
              dlib::rand rnd(time(0)+seed);
              std::vector<matrix<rgb_pixel>> images;
              std::vector<unsigned long> labels;
              while(qimages.is_enabled())
              {
                try
                {
                  load_mini_batch(numid, numface, rnd, objs, images, labels);
                  qimages.enqueue(images);
                  qlabels.enqueue(labels);
                }
                catch(std::exception& e)
                {
                  cout << "EXCEPTION IN LOADING DATA" << endl;
                  cout << e.what() << endl;
                }
              }
            };
            std::thread data_loader1([data_loader](){ data_loader(1); });
            std::thread data_loader2([data_loader](){ data_loader(2); });
            std::thread data_loader3([data_loader](){ data_loader(3); });
            std::thread data_loader4([data_loader](){ data_loader(4); });
            std::thread data_loader5([data_loader](){ data_loader(5); });
						int step_count=1;
						int network_count=1;
						int network_step_size = 2000;
						if (parser.option("network_steps"))
							network_step_size = std::stoi (parser.option("network_steps").argument());
            while(trainer.get_learning_rate() >= 1e-4)
            {
              qimages.dequeue(images);
              qlabels.dequeue(labels);
              trainer.train_one_step(images, labels);
              cout << "Step: " << trainer.get_train_one_step_calls() << " Loss: " << trainer.get_average_loss() << endl;
							// generating occasional networks to create learning curve
							if (parser.option("network_steps"))
							{
								if (step_count % network_step_size == 0)
								{
									trainer.get_net();
									std::string intermediate_network = trained_network+std::to_string(network_count);
									cout << "writing network " << intermediate_network << endl;
									anet_150.clean();
									serialize(intermediate_network) << anet_150;
									network_count++;
								}
							}
							step_count++;
            }
            trainer.get_net();
            cout << "done training" << endl;
            anet_150.clean();
            serialize(trained_network) << anet_150;
            qimages.disable();
            qlabels.disable();
            data_loader1.join();
            data_loader2.join();
            data_loader3.join();
            data_loader4.join();
            data_loader5.join();
          }     // ------- ANET ---------------
        }
        else   //------------  NO PRETRAINED DATA ------
        {
          cout << "... using random weights ....." << endl;
          dnn_trainer<net_type> trainer(net, sgd(0.0001, 0.9));
          trainer.set_learning_rate(0.1);
          sync_file= "rand_sync";
          trainer.be_verbose();
          trainer.set_synchronization_file(sync_file, std::chrono::minutes(5));
          trainer.set_iterations_without_progress_threshold(threshold);
          dlib::pipe<std::vector<matrix<rgb_pixel>>> qimages(4);
          dlib::pipe<std::vector<unsigned long>> qlabels(4);
          auto data_loader = [&numid, &numface, &qimages, &qlabels, &objs](time_t seed)
          {
            dlib::rand rnd(time(0)+seed);
            std::vector<matrix<rgb_pixel>> images;
            std::vector<unsigned long> labels;
            while(qimages.is_enabled())
            {
              try
              {
                load_mini_batch(numid, numface, rnd, objs, images, labels);
                qimages.enqueue(images);
                qlabels.enqueue(labels);
              }
              catch(std::exception& e)
              {
                cout << "EXCEPTION IN LOADING DATA" << endl;
                cout << e.what() << endl;
              }
            }
          };
          std::thread data_loader1([data_loader](){ data_loader(1); });
          std::thread data_loader2([data_loader](){ data_loader(2); });
          std::thread data_loader3([data_loader](){ data_loader(3); });
          std::thread data_loader4([data_loader](){ data_loader(4); });
          std::thread data_loader5([data_loader](){ data_loader(5); });
          while(trainer.get_learning_rate() >= 1e-4)
          {
            qimages.dequeue(images);
            qlabels.dequeue(labels);
            trainer.train_one_step(images, labels);
            cout << "Step: " << trainer.get_train_one_step_calls() << " Loss: " << trainer.get_average_loss() << endl;
          }
          trainer.get_net();
          cout << "done training" << endl;
          net.clean();
          serialize(trained_network) << net;
          qimages.disable();
          qlabels.disable();
          data_loader1.join();
          data_loader2.join();
          data_loader3.join();
          data_loader4.join();
          data_loader5.join();
        }
        /*
        // ----------- START SECTION REPLICATED 3X ABOVE --------------------
        // I've set this to something really small to make the example terminate
        // sooner.  But when you really want to train a good model you should set
        // this to something like 10000 so training doesn't terminate too early.
        //trainer.set_iterations_without_progress_threshold(300);
        // -- trainer.set_iterations_without_progress_threshold(10000);

        // If you have a lot of data then it might not be reasonable to load it all
        // into RAM.  So you will need to be sure you are decompressing your images
        // and loading them fast enough to keep the GPU occupied.  I like to do this
        // using the following coding pattern: create a bunch of threads that dump
        // mini-batches into dlib::pipes.
        dlib::pipe<std::vector<matrix<rgb_pixel>>> qimages(4);
        dlib::pipe<std::vector<unsigned long>> qlabels(4);
        auto data_loader = [&qimages, &qlabels, &objs](time_t seed)
        {
        dlib::rand rnd(time(0)+seed);
        std::vector<matrix<rgb_pixel>> images;
        std::vector<unsigned long> labels;
        while(qimages.is_enabled())
        {
        try
        {
        load_mini_batch(numid, numface, rnd, objs, images, labels);
        // Tried 35x10, trained much slower and did worse on test (86%; training was 100%)
        qimages.enqueue(images);
        qlabels.enqueue(labels);
      }
      catch(std::exception& e)
      {
      cout << "EXCEPTION IN LOADING DATA" << endl;
      cout << e.what() << endl;
    }
  }
};
// Run the data_loader from 5 threads.  You should set the number of threads
// relative to the number of CPU cores you have.
std::thread data_loader1([data_loader](){ data_loader(1); });
std::thread data_loader2([data_loader](){ data_loader(2); });
std::thread data_loader3([data_loader](){ data_loader(3); });
std::thread data_loader4([data_loader](){ data_loader(4); });
std::thread data_loader5([data_loader](){ data_loader(5); });

// Here we do the training.  We keep passing mini-batches to the trainer until the
// learning rate has dropped low enough.
while(trainer.get_learning_rate() >= 1e-4)
{
qimages.dequeue(images);
qlabels.dequeue(labels);
trainer.train_one_step(images, labels);
cout << "Step: " << trainer.get_train_one_step_calls() << " Loss: " << trainer.get_average_loss() << endl;
}

// Wait for training threads to stop
trainer.get_net();
cout << "done training" << endl;

// Save the network to disk
if (parser.option("pretrained_file"))
{
if (parser.option("anet"))
{
anet_150.clean();
serialize(trained_network) << anet_150;
}
else
{
net_150.clean();
serialize(trained_network) << net_150;
}
}
else
{
net.clean();
serialize(trained_network) << net;
}

// stop all the data loading threads and wait for them to terminate.
qimages.disable();
qlabels.disable();
data_loader1.join();
data_loader2.join();
data_loader3.join();
data_loader4.join();
data_loader5.join();
// ----------- END SECTION REPLICATED 3X ABOVE --------------------
*/
}

// Testing pairs
else if (parser.option("test") && parser.option("pair"))
{
  anet_type_150 anet_150;
  anet_type_150 testing_net;

  net_type net;
  // anet_type testing_net;

  std::string test_network = (parser.option("test").argument());

  // boost::filesystem::path test_network(parser.option("train").argument());

  if (parser.option("bn"))
  {  // ------- BATCHNORM, NOT ANET ----------
    cout << "... not anet ..." << endl;
    deserialize(test_network) >> net;
    testing_net = net;
  }  // ------- BATCHNORM, NOT ANET ----------
  else
  {  // ------- ANET ----------
    cout << "... using anet ..." << endl;
    deserialize(test_network) >> testing_net;
  }  // ------- ANET ----------

  cout << "Start testing pairs..." << endl;
  std::vector<string> matchedPairs = objs[0];
  std::vector<string> unmatchedPairs = objs[1];
  std::vector<string> matchedLabels = objs[2];
  std::vector<string> unmatchedLabels = objs[3];

  std::vector<double> true_dets, false_dets;

  int num_right = 0;
  int num_wrong = 0;
  int num_true_pos = 0;
  int num_true_neg = 0;
  int num_false_pos = 0;
  int num_false_neg = 0;

  matrix<rgb_pixel> image;
  std::vector<matrix<rgb_pixel>> images;
  // set up file name for result file
  time_t rawtime;
  struct tm * timeinfo;
  char time_buffer[80];
  time (&rawtime);
  timeinfo = localtime(&rawtime);
  strftime(time_buffer,sizeof(time_buffer),"%Y%m%d%I%M",timeinfo);
  std::string result_filename = "test_result_";
  result_filename.append (time_buffer);
  ofstream result_file;
  result_file.open (result_filename);

  // Matching Pairs

  double dist_max = testing_net.loss_details().get_distance_threshold();
  for (size_t i = 0; i < matchedPairs.size(); i+=2)
  {
	result_file << matchedLabels[i] << "," << matchedLabels[i+1];
    images.clear();
    //cout << "Matched Pair " << i/2 << endl;
    //cout << matchedPairs[i] << endl;
    load_image(image, matchedPairs[i]);
    images.push_back(std::move(image));
    //cout << matchedPairs[i+1] << endl;
    load_image(image, matchedPairs[i+1]);
    images.push_back(std::move(image));

    std::vector<matrix<float,0,1>> matchedEmbedded = testing_net(images);
    double images_dist = length(matchedEmbedded[0]-matchedEmbedded[1]);
    true_dets.push_back(images_dist);

    if (images_dist < dist_max)
    {
      ++num_right;
      ++num_true_pos;
	  result_file << ",1,1,";
      //cout << "RIGHT (same) " << matchedPairs[i] << " to " << matchedPairs[i+1] << " distance is " << to_string(length(matchedEmbedded[0]-matchedEmbedded[1])) << endl;
    }
    else
    {
      ++num_wrong;
      ++num_false_neg;
	  result_file << ",0,1,";
      //cout << "WRONG (same) " << matchedPairs[i] << " to " << matchedPairs[i+1] << " distance is " << to_string(length(matchedEmbedded[0]-matchedEmbedded[1])) << endl;
    }
	result_file << matchedPairs[i] << "," << matchedPairs[i+1];
	result_file << "," << images_dist << "," << dist_max << endl;
  }

  // Unmatching Pairs
  for (size_t i = 0; i < unmatchedPairs.size(); i+=2)
  {
	result_file << unmatchedLabels[i] << "," << unmatchedLabels[i+1];
    images.clear();
    //cout << "Unmatched Pair " << i/2 << endl;
    //cout << unmatchedPairs[i] << endl;
    load_image(image, unmatchedPairs[i]);
    images.push_back(std::move(image));
    //cout << unmatchedPairs[i+1] << endl;
    load_image(image, unmatchedPairs[i+1]);
    images.push_back(std::move(image));

    std::vector<matrix<float,0,1>> unmatchedEmbedded = testing_net(images);
    double images_dist = length(unmatchedEmbedded[0]-unmatchedEmbedded[1]);
    false_dets.push_back(images_dist);

    if (images_dist >= dist_max)
    {
      ++num_right;
      ++num_true_neg;
	  result_file << ",0,0,";
      //cout << "RIGHT (same) " << unmatchedPairs[i] << " to " << unmatchedPairs[i+1] << " distance is " << to_string(length(unmatchedEmbedded[0]-unmatchedEmbedded[1])) << endl;
    }
    else
    {
      ++num_wrong;
      ++num_false_pos;
	  result_file << ",1,0,";
      //cout << "WRONG (same) " << unmatchedPairs[i] << " to " << unmatchedPairs[i+1] << " distance is " << to_string(length(unmatchedEmbedded[0]-unmatchedEmbedded[1])) << endl;
    }
	result_file << unmatchedPairs[i] << "," << unmatchedPairs[i+1];
	result_file << "," << images_dist << "," << dist_max << endl;
  }
  result_file.close ();

  cout << "num_right: "<< num_right << endl;
  cout << "num_wrong: "<< num_wrong << endl;
  cout << "num_true_pos: "<< num_true_pos << endl;
  cout << "num_true_neg: "<< num_true_neg << endl;
  cout << "num_false_pos: "<< num_false_pos << endl;
  cout << "num_false_neg: "<< num_false_neg << endl;

  double accuracy = ((double)num_right / ((double)num_right + (double)num_wrong));
  double tpr = ((double)num_true_pos / ((double)num_true_pos + (double)num_false_neg));
  double fpr = ((double)num_false_pos / ((double)num_false_pos + (double)num_true_neg));
  double tnr = ((double)num_true_neg / ((double)num_true_neg + (double)num_false_pos));
  double fnr = ((double)num_false_neg / ((double)num_true_pos + (double)num_false_neg));
  double f1 = (((double)2 * (double)num_true_pos) / (((double)2 * (double)num_true_pos) + (double)num_false_pos + (double)num_false_neg));
  cout << "Accuracy: " << fixed << accuracy << endl;
  cout << "True positive rate: " << fixed << tpr << endl;
  cout << "False negative rate: " << fixed << fnr << endl;
  cout << "True negative rate: " << fixed << tnr << endl;
  cout << "False positive rate: " << fixed << fpr << endl;
  cout << "F1 score: " << fixed << f1 << endl;

  if (parser.option("roc"))
  {
    ofstream poc_file;
    poc_file.open ("roc_curve.csv");
    // ROC Curve
    cout << "\nGenerate ROC Curve data" << endl;
    //cout << "TD: " << true_dets[0] << " FD: " << false_dets[0] << endl;
    std::vector<roc_point> ROC = compute_roc_curve (true_dets, false_dets);
    cout << "ROC size: " << ROC.size() << endl;

    // compute_roc_curve expects positive scores to be higher than negative scores
    // For the bearembed distance metric, lower is better
    // So compute_roc_curve is actually returning FNR and TNR
    // To get TPR and FPR, we need to invert (1-TPR and 1-FPR)
    poc_file << "Detection Threshold" << ", " << "True Positive Rate" << ", " << "False Positive Rate" << endl;
    for (size_t i = 0; i < ROC.size(); i+=1)
    {
      // Note TPR and FPR are inverted as descibed above
      poc_file << ROC[i].detection_threshold << ", " << (1-ROC[i].true_positive_rate) << ", " << (1-ROC[i].false_positive_rate) << endl;
    }
    poc_file.close();
  }
}

// Testing OLD
else if (parser.option("test") && !parser.option("pair"))
{
  int num_folds = 100;
  double accuracy_arr[num_folds];
  double f1_arr[num_folds];

  anet_type_150 anet_150;
  anet_type_150 testing_net;

  net_type net;
  // anet_type testing_net;

  std::string test_network = (parser.option("test").argument());

  // boost::filesystem::path test_network(parser.option("train").argument());

  if (parser.option("bn"))
  {  // ------- BATCHNORM, NOT ANET ----------
    cout << "... not anet ..." << endl;
    deserialize(test_network) >> net;
    testing_net = net;
  }  // ------- BATCHNORM, NOT ANET ----------
  else
  {  // ------- ANET ----------
    cout << "... using anet ..." << endl;
    deserialize(test_network) >> testing_net;
  }  // ------- ANET ----------

  cout << "Start testing..." << endl;
  // Normally you would use the non-batch-normalized version of the network to do
  // testing, which is what we do here.

	time_t rawtime;
	struct tm * timeinfo;
	char time_buffer[80];
  std::string result_filename;
  std::string result_filename_fold;

	time (&rawtime);
	timeinfo = localtime(&rawtime);
	strftime(time_buffer,sizeof(time_buffer),"%Y%m%d%I%M",timeinfo);
	result_filename = "test_result_";
	result_filename.append (time_buffer);

  for (int folds = 0; folds < num_folds; ++folds)
  {
		ofstream result_file;
		result_filename_fold = result_filename + "_" + to_string (folds);
		result_file.open (result_filename_fold);
    cout << endl;
    cout << "fold: "<< folds << endl;

    // Now, just to show an example of how you would use the network, let's check how well
    // it performs on the training data.
    dlib::rand rnd(time(0)+folds);
    load_mini_batch(numid, numface, rnd, objs, images, labels);

    // Run all the images through the network to get their vector embeddings.
    std::vector<matrix<float,0,1>> embedded = testing_net(images);

    // Now, check if the embedding puts images with the same labels near each other and
    // images with different labels far apart.
    int num_right = 0;
    int num_wrong = 0;
    int num_true_pos = 0;
    int num_true_neg = 0;
    int num_false_pos = 0;
    int num_false_neg = 0;
    for (size_t i = 0; i < embedded.size(); ++i)
    {
      for (size_t j = i+1; j < embedded.size(); ++j)
      {
        if (labels[i] == labels[j])
        {
          // The loss_metric layer will cause images with the same label to be less
          // than net.loss_details().get_distance_threshold() distance from each
          // other.  So we can use that distance value as our testing threshold.
		  result_file << labels[i] << "," << labels[j];
          if (length(embedded[i]-embedded[j]) < testing_net.loss_details().get_distance_threshold())
          {
            ++num_right;
            ++num_true_pos;
            //cout << "RIGHT (same) " << labels[i] << " to " << labels[j] << " distance is " << to_string(length(embedded[i]-embedded[j])) << endl;
			result_file << ",1,1" << endl;
          }
          else
          {
            ++num_wrong;
            ++num_false_neg;
			result_file << ",0,1" << endl;
            //cout << "WRONG (same) " << labels[i] << " to " << labels[j] << " distance is " << to_string(length(embedded[i]-embedded[j])) << endl;
          }
        }
        else
        {
          if (length(embedded[i]-embedded[j]) >= testing_net.loss_details().get_distance_threshold())
          {
            ++num_right;
            ++num_true_neg;
			result_file << ",0,0" << endl;
            //cout << "RIGHT (diff) " << labels[i] << " to " << labels[j] << " distance is " << to_string(length(embedded[i]-embedded[j])) << endl;
          }
          else
          {
            ++num_wrong;
            ++num_false_pos;
			result_file << ",1,0" << endl;
            //cout << "WRONG (diff) " << labels[i] << " to " << labels[j] << " distance is " << to_string(length(embedded[i]-embedded[j])) << endl;
          }
        }
      }
    }
		result_file.close ();

    cout << "num_right: "<< num_right << endl;
    cout << "num_wrong: "<< num_wrong << endl;
    cout << "num_true_pos: "<< num_true_pos << endl;
    cout << "num_true_neg: "<< num_true_neg << endl;
    cout << "num_false_pos: "<< num_false_pos << endl;
    cout << "num_false_neg: "<< num_false_neg << endl;

    double accuracy = ((double)num_right / ((double)num_right + (double)num_wrong));
    double tpr = ((double)num_true_pos / ((double)num_true_pos + (double)num_false_neg));
    double fpr = ((double)num_false_pos / ((double)num_false_pos + (double)num_true_neg));
    double tnr = ((double)num_true_neg / ((double)num_true_neg + (double)num_false_pos));
    double fnr = ((double)num_false_neg / ((double)num_true_pos + (double)num_false_neg));
    double f1 = (((double)2 * (double)num_true_pos) / (((double)2 * (double)num_true_pos) + (double)num_false_pos + (double)num_false_neg));
    cout << "Accuracy: " << fixed << accuracy << endl;
    cout << "True positive rate: " << fixed << tpr << endl;
    cout << "False negative rate: " << fixed << fnr << endl;
    cout << "True negative rate: " << fixed << tnr << endl;
    cout << "False positive rate: " << fixed << fpr << endl;
    cout << "F1 score: " << fixed << f1 << endl;

    accuracy_arr[folds] = accuracy;
    f1_arr[folds] = f1;
  }

  // Calculate the mean accuracy and f1 score
  double accuracy_sum = 0;
  double f1_sum = 0;
  for (int folds = 0; folds < num_folds; ++folds)
  {
    accuracy_sum += accuracy_arr[folds];
    f1_sum += f1_arr[folds];
  }
  double accuracy_mean = accuracy_sum / num_folds;
  double f1_mean = f1_sum / num_folds;

  // Calculate the standard error of the mean
  double deviation_sum = 0;
  for (int folds = 0; folds < num_folds; ++folds)
  {
    deviation_sum += pow((accuracy_arr[folds] - accuracy_mean), 2);
  }
  double se_of_mean = sqrt(deviation_sum / (num_folds-1));
  cout << endl;
  cout << "Accuracy Mean: " << fixed << accuracy_mean << endl;
  cout << "Standard Error of the Mean: " << fixed << se_of_mean << endl;
  cout << "F1 Mean: " << fixed << f1_mean << endl;
}
else if (parser.option("embed"))
{
  // Generate embeddings in a directory
  //   use optional --output, else create dir in current directory
  boost::filesystem::path src_path(parser[0]);
  std::string chip_root = "/home/data/bears/faceDogHip/dhTreeChip/";
  cout << "Generating embeddings..." << endl;
  std::string dst_path;
  if (parser.option ("output"))
  {
    dst_path = parser.option("output").argument();
    if (!boost::filesystem::exists(dst_path)) // create new directory
    {
	  boost::filesystem::create_directories(dst_path);
    }
  }
  if (dst_path.empty())
  {
    time_t rawtime;
    struct tm * timeinfo;
    char buffer[80];

    time (&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer,sizeof(buffer),"%Y%m%d%I%M",timeinfo);
    dst_path = "embed_";
    dst_path.append (buffer);
	boost::filesystem::create_directories(dst_path);
  }
  if (parser.option ("root"))
  {
  	chip_root = parser.option ("root").argument();
  }

  // create XML content file
  std::string cmd;
  for (int m = 0; m < argc; m++) {
  	cmd.append (argv[m]);
	cmd.append (" ");
  }
  boost::filesystem::path current_dir (boost::filesystem::current_path());
  g_xml_tree.add ("dataset.command", cmd);
  g_xml_tree.add ("dataset.cwd", current_dir);
  g_xml_tree.add ("dataset.filetype", "embeddings");
  ptree &embeds = g_xml_tree.add ("dataset.embeddings", "");

  /*
  src_path = boost::filesystem::canonical(src_path);
  dst_path = boost::filesystem::canonical(dst_path);
  cout << "Source path: " << src_path.string() << endl;
  cout << "Destination path: " << dst_path.string() << endl;
  cout << "Destination path: " << dst_path << endl;
  */

  anet_type_150 anet_150;
  anet_type_150 embedding_anet_150;
  net_type net;
  anet_type embedding_anet;
  std::string embed_network = (parser.option("embed").argument());
  if (parser.option("bn"))
  {  // ------- BATCHNORM, NOT ANET ----------
    deserialize(embed_network) >> net;
    embedding_anet = net;
  }  // ------- BATCHNORM, NOT ANET ----------
  else
  {  // ------- ANET ----------
    deserialize(embed_network) >> anet_150;
    embedding_anet_150 = anet_150;
  }  // ------- ANET ----------

  // Normally you would use the non-batch-normalized version of the network to do
  // testing, which is what we do here.
  matrix<rgb_pixel> image;

  int embeddings_count = 0;
  boost::filesystem::path emb_dir = boost::filesystem::canonical(dst_path);
  // go through each chip
  for (size_t i = 0; i < objs.size(); ++i)
  {
    // cout << "ID: " << i << " - label: " << obj_labels[i] << " - Files: " << objs[i].size() << endl;

    embeddings_count += objs[i].size();
    for (size_t j = 0; j < objs[i].size(); ++j)
    {
      string img_str = objs[i][j];
      string emb_str = objs[i][j];
      matrix<float,0,1> embedded;
      //cout << "Image: " << img_str << endl;
      load_image(image, img_str);
      // get embedding
      // TODO add jitter (see dnn_face_recognition_ex.cpp)
      if (parser.option("bn"))
	  {  // ------- BATCHNORM, NOT ANET ----------
        embedded = embedding_anet(image);
	  }  // ------- BATCHNORM, NOT ANET ----------
	  else
	  {  // ------- ANET ----------
        embedded = embedding_anet_150(image);
	  }  // ------- ANET ----------
	  // extract subdir path of file.  if nothing matched, the result
	  //   is the orig path.
      std::string chip_subdir_file = erase_first_copy (emb_str, chip_root); // -> bf/fitz/bf480/IMG123.JPG

      boost::filesystem::path emb_subdir_file (chip_subdir_file);
      boost::filesystem::path emb_path = emb_dir / emb_subdir_file;
      emb_path.replace_extension("dat");
      boost::filesystem::create_directories(emb_path.parent_path());
      /*
      if (!boost::filesystem::exists(emb_path.parent_path()))
      {
      //cout << "mkdir " << emb_path.parent_path() << endl;
      boost::filesystem::create_directories(emb_path.parent_path());
		}
		*/
		ptree &xml_embed = embeds.add ("embedding", "");
		char embed_str[2048]={0};
		float embed_val;
		std::string s, embed_stdstr;

		//  write embedding string to xml
		for (long r = 0; r < embedded.nr(); ++r)
		{
			// loop over all the columns
			for (long c = 0; c < embedded.nc(); ++c)
			{
				embed_val = embedded (r,c);
				s = boost::lexical_cast<std::string>(embed_val);
				embed_stdstr.append (" ");
				embed_stdstr.append (s);
				// sprintf (embed_str + strlen (embed_str), " %a", embed_val);
			}
		}
		std::vector <ptree> chips = g_chips;
		xml_embed.add ("embed_val", embed_stdstr);
		xml_embed.add ("<xmlattr>.file", emb_path.string());
		xml_embed.add ("label", obj_labels[i]);
		xml_embed.add ("chip_source", img_str);
		int chip_index = find_chip_index (chips, img_str);
		if (chip_index >= 0)
			xml_embed.add_child ("chip", chips[chip_index]);

		serialize(emb_path.string()) << embedded;
		//cout << "Embedded: " << emb_path.string() << endl;
		if (0) //((i==0) && (j==0))
		{
		  cout << "Check embedding for " << img_str << endl;
		  matrix<float,0,1> new_embedded;
		  deserialize(emb_path.string()) >> new_embedded;
		  cout << "embedded: " << endl;
		  cout << csv << embedded << endl;
		  cout << "new embedded: " << endl;
		  cout << csv << new_embedded << endl;
		  if (embedded == new_embedded)
		  {
			cout << "YAY!" << endl;
		  } else
		  {
			cout << "BOO!" << endl;
		  }
		}
	}
  }

  boost::filesystem::path xml_file (parser[0]);
  std::string embed_xml_file;
  if (xml_file.has_parent_path ())
	  embed_xml_file = xml_file.parent_path().string() + "/";
  embed_xml_file += xml_file.filename().stem().string() + "_embeds.xml";

  xml_add_headers (); // put at end since writing reverse order added
  // boost::property_tree::xml_writer_settings<char> settings ('\t', 1);
  boost::property_tree::xml_writer_settings<std::string> settings (' ', 4);
  write_xml(embed_xml_file, g_xml_tree,std::locale(),settings);
	/*
  write_xml(embed_xml_file, g_xml_tree,std::locale(),
  boost::property_tree::xml_writer_make_settings<std::string>(' ', 4, "utf-8"));
	*/
  cout << "\ngenerated: \n\t-- " << embed_xml_file << endl;
  cout << "\n\t-- " << embeddings_count << " embedding files under " << dst_path << "\n" << endl;
  }
  else
  {
	cout << "Need one of <train|test|embed> to run bearembed." << endl;
  }
  time_t timeEnd = time(NULL);

  cout << "End time: " << timeStart << endl;
  cout << "Elapsed time (s): " << difftime(timeEnd, timeStart) << endl;
  }

  catch (exception& e)
  {
	// Note that this will catch any cmd_line_parse_error exceptions and print
	// the default message.
	cout << e.what() << endl;
  }

}


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

// Copyright (c) 2015-2018, Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_MULTIPOINT_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_MULTIPOINT_HPP

#include <cstddef>
#include <algorithm>
#include <utility>
#include <vector>

#include <boost/algorithm/minmax_element.hpp>
#include <boost/range.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/tags.hpp>

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

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

#include <boost/geometry/algorithms/detail/envelope/box.hpp>
#include <boost/geometry/algorithms/detail/envelope/initialize.hpp>
#include <boost/geometry/algorithms/detail/envelope/range.hpp>
#include <boost/geometry/algorithms/detail/expand/point.hpp>

#include <boost/geometry/strategies/cartesian/envelope_point.hpp>
#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/spherical/envelope_box.hpp>
#include <boost/geometry/strategies/spherical/envelope_point.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace envelope
{

class spherical_multipoint
{
private:
    template <std::size_t Dim>
    struct coordinate_less
    {
        template <typename Point>
        inline bool operator()(Point const& point1, Point const& point2) const
        {
            return math::smaller(geometry::get<Dim>(point1),
                                 geometry::get<Dim>(point2));
        }
    };

    template <typename Constants, typename MultiPoint, typename OutputIterator>
    static inline void analyze_point_coordinates(MultiPoint const& multipoint,
                                                 bool& has_south_pole,
                                                 bool& has_north_pole,
                                                 OutputIterator oit)
    {
        typedef typename boost::range_value<MultiPoint>::type point_type;
        typedef typename boost::range_iterator
            <
                MultiPoint const
            >::type iterator_type;

        // analyze point coordinates:
        // (1) normalize point coordinates
        // (2) check if any point is the north or the south pole
        // (3) put all non-pole points in a container
        //
        // notice that at this point in the algorithm, we have at
        // least two points on the spheroid
        has_south_pole = false;
        has_north_pole = false;

        for (iterator_type it = boost::begin(multipoint);
             it != boost::end(multipoint);
             ++it)
        {
            point_type point;
            normalize::spherical_point::apply(*it, point);

            if (math::equals(geometry::get<1>(point),
                             Constants::min_latitude()))
            {
                has_south_pole = true;
            }
            else if (math::equals(geometry::get<1>(point),
                                  Constants::max_latitude()))
            {
                has_north_pole = true;
            }
            else
            {
                *oit++ = point;
            }
        }
    }

    template <typename SortedRange, typename Value>
    static inline Value maximum_gap(SortedRange const& sorted_range,
                                    Value& max_gap_left,
                                    Value& max_gap_right)
    {
        typedef typename boost::range_iterator
            <
                SortedRange const
            >::type iterator_type;

        iterator_type it1 = boost::begin(sorted_range), it2 = it1;
        ++it2;
        max_gap_left = geometry::get<0>(*it1);
        max_gap_right = geometry::get<0>(*it2);

        Value max_gap = max_gap_right - max_gap_left;
        for (++it1, ++it2; it2 != boost::end(sorted_range); ++it1, ++it2)
        {
            Value gap = geometry::get<0>(*it2) - geometry::get<0>(*it1);
            if (math::larger(gap, max_gap))
            {
                max_gap_left = geometry::get<0>(*it1);
                max_gap_right = geometry::get<0>(*it2);
                max_gap = gap;
            }
        }

        return max_gap;
    }

    template
    <
        typename Constants,
        typename PointRange,
        typename LongitudeLess,
        typename CoordinateType
    >
    static inline void get_min_max_longitudes(PointRange& range,
                                              LongitudeLess const& lon_less,
                                              CoordinateType& lon_min,
                                              CoordinateType& lon_max)
    {
        typedef typename boost::range_iterator
            <
                PointRange const
            >::type iterator_type;

        // compute min and max longitude values
        std::pair<iterator_type, iterator_type> min_max_longitudes
            = boost::minmax_element(boost::begin(range),
                                    boost::end(range),
                                    lon_less);

        lon_min = geometry::get<0>(*min_max_longitudes.first);
        lon_max = geometry::get<0>(*min_max_longitudes.second);

        // if the longitude span is "large" compute the true maximum gap
        if (math::larger(lon_max - lon_min, Constants::half_period()))
        {
            std::sort(boost::begin(range), boost::end(range), lon_less);

            CoordinateType max_gap_left = 0, max_gap_right = 0;
            CoordinateType max_gap
                = maximum_gap(range, max_gap_left, max_gap_right);

            CoordinateType complement_gap
                = Constants::period() + lon_min - lon_max;

            if (math::larger(max_gap, complement_gap))
            {
                lon_min = max_gap_right;
                lon_max = max_gap_left + Constants::period();
            }
        }
    }

    template
    <
        typename Constants,
        typename Iterator,
        typename LatitudeLess,
        typename CoordinateType
    >
    static inline void get_min_max_latitudes(Iterator const first,
                                             Iterator const last,
                                             LatitudeLess const& lat_less,
                                             bool has_south_pole,
                                             bool has_north_pole,
                                             CoordinateType& lat_min,
                                             CoordinateType& lat_max)
    {
        if (has_south_pole && has_north_pole)
        {
            lat_min = Constants::min_latitude();
            lat_max = Constants::max_latitude();
        }
        else if (has_south_pole)
        {
            lat_min = Constants::min_latitude();
            lat_max
                = geometry::get<1>(*std::max_element(first, last, lat_less));
        }
        else if (has_north_pole)
        {
            lat_min
                = geometry::get<1>(*std::min_element(first, last, lat_less));
            lat_max = Constants::max_latitude();
        }
        else
        {
            std::pair<Iterator, Iterator> min_max_latitudes
                = boost::minmax_element(first, last, lat_less);

            lat_min = geometry::get<1>(*min_max_latitudes.first);
            lat_max = geometry::get<1>(*min_max_latitudes.second);
        }
    }

public:
    template <typename MultiPoint, typename Box>
    static inline void apply(MultiPoint const& multipoint, Box& mbr)
    {
        typedef typename point_type<MultiPoint>::type point_type;
        typedef typename coordinate_type<MultiPoint>::type coordinate_type;
        typedef typename boost::range_iterator
            <
                MultiPoint const
            >::type iterator_type;

        typedef math::detail::constants_on_spheroid
            <
                coordinate_type,
                typename geometry::detail::cs_angular_units<MultiPoint>::type
            > constants;

        if (boost::empty(multipoint))
        {
            geometry::detail::envelope::initialize<Box, 0, dimension<Box>::value>::apply(mbr);
            return;
        }

        geometry::detail::envelope::initialize<Box, 0, 2>::apply(mbr);

        if (boost::size(multipoint) == 1)
        {
            return dispatch::envelope
                <
                    typename boost::range_value<MultiPoint>::type
                >::apply(range::front(multipoint), mbr, strategy::envelope::spherical_point());
        }

        // analyze the points and put the non-pole ones in the
        // points vector
        std::vector<point_type> points;
        bool has_north_pole = false, has_south_pole = false;

        analyze_point_coordinates<constants>(multipoint,
                                             has_south_pole, has_north_pole,
                                             std::back_inserter(points));

        coordinate_type lon_min, lat_min, lon_max, lat_max;
        if (points.size() == 1)
        {
            // we have one non-pole point and at least one pole point
            lon_min = geometry::get<0>(range::front(points));
            lon_max = geometry::get<0>(range::front(points));
            lat_min = has_south_pole
                ? constants::min_latitude()
                : constants::max_latitude();
            lat_max = has_north_pole
                ? constants::max_latitude()
                : constants::min_latitude();
        }
        else if (points.empty())
        {
            // all points are pole points
            BOOST_GEOMETRY_ASSERT(has_south_pole || has_north_pole);
            lon_min = coordinate_type(0);
            lon_max = coordinate_type(0);
            lat_min = has_south_pole
                ? constants::min_latitude()
                : constants::max_latitude();
            lat_max = (has_north_pole)
                ? constants::max_latitude()
                : constants::min_latitude();
        }
        else
        {
            get_min_max_longitudes<constants>(points,
                                              coordinate_less<0>(),
                                              lon_min,
                                              lon_max);

            get_min_max_latitudes<constants>(points.begin(),
                                             points.end(),
                                             coordinate_less<1>(),
                                             has_south_pole,
                                             has_north_pole,
                                             lat_min,
                                             lat_max);
        }

        typedef typename helper_geometry
            <
                Box,
                coordinate_type,
                typename geometry::detail::cs_angular_units<MultiPoint>::type
            >::type helper_box_type;

        helper_box_type helper_mbr;

        geometry::set<min_corner, 0>(helper_mbr, lon_min);
        geometry::set<min_corner, 1>(helper_mbr, lat_min);
        geometry::set<max_corner, 0>(helper_mbr, lon_max);
        geometry::set<max_corner, 1>(helper_mbr, lat_max);

        // now transform to output MBR (per index)
        geometry::detail::envelope::envelope_indexed_box_on_spheroid<min_corner, 2>::apply(helper_mbr, mbr);
        geometry::detail::envelope::envelope_indexed_box_on_spheroid<max_corner, 2>::apply(helper_mbr, mbr);

        // compute envelope for higher coordinates
        iterator_type it = boost::begin(multipoint);
        geometry::detail::envelope::envelope_one_point<2, dimension<Box>::value>::apply(*it, mbr);

        for (++it; it != boost::end(multipoint); ++it)
        {
            strategy::expand::detail::point_loop
                <
                    2, dimension<Box>::value
                >::apply(mbr, *it);
        }
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename CalculationType>
struct default_strategy<multi_point_tag, spherical_equatorial_tag, CalculationType>
{
    typedef strategy::envelope::spherical_multipoint type;
};

template <typename CalculationType>
struct default_strategy<multi_point_tag, spherical_polar_tag, CalculationType>
{
    typedef strategy::envelope::spherical_multipoint type;
};

template <typename CalculationType>
struct default_strategy<multi_point_tag, geographic_tag, CalculationType>
{
    typedef strategy::envelope::spherical_multipoint type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::envelope

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_MULTIPOINT_HPP


/*
 * lorenz_multi.cpp
 * Date: 2016-02-26
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include "generate_data.hpp"
#include "tree_types.hpp"
#include "program_options.hpp"
#include "serialize.hpp"

#include <gpcxx/app.hpp>
#include <gpcxx/eval.hpp>
#include <gpcxx/evolve.hpp>
#include <gpcxx/tree.hpp>
#include <gpcxx/intrusive_nodes.hpp>
#include <gpcxx/operator.hpp>
#include <gpcxx/stat.hpp>
#include <gpcxx/generate.hpp>
#include <gpcxx/io.hpp>

#include <boost/program_options.hpp>

#include <iostream>

namespace po = boost::program_options;

int main( int argc , char** argv )
{
    auto options = dynsys::get_options();
    auto positional_options = dynsys::get_positional_options();

    po::options_description cmdline_options;
    cmdline_options.add( options ).add( positional_options.second );

    po::variables_map vm;
    try
    {
        po::store( po::command_line_parser( argc , argv ).options( cmdline_options ).positional( positional_options.first ).run() , vm );
        po::notify( vm );
    }
    catch( std::exception& e )
    {
        std::cerr << "Error " << e.what() << "\n\n";
        std::cerr << cmdline_options << "\n";
        return -1;
    }

    dynsys::rng_type rng;

    //[ create_lorenz_data
    auto training_data = dynsys::generate_data();

    std::array< std::pair< double , double > , dynsys::dim > xstat , ystat;
    for( size_t i=0 ; i<dynsys::dim ; ++i )
    {
        xstat[i].first = ystat[i].first = 0.0;
        xstat[i].second = ystat[i].second = 1.0;
    }
    if( vm.count( "normalize" ) )
    {
        xstat = dynsys::normalize_data( training_data.first );
        ystat = dynsys::normalize_data( training_data.second );
    }
    // plot_data( training_data );
    //]

    //[ lorenz_define_node_generator
    auto node_generator = dynsys::create_node_generator();
    //]

    //[ define_gp_parameters
//     size_t population_size = 256 ;
//     size_t generation_size = 20;
    size_t population_size = 512 * 32;
    size_t generation_size = 2000;

    size_t number_elite = 1;
    double mutation_rate = 0.2;
    double crossover_rate = 0.6;
    double reproduction_rate = 0.3;
    size_t min_tree_height = 1 , max_tree_height = 8;
    size_t tournament_size = 15;
    //]

    //[ define_population_and_fitness
    using population_type = std::vector< dynsys::individual_type >;
    using fitness_type = std::vector< double >;

    fitness_type fitness ( population_size , 0.0 );
    population_type population ( population_size );
    //]

    //[ define_evolution
    using evolver_type = gpcxx::dynamic_pipeline< population_type , fitness_type , dynsys::rng_type >;
    evolver_type evolver( rng , number_elite );
    //]

    //[define_evaluator

    auto evaluator = []( auto const& individual , auto const& context ) {
        return dynsys::state_type {{
            individual[0].root()->eval( context ) ,
            individual[1].root()->eval( context ) ,
            individual[2].root()->eval( context ) }}; };

    auto distance = []( auto const& y , auto const& ytrain ) {
        const double weight_x = 1.0;
        const double weight_y = 1.0;
        const double weight_z = 1.0;
        dynsys::state_type diff {{ y[0] - ytrain[0] , y[1] - ytrain[1] , y[2] - ytrain[2] }};
        return weight_x * diff[0] * diff[0] + weight_y * diff[1] * diff[1] + weight_z * diff[2] * diff[2]; };
    //]

    //[define_genetic_operators
    auto tree_generator = gpcxx::make_ramp( rng , node_generator , min_tree_height , max_tree_height , 0.5 );
    auto fitness_f1 = gpcxx::make_multi_regression_fitness( evaluator , distance );
    auto fitness_f = [ fitness_f1 ]( auto const& individual , auto const& x , auto const& y ) {
        return fitness_f1.get_chi2( individual , x , y ); };
    evolver.add_operator(
        gpcxx::make_multi_mutation(
            rng ,
            gpcxx::make_point_mutation( rng , tree_generator , max_tree_height , 20 ) ,
            gpcxx::make_tournament_selector( rng , tournament_size )
        ) , mutation_rate );
    evolver.add_operator(
        gpcxx::make_multi_crossover(
            rng ,
            gpcxx::make_one_point_crossover_strategy( rng , 10 ) ,
            gpcxx::make_tournament_selector( rng , tournament_size )
        ) , crossover_rate );
    evolver.add_operator(
        gpcxx::make_reproduce(
            gpcxx::make_tournament_selector( rng , tournament_size )
        ) , reproduction_rate );
    //]

    //[init_population
    std::ofstream fout { vm[ "evolution" ].as< std::string >() };
    for( size_t i=0 ; i<population.size() ; )
    {
        for( size_t j=0 ; j<dynsys::dim ; ++j )
        {
            population[i][j].clear();
            tree_generator( population[i][j] );
        }
        fitness[i] = fitness_f( population[i] , training_data.first , training_data.second );
        if( ! dynsys::is_number( fitness[i] ) )
        {
            continue;
        }
        ++i;
    }

    std::cout << "Initial population" << std::endl;
    dynsys::write_best_individuals( std::cout , population , fitness , 10 );
    dynsys::write_best_individuals( fout , population , fitness , 10 , true );
    //]

    //[main_loop
    for( size_t i=0 ; i<generation_size ; ++i )
    {
        evolver.next_generation( population , fitness );
        for( size_t i=0 ; i<population.size() ; ++i )
            fitness[i] = fitness_f( population[i] , training_data.first , training_data.second );

        std::cout << "Iteration " << i << std::endl;
        dynsys::write_best_individuals( std::cout , population , fitness , 10 );
        dynsys::write_best_individuals( fout , population , fitness , 10 , true );

        auto min_fitness = * ( std::min_element( fitness.begin() , fitness.end() ) );
        if( std::abs( min_fitness ) < 1.0e-7 )
        {
            std::cout << "Minimal fitness is small then 1.0e-7. Stopping now." << std::endl << std::endl << std::endl << std::endl;
            fout << "Minimal fitness is small then 1.0e-7. Stopping now." << std::endl << std::endl << std::endl << std::endl;
            break;
        }
    }
    //]

    std::ofstream fout2 { vm[ "result" ].as< std::string >() };
    fout2.precision( 14 );
    for( size_t j=0 ; j<dynsys::dim ; ++j )
        fout2 << xstat[j].first << " " << xstat[j].second << " " << ystat[j].first << " " << ystat[j].second << "\n";
    fout2 << std::endl << std::endl;
    dynsys::write_best_individuals( fout2 , population , fitness , 10 , true );

    std::vector< size_t > idx;
    gpcxx::sort_indices( fitness , idx );
    std::ofstream winner_out { vm[ "winner" ].as< std::string >() };
    winner_out << dynsys::serialize_winner( population[ idx[0] ] , xstat , ystat ) << "\n";

    return 0;
}