/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     visibility_graph.cpp
* \author   Collin Johnson
*
* Definition of VisibilityGraph.
*/

#include <utils/visibility_graph.h>
#include <utils/algorithm_ext.h>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/degree_centrality.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/page_rank.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <algorithm>
#include <iostream>
#include <unordered_set>

namespace vulcan
{
namespace utils
{

using namespace boost;

// Define property map for storing the distances between nodes
using DistProperty = exterior_vertex_property<VisGraphType, int>;
using DistVec = DistProperty::container_type;
using DistMatrix = DistProperty::matrix_type;
using DistMap = DistProperty::matrix_map_type;

// Define a property map for storing the calculated feature values
using FeatureProperty = exterior_vertex_property<VisGraphType, float>;
using FeatureContainer = FeatureProperty::container_type;
using FeatureMap = FeatureProperty::map_type;

// Types for creating subgraphs easily
using VertexSet = PointSet<int>;

struct VertexSetVertex
{
    const VertexSet* idSet;
    const VisGraphType* graph;

    bool operator()(VisGraphType::vertex_descriptor vertex) const
    {
        return idSet->find((*graph)[vertex].position) != idSet->end();
    }

    explicit VertexSetVertex(const VertexSet* idSet= nullptr,
                             const VisGraphType* graph = nullptr)
    : idSet(idSet)
    , graph(graph)
    {
    }
};

struct VertexSetEdge
{
    const VertexSet* idSet;
    const VisGraphType* graph;

    bool operator()(VisGraphType::edge_descriptor edge) const
    {
        return (idSet->find((*graph)[source(edge, *graph)].position) != idSet->end())
            && (idSet->find((*graph)[target(edge, *graph)].position) != idSet->end());
    }

    explicit VertexSetEdge(const VertexSet* idSet= nullptr,
                           const VisGraphType* graph = nullptr)
    : idSet(idSet)
    , graph(graph)
    {
    }
};

void all_pairs_paths(const VisGraphType& graph, DistMap& distanceMap);
VisibilityGraphFeature feature_map_to_graph_feature(const FeatureMap& featMap,
                                                    const VisGraphType& graph,
                                                    VisibilityGraphFeatureType type);

std::tuple<double, double> edge_count_stats(const DistMatrix& distMap, int numVertices);

/////////////////// VisibilityGraph implementation //////////////////////////

VisibilityGraph::VisibilityGraph(const std::vector<VisGraphVertex>& points, const std::vector<std::pair<int, int>>& edges)
: graph_(points.size())
, vertices_(points)
{
    for(std::size_t n = 0; n < points.size(); ++n)
    {
        graph_[n].position = points[n];
    }

    for(auto& edge : edges)
    {
        auto desc = add_edge(edge.first, edge.second, graph_);
        if(desc.second)
        {
            graph_[desc.first].distance = distance_between_points(vertices_[edge.first], vertices_[edge.second]);
            edges_.push_back(std::make_pair(vertices_[edge.first], vertices_[edge.second]));
        }
    }
}

VisibilityGraphFeature VisibilityGraph::calculateFeature(VisibilityGraphFeatureType type) const
{
    int cachedFeatureIndex = findCachedFeatureIndex(type);
    if(cachedFeatureIndex != -1)
    {
        return features_[cachedFeatureIndex];
    }

    VisibilityGraphFeature feature;

    switch(type)
    {
    case VisibilityGraphFeatureType::mean_edge_count:
        feature = meanPathCount();
        break;

    case VisibilityGraphFeatureType::clustering_coeff:
        feature = clusteringCoeff();
        break;

    case VisibilityGraphFeatureType::degree_centrality:
        feature = degreeCentrality();
        break;

    case VisibilityGraphFeatureType::closeness_centrality:
        feature = closenessCentrality();
        break;

    case VisibilityGraphFeatureType::betweenness_centrality:
        feature = betweennessCentrality();
        break;

    case VisibilityGraphFeatureType::pagerank:
        feature = pagerank();
        break;

    case VisibilityGraphFeatureType::none:
    case VisibilityGraphFeatureType::num_features:
    default:
        break;
    }

    if(feature.type() != VisibilityGraphFeatureType::none)
    {
        features_.emplace_back(feature);
    }

    return feature;
}

bool VisibilityGraph::isVertex(VisGraphVertex vertex) const
{
    return contains(vertices_, vertex);
}

VisibilityGraph VisibilityGraph::createSubgraph(VisVertexIter begin, VisVertexIter end) const
{
    VertexSet subVerts(begin, end);

    for(auto v : make_iterator_range(begin, end))
    {
        for(auto adj : make_iterator_range(adjacent_vertices(descriptors_.at(v), graph_)))
        {
            subVerts.insert(graph_[adj].position);
        }
    }

    VertexSetVertex vertexFilter(&subVerts, &graph_);
    VertexSetEdge edgeFilter(&subVerts, &graph_);
    auto filtered = make_filtered_graph(graph_, edgeFilter, vertexFilter);

    std::unordered_map<VisGraphType::vertex_descriptor, std::size_t> oldToNew;

    // Copy the filtered graph into a new graph
    VisibilityGraph subgraph;

    // Copy over the vertices that exist in the new subgraph
    for(auto v : make_iterator_range(boost::vertices(filtered)))
    {
        oldToNew[v] = add_vertex(graph_[v], subgraph.graph_);
        subgraph.vertices_.push_back(graph_[v].position);
    }

    // Copy over the edges that exist in the new subgraph
    for(auto e : make_iterator_range(boost::edges(filtered)))
    {
        add_edge(oldToNew[source(e, graph_)], oldToNew[target(e, graph_)], subgraph.graph_);
        subgraph.edges_.push_back(std::make_pair(graph_[source(e, graph_)].position,
                                                 graph_[target(e, graph_)].position));
    }

    return subgraph;
}

int VisibilityGraph::findCachedFeatureIndex(VisibilityGraphFeatureType type) const
{
    for(std::size_t n = 0; n < features_.size(); ++n)
    {
        if(features_[n].type() == type)
        {
            return n;
        }
    }

    return -1;
}

VisibilityGraphFeature VisibilityGraph::meanPathCount(void) const
{
    int numVertices = num_vertices(graph_);

    // Compute the distances between all pairs of vertices
    DistMatrix distances(numVertices);
    DistMap distMap(distances, graph_);
    all_pairs_paths(graph_, distMap);

    FeatureContainer pathDists(numVertices);
    FeatureMap pathDistMap(pathDists, graph_);

    for(int n = 0; n < numVertices; ++n)
    {
        double sum = 0.0;

        for(auto& pathLength : distances[n])
        {
            // Only care about paths leading to other areas
            if((pathLength < numVertices) && (pathLength > 0))
            {
                sum += pathLength;
            }
        }

        pathDistMap[n] = (numVertices > 1) ? sum / (numVertices - 1) : 0.0f;
    }

    double maxMean = *std::max_element(pathDists.begin(), pathDists.end());
    if(maxMean > 0.0)
    {
        for(auto& dist : pathDists)
        {
            dist /= maxMean;
        }
    }

    return feature_map_to_graph_feature(pathDistMap, graph_, VisibilityGraphFeatureType::mean_edge_count);
}

VisibilityGraphFeature VisibilityGraph::clusteringCoeff(void) const
{
    // Compute the degree centrality for graph.
    FeatureContainer coeffs(num_vertices(graph_));
    FeatureMap coeffMap(coeffs, graph_);
    all_clustering_coefficients(graph_, coeffMap);

    return feature_map_to_graph_feature(coeffMap, graph_, VisibilityGraphFeatureType::clustering_coeff);
}

VisibilityGraphFeature VisibilityGraph::degreeCentrality(void) const
{
    // Compute the degree centrality for graph.
    FeatureContainer centralities(num_vertices(graph_));
    FeatureMap centMap(centralities, graph_);
    all_degree_centralities(graph_, centMap);

    double maxDegree = *std::max_element(centralities.begin(), centralities.end());
    // Normalize the degree centrality by the maximum possible value (num verts - 1)
    if(maxDegree > 0.0)
    {
        for(auto& c : centralities)
        {
            c /= maxDegree;
        }
    }

    return feature_map_to_graph_feature(centMap, graph_, VisibilityGraphFeatureType::degree_centrality);
}

VisibilityGraphFeature VisibilityGraph::closenessCentrality(void) const
{
    // Compute the distances between all pairs of vertices
    DistMatrix distances(num_vertices(graph_));
    DistMap distMap(distances, graph_);
    all_pairs_paths(graph_, distMap);

    // Compute the closeness centrality for graph.
    FeatureContainer centralities(num_vertices(graph_));
    FeatureMap centMap(centralities, graph_);
    all_closeness_centralities(graph_, distMap, centMap);

    // Normalize the closeness to have a value of 1 if a node is connected to every other node and decrease
    // from there. The value returned here is 1 / sum(path dists). If fully connected, this sum is (num verts - 1),
    // so just multiply by that to get the normalized value here
    if(num_vertices(graph_) > 1)
    {
        double normalizer = num_vertices(graph_) - 1.0;

        for(auto& c : centralities)
        {
            c *= normalizer;
        }
    }

    return feature_map_to_graph_feature(centMap, graph_, VisibilityGraphFeatureType::closeness_centrality);
}

VisibilityGraphFeature VisibilityGraph::betweennessCentrality(void) const
{
    // Compute the betweenness centrality for graph.
    FeatureContainer centralities(num_vertices(graph_));
    FeatureMap centMap(centralities, graph_);
    brandes_betweenness_centrality(graph_, centMap);

    // Normalize the betweenness values
    if(num_vertices(graph_) > 1)
    {
        double min = *std::min_element(centralities.begin(), centralities.end());
        double max = *std::max_element(centralities.begin(), centralities.end());

        // Scale the betweenness relative to the min and max to make it a relative measure.
        double normalizer = (max > min) ? max - min : 1.0;

        for(auto& c : centralities)
        {
            c = (c - min) / normalizer;
        }
    }

    return feature_map_to_graph_feature(centMap, graph_, VisibilityGraphFeatureType::betweenness_centrality);
}

VisibilityGraphFeature VisibilityGraph::pagerank(void) const
{
    // Compute the betweenness centrality for graph.
    FeatureContainer ranks(num_vertices(graph_));
    FeatureMap rankMap(ranks, graph_);
    page_rank(graph_, rankMap);

    return feature_map_to_graph_feature(rankMap, graph_, VisibilityGraphFeatureType::pagerank);
}

void all_pairs_paths(const VisGraphType& graph, DistMap& distanceMap)
{
    auto uniformCostMap = static_property_map<int>(1);
    johnson_all_pairs_shortest_paths(graph, distanceMap, weight_map(uniformCostMap));
}

VisibilityGraphFeature feature_map_to_graph_feature(const FeatureMap& featMap,
                                                    const VisGraphType& graph,
                                                    VisibilityGraphFeatureType type)
{
    std::vector<VisibilityGraphFeature::value_type> values(num_vertices(graph));
    for(int n = 0, end = num_vertices(graph); n < end; ++n)
    {
        values[n] = std::make_pair(graph[n].position, featMap[n]);
    }

    return VisibilityGraphFeature(type, values);
}

std::tuple<double, double> edge_count_stats(const DistMatrix& distMap, int numVertices)
{
    using namespace accumulators;
    accumulator_set<double, stats<tag::mean, tag::variance>> statsAcc;

    for(int n = 0; n < numVertices; ++n)
    {
        for(auto& pathLength : distMap[n])
        {
            // Only care about paths leading to other areas
            if((pathLength < numVertices) && (pathLength > 0))
            {
                statsAcc(pathLength);
            }
        }
    }

    return std::make_pair(mean(statsAcc), std::sqrt(variance(statsAcc)));
}

} // namespace utils
} // namespace vulcan


//  Copyright 2015 Stephan Menzel. 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 "Random.hpp"
#include "ThreadId.hpp"
#include "Assert.hpp"

#include <boost/thread/tss.hpp>
#include <boost/chrono/chrono.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/uuid/uuid_generators.hpp>

namespace moose {
namespace tools {

namespace {

	using prng_type = boost::random::mt19937;
	boost::thread_specific_ptr<prng_type>                      local_gen;
	using uuid_generator_type = boost::uuids::basic_random_generator<prng_type>;
	boost::thread_specific_ptr<uuid_generator_type>            random_uid_generator;

	/// get access to a thread local instance of the PRNG
	inline prng_type* gen() {

		if (!local_gen.get()) {

			// I try to get a high resolution nanosecond clock and taint
			// it somehow with some burned time
			boost::chrono::high_resolution_clock::time_point start = boost::chrono::high_resolution_clock::now();
			unsigned int tid = faked_thread_id(); // this should take long enough...
			boost::chrono::nanoseconds ns = boost::chrono::high_resolution_clock::now() - start;
			local_gen.reset(new prng_type(ns.count() % tid));
		}

		return local_gen.get();
	}

	// get access to a thread local instance of a uuid generator
	inline uuid_generator_type *get_uuid_generator() {

		if (!random_uid_generator.get()) {
			prng_type *prng = gen();
			random_uid_generator.reset(new uuid_generator_type(*prng));
		}

		return random_uid_generator.get();
	}

}

boost::uint64_t urand(const boost::uint64_t n_max) {

	// get the thread local PRNG
	boost::random::mt19937 *prng = gen();
	MOOSE_ASSERT(prng);
	boost::random::uniform_int_distribution<boost::uint64_t> dist(0, n_max);
	return dist(*prng);
}

boost::uint64_t urand(const boost::uint64_t n_min, const boost::uint64_t n_max) {

	MOOSE_ASSERT_MSG((n_min < n_max), "minimum value must be lower than maximum value when calling moose::tools::urand()");

	// get the thread local PRNG
	boost::random::mt19937 *prng = gen();
	MOOSE_ASSERT(prng);
	boost::random::uniform_int_distribution<boost::uint64_t> dist(n_min, n_max);
	return dist(*prng);
}

boost::uint64_t urand() {

	return moose::tools::urand(std::numeric_limits<boost::uint64_t>::max());
}

boost::uuids::uuid ruuid() {

	uuid_generator_type *gen = get_uuid_generator();
	return (*gen)();
};

static unsigned long x = 123456789;
static unsigned long y = 362436069;
static unsigned long z = 521288629;

unsigned long xorshf96() {          // period 2^96-1

	unsigned long t;
	x ^= x << 16;
	x ^= x >> 5;
	x ^= x << 1;

	t = x;
	x = y;
	y = z;
	z = t ^ x ^ y;

	return z;
}

}
}


#include <gtest/gtest.h>

#include <Eigen/Core>
#include <algorithm>
#include <csim/init_ops.hpp>
#include <csim/memory_ops.hpp>
#include <csim/stat_ops.hpp>
#include <csim/update_ops.hpp>
#include <csim/update_ops_cpp.hpp>
#include <string>

#include "../util/util.hpp"

void test_single_dense_matrix_gate(
    std::function<void(UINT, const CTYPE*, CTYPE*, ITYPE)> func) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit dense matrix gate
        // NOTE: Eigen uses column major by default. To use raw-data of eigen
        // matrix, we need to specify RowMajor.
        target = rand_int(n);
        U = get_eigen_matrix_random_single_qubit_unitary();
        func(target, (CTYPE*)U.data(), state, dim);
        test_state =
            get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
        state_equal(state, test_state, dim, "single dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleDenseMatrixTest) {
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate);
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate_single);
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate_single_unroll);
#ifdef _OPENMP
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate_parallel);
    test_single_dense_matrix_gate(
        single_qubit_dense_matrix_gate_parallel_unroll);
#endif
#ifdef _USE_SIMD
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate_single_simd);
#ifdef _OPENMP
    test_single_dense_matrix_gate(single_qubit_dense_matrix_gate_parallel_simd);
#endif
#endif
}

void test_general_dense_matrix_gate(
    std::function<void(const UINT*, UINT, const CTYPE*, CTYPE*, ITYPE)> func) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U1, U2, U3;
    UINT targets[3];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    // general single
    {
        Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> Umerge;
        for (UINT rep = 0; rep < max_repeat; ++rep) {
            // two qubit dense matrix gate
            U1 = get_eigen_matrix_random_single_qubit_unitary();
            std::random_shuffle(index_list.begin(), index_list.end());
            targets[0] = index_list[0];
            Umerge = U1;

            test_state =
                get_expanded_eigen_matrix_with_identity(targets[0], U1, n) *
                test_state;
            func(targets, 1, (CTYPE*)Umerge.data(), state, dim);
            state_equal(
                state, test_state, dim, "single-qubit separable dense gate");
        }
    }
    // general double
    {
        Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

        for (UINT rep = 0; rep < max_repeat; ++rep) {
            // two qubit dense matrix gate
            U1 = get_eigen_matrix_random_single_qubit_unitary();
            U2 = get_eigen_matrix_random_single_qubit_unitary();

            std::random_shuffle(index_list.begin(), index_list.end());
            targets[0] = index_list[0];
            targets[1] = index_list[1];
            Umerge = kronecker_product(U2, U1);

            test_state =
                get_expanded_eigen_matrix_with_identity(targets[1], U2, n) *
                get_expanded_eigen_matrix_with_identity(targets[0], U1, n) *
                test_state;
            func(targets, 2, (CTYPE*)Umerge.data(), state, dim);
            state_equal(
                state, test_state, dim, "two-qubit separable dense gate");
        }
    }
    // general triple
    {
        Eigen::Matrix<std::complex<double>, 8, 8, Eigen::RowMajor> Umerge;

        for (UINT rep = 0; rep < max_repeat; ++rep) {
            // two qubit dense matrix gate
            U1 = get_eigen_matrix_random_single_qubit_unitary();
            U2 = get_eigen_matrix_random_single_qubit_unitary();
            U3 = get_eigen_matrix_random_single_qubit_unitary();

            std::random_shuffle(index_list.begin(), index_list.end());
            targets[0] = index_list[0];
            targets[1] = index_list[1];
            targets[2] = index_list[2];
            Umerge = kronecker_product(U3, kronecker_product(U2, U1));

            test_state =
                get_expanded_eigen_matrix_with_identity(targets[2], U3, n) *
                get_expanded_eigen_matrix_with_identity(targets[1], U2, n) *
                get_expanded_eigen_matrix_with_identity(targets[0], U1, n) *
                test_state;
            func(targets, 3, (CTYPE*)Umerge.data(), state, dim);
            state_equal(
                state, test_state, dim, "three-qubit separable dense gate");
        }
    }
    release_quantum_state(state);
}

TEST(UpdateTest, ThreeQubitDenseMatrixTest) {
    test_general_dense_matrix_gate(multi_qubit_dense_matrix_gate);
    test_general_dense_matrix_gate(multi_qubit_dense_matrix_gate_single);
#ifdef _OPENMP
    test_general_dense_matrix_gate(multi_qubit_dense_matrix_gate_parallel);
#endif
}


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

#ifndef BOOST_MATH_BESSEL_JY_HPP
#define BOOST_MATH_BESSEL_JY_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/hypot.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/math/special_functions/detail/bessel_jy_asym.hpp>
#include <boost/math/special_functions/detail/bessel_jy_series.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <complex>

// Bessel functions of the first and second kind of fractional order

namespace boost { namespace math {

   namespace detail {

      //
      // Simultaneous calculation of A&S 9.2.9 and 9.2.10
      // for use in A&S 9.2.5 and 9.2.6.
      // This series is quick to evaluate, but divergent unless
      // x is very large, in fact it's pretty hard to figure out
      // with any degree of precision when this series actually
      // *will* converge!!  Consequently, we may just have to
      // try it and see...
      //
      template <class T, class Policy>
      bool hankel_PQ(T v, T x, T* p, T* q, const Policy& )
      {
         BOOST_MATH_STD_USING
            T tolerance = 2 * policies::get_epsilon<T, Policy>();
         *p = 1;
         *q = 0;
         T k = 1;
         T z8 = 8 * x;
         T sq = 1;
         T mu = 4 * v * v;
         T term = 1;
         bool ok = true;
         do
         {
            term *= (mu - sq * sq) / (k * z8);
            *q += term;
            k += 1;
            sq += 2;
            T mult = (sq * sq - mu) / (k * z8);
            ok = fabs(mult) < 0.5f;
            term *= mult;
            *p += term;
            k += 1;
            sq += 2;
         }
         while((fabs(term) > tolerance * *p) && ok);
         return ok;
      }

      // Calculate Y(v, x) and Y(v+1, x) by Temme's method, see
      // Temme, Journal of Computational Physics, vol 21, 343 (1976)
      template <typename T, typename Policy>
      int temme_jy(T v, T x, T* Y, T* Y1, const Policy& pol)
      {
         T g, h, p, q, f, coef, sum, sum1, tolerance;
         T a, d, e, sigma;
         unsigned long k;

         BOOST_MATH_STD_USING
            using namespace boost::math::tools;
         using namespace boost::math::constants;

         BOOST_MATH_ASSERT(fabs(v) <= 0.5f);  // precondition for using this routine

         T gp = boost::math::tgamma1pm1(v, pol);
         T gm = boost::math::tgamma1pm1(-v, pol);
         T spv = boost::math::sin_pi(v, pol);
         T spv2 = boost::math::sin_pi(v/2, pol);
         T xp = pow(x/2, v);

         a = log(x / 2);
         sigma = -a * v;
         d = abs(sigma) < tools::epsilon<T>() ?
            T(1) : sinh(sigma) / sigma;
         e = abs(v) < tools::epsilon<T>() ? T(v*pi<T>()*pi<T>() / 2)
            : T(2 * spv2 * spv2 / v);

         T g1 = (v == 0) ? T(-euler<T>()) : T((gp - gm) / ((1 + gp) * (1 + gm) * 2 * v));
         T g2 = (2 + gp + gm) / ((1 + gp) * (1 + gm) * 2);
         T vspv = (fabs(v) < tools::epsilon<T>()) ? T(1/constants::pi<T>()) : T(v / spv);
         f = (g1 * cosh(sigma) - g2 * a * d) * 2 * vspv;

         p = vspv / (xp * (1 + gm));
         q = vspv * xp / (1 + gp);

         g = f + e * q;
         h = p;
         coef = 1;
         sum = coef * g;
         sum1 = coef * h;

         T v2 = v * v;
         T coef_mult = -x * x / 4;

         // series summation
         tolerance = policies::get_epsilon<T, Policy>();
         for (k = 1; k < policies::get_max_series_iterations<Policy>(); k++)
         {
            f = (k * f + p + q) / (k*k - v2);
            p /= k - v;
            q /= k + v;
            g = f + e * q;
            h = p - k * g;
            coef *= coef_mult / k;
            sum += coef * g;
            sum1 += coef * h;
            if (abs(coef * g) < abs(sum) * tolerance)
            {
               break;
            }
         }
         policies::check_series_iterations<T>("boost::math::bessel_jy<%1%>(%1%,%1%) in temme_jy", k, pol);
         *Y = -sum;
         *Y1 = -2 * sum1 / x;

         return 0;
      }

      // Evaluate continued fraction fv = J_(v+1) / J_v, see
      // Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73
      template <typename T, typename Policy>
      int CF1_jy(T v, T x, T* fv, int* sign, const Policy& pol)
      {
         T C, D, f, a, b, delta, tiny, tolerance;
         unsigned long k;
         int s = 1;

         BOOST_MATH_STD_USING

            // |x| <= |v|, CF1_jy converges rapidly
            // |x| > |v|, CF1_jy needs O(|x|) iterations to converge

            // modified Lentz's method, see
            // Lentz, Applied Optics, vol 15, 668 (1976)
            tolerance = 2 * policies::get_epsilon<T, Policy>();
         tiny = sqrt(tools::min_value<T>());
         C = f = tiny;                           // b0 = 0, replace with tiny
         D = 0;
         for (k = 1; k < policies::get_max_series_iterations<Policy>() * 100; k++)
         {
            a = -1;
            b = 2 * (v + k) / x;
            C = b + a / C;
            D = b + a * D;
            if (C == 0) { C = tiny; }
            if (D == 0) { D = tiny; }
            D = 1 / D;
            delta = C * D;
            f *= delta;
            if (D < 0) { s = -s; }
            if (abs(delta - 1) < tolerance)
            { break; }
         }
         policies::check_series_iterations<T>("boost::math::bessel_jy<%1%>(%1%,%1%) in CF1_jy", k / 100, pol);
         *fv = -f;
         *sign = s;                              // sign of denominator

         return 0;
      }
      //
      // This algorithm was originally written by Xiaogang Zhang
      // using std::complex to perform the complex arithmetic.
      // However, that turns out to 10x or more slower than using
      // all real-valued arithmetic, so it's been rewritten using
      // real values only.
      //
      template <typename T, typename Policy>
      int CF2_jy(T v, T x, T* p, T* q, const Policy& pol)
      {
         BOOST_MATH_STD_USING

            T Cr, Ci, Dr, Di, fr, fi, a, br, bi, delta_r, delta_i, temp;
         T tiny;
         unsigned long k;

         // |x| >= |v|, CF2_jy converges rapidly
         // |x| -> 0, CF2_jy fails to converge
         BOOST_MATH_ASSERT(fabs(x) > 1);

         // modified Lentz's method, complex numbers involved, see
         // Lentz, Applied Optics, vol 15, 668 (1976)
         T tolerance = 2 * policies::get_epsilon<T, Policy>();
         tiny = sqrt(tools::min_value<T>());
         Cr = fr = -0.5f / x;
         Ci = fi = 1;
         //Dr = Di = 0;
         T v2 = v * v;
         a = (0.25f - v2) / x; // Note complex this one time only!
         br = 2 * x;
         bi = 2;
         temp = Cr * Cr + 1;
         Ci = bi + a * Cr / temp;
         Cr = br + a / temp;
         Dr = br;
         Di = bi;
         if (fabs(Cr) + fabs(Ci) < tiny) { Cr = tiny; }
         if (fabs(Dr) + fabs(Di) < tiny) { Dr = tiny; }
         temp = Dr * Dr + Di * Di;
         Dr = Dr / temp;
         Di = -Di / temp;
         delta_r = Cr * Dr - Ci * Di;
         delta_i = Ci * Dr + Cr * Di;
         temp = fr;
         fr = temp * delta_r - fi * delta_i;
         fi = temp * delta_i + fi * delta_r;
         for (k = 2; k < policies::get_max_series_iterations<Policy>(); k++)
         {
            a = k - 0.5f;
            a *= a;
            a -= v2;
            bi += 2;
            temp = Cr * Cr + Ci * Ci;
            Cr = br + a * Cr / temp;
            Ci = bi - a * Ci / temp;
            Dr = br + a * Dr;
            Di = bi + a * Di;
            if (fabs(Cr) + fabs(Ci) < tiny) { Cr = tiny; }
            if (fabs(Dr) + fabs(Di) < tiny) { Dr = tiny; }
            temp = Dr * Dr + Di * Di;
            Dr = Dr / temp;
            Di = -Di / temp;
            delta_r = Cr * Dr - Ci * Di;
            delta_i = Ci * Dr + Cr * Di;
            temp = fr;
            fr = temp * delta_r - fi * delta_i;
            fi = temp * delta_i + fi * delta_r;
            if (fabs(delta_r - 1) + fabs(delta_i) < tolerance)
               break;
         }
         policies::check_series_iterations<T>("boost::math::bessel_jy<%1%>(%1%,%1%) in CF2_jy", k, pol);
         *p = fr;
         *q = fi;

         return 0;
      }

      static const int need_j = 1;
      static const int need_y = 2;

      // Compute J(v, x) and Y(v, x) simultaneously by Steed's method, see
      // Barnett et al, Computer Physics Communications, vol 8, 377 (1974)
      template <typename T, typename Policy>
      int bessel_jy(T v, T x, T* J, T* Y, int kind, const Policy& pol)
      {
         BOOST_MATH_ASSERT(x >= 0);

         T u, Jv, Ju, Yv, Yv1, Yu, Yu1(0), fv, fu;
         T W, p, q, gamma, current, prev, next;
         bool reflect = false;
         unsigned n, k;
         int s;
         int org_kind = kind;
         T cp = 0;
         T sp = 0;

         static const char* function = "boost::math::bessel_jy<%1%>(%1%,%1%)";

         BOOST_MATH_STD_USING
            using namespace boost::math::tools;
         using namespace boost::math::constants;

         if (v < 0)
         {
            reflect = true;
            v = -v;                             // v is non-negative from here
         }
         if (v > static_cast<T>((std::numeric_limits<int>::max)()))
         {
            *J = *Y = policies::raise_evaluation_error<T>(function, "Order of Bessel function is too large to evaluate: got %1%", v, pol);
            return 1;
         }
         n = iround(v, pol);
         u = v - n;                              // -1/2 <= u < 1/2

         if(reflect)
         {
            T z = (u + n % 2);
            cp = boost::math::cos_pi(z, pol);
            sp = boost::math::sin_pi(z, pol);
            if(u != 0)
               kind = need_j|need_y;               // need both for reflection formula
         }

         if(x == 0)
         {
            if(v == 0)
               *J = 1;
            else if((u == 0) || !reflect)
               *J = 0;
            else if(kind & need_j)
               *J = policies::raise_domain_error<T>(function, "Value of Bessel J_v(x) is complex-infinity at %1%", x, pol); // complex infinity
            else
               *J = std::numeric_limits<T>::quiet_NaN();  // any value will do, not using J.

            if((kind & need_y) == 0)
               *Y = std::numeric_limits<T>::quiet_NaN();  // any value will do, not using Y.
            else if(v == 0)
               *Y = -policies::raise_overflow_error<T>(function, 0, pol);
            else
               *Y = policies::raise_domain_error<T>(function, "Value of Bessel Y_v(x) is complex-infinity at %1%", x, pol); // complex infinity
            return 1;
         }

         // x is positive until reflection
         W = T(2) / (x * pi<T>());               // Wronskian
         T Yv_scale = 1;
         if(((kind & need_y) == 0) && ((x < 1) || (v > x * x / 4) || (x < 5)))
         {
            //
            // This series will actually converge rapidly for all small
            // x - say up to x < 20 - but the first few terms are large
            // and divergent which leads to large errors :-(
            //
            Jv = bessel_j_small_z_series(v, x, pol);
            Yv = std::numeric_limits<T>::quiet_NaN();
         }
         else if((x < 1) && (u != 0) && (log(policies::get_epsilon<T, Policy>() / 2) > v * log((x/2) * (x/2) / v)))
         {
            // Evaluate using series representations.
            // This is particularly important for x << v as in this
            // area temme_jy may be slow to converge, if it converges at all.
            // Requires x is not an integer.
            if(kind&need_j)
               Jv = bessel_j_small_z_series(v, x, pol);
            else
               Jv = std::numeric_limits<T>::quiet_NaN();
            if((org_kind&need_y && (!reflect || (cp != 0)))
               || (org_kind & need_j && (reflect && (sp != 0))))
            {
               // Only calculate if we need it, and if the reflection formula will actually use it:
               Yv = bessel_y_small_z_series(v, x, &Yv_scale, pol);
            }
            else
               Yv = std::numeric_limits<T>::quiet_NaN();
         }
         else if((u == 0) && (x < policies::get_epsilon<T, Policy>()))
         {
            // Truncated series evaluation for small x and v an integer,
            // much quicker in this area than temme_jy below.
            if(kind&need_j)
               Jv = bessel_j_small_z_series(v, x, pol);
            else
               Jv = std::numeric_limits<T>::quiet_NaN();
            if((org_kind&need_y && (!reflect || (cp != 0)))
               || (org_kind & need_j && (reflect && (sp != 0))))
            {
               // Only calculate if we need it, and if the reflection formula will actually use it:
               Yv = bessel_yn_small_z(n, x, &Yv_scale, pol);
            }
            else
               Yv = std::numeric_limits<T>::quiet_NaN();
         }
         else if(asymptotic_bessel_large_x_limit(v, x))
         {
            if(kind&need_y)
            {
               Yv = asymptotic_bessel_y_large_x_2(v, x, pol);
            }
            else
               Yv = std::numeric_limits<T>::quiet_NaN(); // any value will do, we're not using it.
            if(kind&need_j)
            {
               Jv = asymptotic_bessel_j_large_x_2(v, x, pol);
            }
            else
               Jv = std::numeric_limits<T>::quiet_NaN(); // any value will do, we're not using it.
         }
         else if((x > 8) && hankel_PQ(v, x, &p, &q, pol))
         {
            //
            // Hankel approximation: note that this method works best when x
            // is large, but in that case we end up calculating sines and cosines
            // of large values, with horrendous resulting accuracy.  It is fast though
            // when it works....
            //
            // Normally we calculate sin/cos(chi) where:
            //
            // chi = x - fmod(T(v / 2 + 0.25f), T(2)) * boost::math::constants::pi<T>();
            //
            // But this introduces large errors, so use sin/cos addition formulae to
            // improve accuracy:
            //
            T mod_v = fmod(T(v / 2 + 0.25f), T(2));
            T sx = sin(x);
            T cx = cos(x);
            T sv = boost::math::sin_pi(mod_v, pol);
            T cv = boost::math::cos_pi(mod_v, pol);

            T sc = sx * cv - sv * cx; // == sin(chi);
            T cc = cx * cv + sx * sv; // == cos(chi);
            T chi = boost::math::constants::root_two<T>() / (boost::math::constants::root_pi<T>() * sqrt(x)); //sqrt(2 / (boost::math::constants::pi<T>() * x));
            Yv = chi * (p * sc + q * cc);
            Jv = chi * (p * cc - q * sc);
         }
         else if (x <= 2)                           // x in (0, 2]
         {
            if(temme_jy(u, x, &Yu, &Yu1, pol))             // Temme series
            {
               // domain error:
               *J = *Y = Yu;
               return 1;
            }
            prev = Yu;
            current = Yu1;
            T scale = 1;
            policies::check_series_iterations<T>(function, n, pol);
            for (k = 1; k <= n; k++)            // forward recurrence for Y
            {
               T fact = 2 * (u + k) / x;
               if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
               {
                  scale /= current;
                  prev /= current;
                  current = 1;
               }
               next = fact * current - prev;
               prev = current;
               current = next;
            }
            Yv = prev;
            Yv1 = current;
            if(kind&need_j)
            {
               CF1_jy(v, x, &fv, &s, pol);                 // continued fraction CF1_jy
               Jv = scale * W / (Yv * fv - Yv1);           // Wronskian relation
            }
            else
               Jv = std::numeric_limits<T>::quiet_NaN(); // any value will do, we're not using it.
            Yv_scale = scale;
         }
         else                                    // x in (2, \infty)
         {
            // Get Y(u, x):

            T ratio;
            CF1_jy(v, x, &fv, &s, pol);
            // tiny initial value to prevent overflow
            T init = sqrt(tools::min_value<T>());
            BOOST_MATH_INSTRUMENT_VARIABLE(init);
            prev = fv * s * init;
            current = s * init;
            if(v < max_factorial<T>::value)
            {
               policies::check_series_iterations<T>(function, n, pol);
               for (k = n; k > 0; k--)             // backward recurrence for J
               {
                  next = 2 * (u + k) * current / x - prev;
                  prev = current;
                  current = next;
               }
               ratio = (s * init) / current;     // scaling ratio
               // can also call CF1_jy() to get fu, not much difference in precision
               fu = prev / current;
            }
            else
            {
               //
               // When v is large we may get overflow in this calculation
               // leading to NaN's and other nasty surprises:
               //
               policies::check_series_iterations<T>(function, n, pol);
               bool over = false;
               for (k = n; k > 0; k--)             // backward recurrence for J
               {
                  T t = 2 * (u + k) / x;
                  if((t > 1) && (tools::max_value<T>() / t < current))
                  {
                     over = true;
                     break;
                  }
                  next = t * current - prev;
                  prev = current;
                  current = next;
               }
               if(!over)
               {
                  ratio = (s * init) / current;     // scaling ratio
                  // can also call CF1_jy() to get fu, not much difference in precision
                  fu = prev / current;
               }
               else
               {
                  ratio = 0;
                  fu = 1;
               }
            }
            CF2_jy(u, x, &p, &q, pol);                  // continued fraction CF2_jy
            T t = u / x - fu;                   // t = J'/J
            gamma = (p - t) / q;
            //
            // We can't allow gamma to cancel out to zero completely as it messes up
            // the subsequent logic.  So pretend that one bit didn't cancel out
            // and set to a suitably small value.  The only test case we've been able to
            // find for this, is when v = 8.5 and x = 4*PI.
            //
            if(gamma == 0)
            {
               gamma = u * tools::epsilon<T>() / x;
            }
            BOOST_MATH_INSTRUMENT_VARIABLE(current);
            BOOST_MATH_INSTRUMENT_VARIABLE(W);
            BOOST_MATH_INSTRUMENT_VARIABLE(q);
            BOOST_MATH_INSTRUMENT_VARIABLE(gamma);
            BOOST_MATH_INSTRUMENT_VARIABLE(p);
            BOOST_MATH_INSTRUMENT_VARIABLE(t);
            Ju = sign(current) * sqrt(W / (q + gamma * (p - t)));
            BOOST_MATH_INSTRUMENT_VARIABLE(Ju);

            Jv = Ju * ratio;                    // normalization

            Yu = gamma * Ju;
            Yu1 = Yu * (u/x - p - q/gamma);

            if(kind&need_y)
            {
               // compute Y:
               prev = Yu;
               current = Yu1;
               policies::check_series_iterations<T>(function, n, pol);
               for (k = 1; k <= n; k++)            // forward recurrence for Y
               {
                  T fact = 2 * (u + k) / x;
                  if((tools::max_value<T>() - fabs(prev)) / fact < fabs(current))
                  {
                     prev /= current;
                     Yv_scale /= current;
                     current = 1;
                  }
                  next = fact * current - prev;
                  prev = current;
                  current = next;
               }
               Yv = prev;
            }
            else
               Yv = std::numeric_limits<T>::quiet_NaN(); // any value will do, we're not using it.
         }

         if (reflect)
         {
            if((sp != 0) && (tools::max_value<T>() * fabs(Yv_scale) < fabs(sp * Yv)))
               *J = org_kind & need_j ? T(-sign(sp) * sign(Yv) * (Yv_scale != 0 ? sign(Yv_scale) : 1) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
            else
               *J = cp * Jv - (sp == 0 ? T(0) : T((sp * Yv) / Yv_scale));     // reflection formula
            if((cp != 0) && (tools::max_value<T>() * fabs(Yv_scale) < fabs(cp * Yv)))
               *Y = org_kind & need_y ? T(-sign(cp) * sign(Yv) * (Yv_scale != 0 ? sign(Yv_scale) : 1) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
            else
               *Y = (sp != 0 ? sp * Jv : T(0)) + (cp == 0 ? T(0) : T((cp * Yv) / Yv_scale));
         }
         else
         {
            *J = Jv;
            if(tools::max_value<T>() * fabs(Yv_scale) < fabs(Yv))
               *Y = org_kind & need_y ? T(sign(Yv) * sign(Yv_scale) * policies::raise_overflow_error<T>(function, 0, pol)) : T(0);
            else
               *Y = Yv / Yv_scale;
         }

         return 0;
      }

   } // namespace detail

}} // namespaces

#endif // BOOST_MATH_BESSEL_JY_HPP


// Boost.Geometry

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

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DENSIFY_HPP

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/signed_size_type.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/cross_product.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/arithmetic/normalize.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/formulas/spherical.hpp>
#include <boost/geometry/srs/sphere.hpp>
#include <boost/geometry/strategies/densify.hpp>
#include <boost/geometry/strategies/spherical/get_radius.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace densify
{

/*!
\brief Densification of spherical segment.
\ingroup strategies
\tparam RadiusTypeOrSphere \tparam_radius_or_sphere
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.densify.densify_4_with_strategy densify (with strategy)]
}
 */
template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
{
public:
    // For consistency with area strategy the radius is set to 1
    inline spherical()
        : m_radius(1.0)
    {}

    template <typename RadiusOrSphere>
    explicit inline spherical(RadiusOrSphere const& radius_or_sphere)
        : m_radius(strategy_detail::get_radius
                    <
                        RadiusOrSphere
                    >::apply(radius_or_sphere))
    {}

    template <typename Point, typename AssignPolicy, typename T>
    inline void apply(Point const& p0, Point const& p1, AssignPolicy & policy, T const& length_threshold) const
    {
        typedef typename AssignPolicy::point_type out_point_t;
        typedef typename select_most_precise
            <
                typename coordinate_type<Point>::type,
                typename coordinate_type<out_point_t>::type,
                CalculationType
            >::type calc_t;

        calc_t const c0 = 0;
        calc_t const c1 = 1;
        calc_t const pi = math::pi<calc_t>();

        typedef model::point<calc_t, 3, cs::cartesian> point3d_t;
        point3d_t const xyz0 = formula::sph_to_cart3d<point3d_t>(p0);
        point3d_t const xyz1 = formula::sph_to_cart3d<point3d_t>(p1);
        calc_t const dot01 = geometry::dot_product(xyz0, xyz1);
        calc_t const angle01 = acos(dot01);

        BOOST_GEOMETRY_ASSERT(length_threshold > T(0));

        signed_size_type n = signed_size_type(angle01 * m_radius / length_threshold);
        if (n <= 0)
            return;

        point3d_t axis;
        if (! math::equals(angle01, pi))
        {
            axis = geometry::cross_product(xyz0, xyz1);
            geometry::detail::vec_normalize(axis);
        }
        else // antipodal
        {
            calc_t const half_pi = math::half_pi<calc_t>();
            calc_t const lat = geometry::get_as_radian<1>(p0);

            if (math::equals(lat, half_pi))
            {
                // pointing east, segment lies on prime meridian, going south
                axis = point3d_t(c0, c1, c0);
            }
            else if (math::equals(lat, -half_pi))
            {
                // pointing west, segment lies on prime meridian, going north
                axis = point3d_t(c0, -c1, c0);
            }
            else
            {
                // lon rotated west by pi/2 at equator
                calc_t const lon = geometry::get_as_radian<0>(p0);
                axis = point3d_t(sin(lon), -cos(lon), c0);
            }
        }

        calc_t step = angle01 / (n + 1);

        calc_t a = step;
        for (signed_size_type i = 0 ; i < n ; ++i, a += step)
        {
            // Axis-Angle rotation
            // see: https://en.wikipedia.org/wiki/Axis-angle_representation
            calc_t const cos_a = cos(a);
            calc_t const sin_a = sin(a);
            // cos_a * v
            point3d_t s1 = xyz0;
            geometry::multiply_value(s1, cos_a);
            // sin_a * (n x v)
            point3d_t s2 = geometry::cross_product(axis, xyz0);
            geometry::multiply_value(s2, sin_a);
            // (1 - cos_a)(n.v) * n
            point3d_t s3 = axis;
            geometry::multiply_value(s3, (c1 - cos_a) * geometry::dot_product(axis, xyz0));
            // v_rot = cos_a * v + sin_a * (n x v) + (1 - cos_a)(n.v) * e
            point3d_t v_rot = s1;
            geometry::add_point(v_rot, s2);
            geometry::add_point(v_rot, s3);

            out_point_t p = formula::cart3d_to_sph<out_point_t>(v_rot);
            geometry::detail::conversion::point_to_point
                <
                    Point, out_point_t,
                    2, dimension<out_point_t>::value
                >::apply(p0, p);

            policy.apply(p);
        }
    }

private:
    typename strategy_detail::get_radius
        <
            RadiusTypeOrSphere
        >::type m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <>
struct default_strategy<spherical_equatorial_tag>
{
    typedef strategy::densify::spherical<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DENSIFY_HPP


/*!
 * Copyright (C) tkornuta, IBM Corporation 2015-2019
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
/*!
 * @file mnist_mlnn_features_visualization_test.cpp
 * @brief Program for visualization of features of mlnn layers trained on MNIST digits.
 * @author tkornuta
 * @date:   03-04-2017
 *
 * Copyright (c) 2017, Tomasz Kornuta, IBM Corporation. All rights reserved.
 *
 */

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <data_io/MNISTMatrixImporter.hpp>

#include <logger/Log.hpp>
#include <logger/ConsoleOutput.hpp>
using namespace mic::logger;

#include <application/ApplicationState.hpp>

#include <configuration/ParameterServer.hpp>

#include <opengl/visualization/WindowManager.hpp>
#include <opengl/visualization/WindowGrayscaleBatch.hpp>
#include <opengl/visualization/WindowCollectorChart.hpp>
using namespace mic::opengl::visualization;

// Neural net.
#include <mlnn/BackpropagationNeuralNetwork.hpp>
using namespace mic::mlnn;

// Encoders.
#include <encoders/MatrixXfMatrixXfEncoder.hpp>
#include <encoders/UIntMatrixXfEncoder.hpp>

/// Windows for displaying activations.
WindowGrayscaleBatch<float> *w_conv10, *w_conv11, *w_conv12, *w_conv13, *w_conv14, *w_conv15;
WindowGrayscaleBatch<float> *w_conv20, *w_conv21, *w_conv22, *w_conv23, *w_conv24, *w_conv25;
WindowGrayscaleBatch<float> *w_conv30, *w_conv31, *w_conv32, *w_conv33, *w_conv34, *w_conv35;
/// Window for displaying chart with statistics.
WindowCollectorChart<float>* w_chart;
/// Data collector .
mic::data_io::DataCollectorPtr<std::string, float> collector_ptr;

/// MNIST importer.
mic::data_io::MNISTMatrixImporter<float>* importer;
/// Multi-layer neural network.
BackpropagationNeuralNetwork<float> neural_net;

/// MNIST matrix encoder.
mic::encoders::MatrixXfMatrixXfEncoder* mnist_encoder;
/// Label 2 matrix encoder (1 hot).
mic::encoders::UIntMatrixXfEncoder* label_encoder;

const size_t batch_size = 9;
const char* fileName = "nn_autoencoder_weights_visualization.txt";

/*!
 * \brief Function for batch sampling.
 * \author tkornuta
 */
void batch_function (void) {

/*	if (neural_net.load(fileName)) {
		LOG(LINFO) << "Loaded neural network from a file";
	} else {*/
		{
			/*neural_net.pushLayer(new mic::mlnn::convolution::Cropping<float>(28, 28, 1, 2));
			neural_net.pushLayer(new Linear<float>(24, 24, 1, 10, 1, 1));
			neural_net.pushLayer(new Softmax<float>(10));*/

			neural_net.pushLayer(new mic::mlnn::convolution::Cropping<float>(28, 28, 1, 2));
			neural_net.pushLayer(new Linear<float>(24, 24, 1, 24, 24, 1));
			neural_net.pushLayer(new mic::mlnn::convolution::Padding<float>(24, 24, 1, 2));

			if (!neural_net.verify())
				exit(-1);

			neural_net.setLoss<  mic::neural_nets::loss::SquaredErrorLoss<float> >();
			neural_net.setOptimization<  mic::neural_nets::optimization::Adam<float> >();

		LOG(LINFO) << "Generated new neural network";
	}//: else

	// Import data from datasets.
	if (!importer->importData())
		exit(-1);

	size_t iteration = 0;

	// Retrieve the next minibatch.
	//mic::types::MNISTBatch bt = importer->getNextBatch();
	//importer->setNextSampleIndex(5);

	// Main application loop.
	while (!APP_STATE->Quit()) {

		// If not paused.
		if (!APP_STATE->isPaused()) {

			// If single step mode - pause after the step.
			if (APP_STATE->isSingleStepModeOn())
				APP_STATE->pressPause();

			{ // Enter critical section - with the use of scoped lock from AppState!
				APP_DATA_SYNCHRONIZATION_SCOPED_LOCK();

				// Retrieve the next minibatch.
				mic::types::MNISTBatch<float> bt = importer->getRandomBatch();

				// Encode data.
				mic::types::MatrixXfPtr encoded_batch = mnist_encoder->encodeBatch(bt.data());
				mic::types::MatrixXfPtr encoded_labels = label_encoder->encodeBatch(bt.labels());

				// Train the autoencoder.
				float loss = neural_net.train (encoded_batch, encoded_batch, 0.001, 0.0001);

				if (iteration%10 == 0) {

					std::shared_ptr<mic::mlnn::fully_connected::Linear<float> > lin1 =
							neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(1);
					w_conv10->setBatchUnsynchronized(lin1->getInputActivations());
					w_conv11->setBatchUnsynchronized(lin1->getInputGradientActivations());
					w_conv12->setBatchUnsynchronized(lin1->getWeightActivations());
					w_conv13->setBatchUnsynchronized(lin1->getWeightGradientActivations());
					w_conv14->setBatchUnsynchronized(lin1->getOutputActivations());
					w_conv15->setBatchUnsynchronized(lin1->getOutputGradientActivations());

					w_conv20->setBatchUnsynchronized(lin1->getInverseWeightActivations());
					w_conv21->setBatchUnsynchronized(lin1->getInverseOutputActivations());

					/*std::shared_ptr<Layer<float> > sm1 = neural_net.getLayer(2);
					w_conv34->setBatchUnsynchronized(sm1->getOutputActivations());
					w_conv35->setBatchUnsynchronized(sm1->getOutputGradientActivations());*/

					// Add data to chart window.
					collector_ptr->addDataToContainer("Loss", loss);
					float reconstruction_error = neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(1)->calculateMeanReconstructionError();
					collector_ptr->addDataToContainer("Reconstruction Error", reconstruction_error);
				}//: if

				iteration++;
				//float reconstruction_error = neural_net.getLayer<mic::mlnn::fully_connected::Linear<float> >(1)->calculateMeanReconstructionError();

				LOG(LINFO) << "Iteration: " << iteration << " loss =" << loss;// << " reconstruction error =" << reconstruction_error;
			}//: end of critical section

		}//: if

		// Sleep.
		APP_SLEEP();
	}//: while

}//: image_encoder_and_visualization_test

/*!
 * \brief Main program function. Runs two threads: main (for GLUT) and another one (for data processing).
 * \author tkornuta
 * @param[in] argc Number of parameters (passed to glManaged).
 * @param[in] argv List of parameters (passed to glManaged).
 * @return (not used)
 */
int main(int argc, char* argv[]) {
	// Set console output to logger.
	LOGGER->addOutput(new ConsoleOutput());
	LOG(LINFO) << "Logger initialized. Starting application";

	// Parse parameters.
	PARAM_SERVER->parseApplicationParameters(argc, argv);

	// Initilize application state ("touch it") ;)
	APP_STATE;

	// Load dataset.
	importer = new mic::data_io::MNISTMatrixImporter<float>();
	importer->setBatchSize(batch_size);

	// Initialize the encoders.
	mnist_encoder = new mic::encoders::MatrixXfMatrixXfEncoder(28, 28);
	label_encoder = new mic::encoders::UIntMatrixXfEncoder(10);

	// Set parameters of all property-tree derived objects - USER independent part.
	PARAM_SERVER->loadPropertiesFromConfiguration();

	// Initialize property-dependent variables of all registered property-tree objects - USER dependent part.
	PARAM_SERVER->initializePropertyDependentVariables();

	// Initialize GLUT! :]
	VGL_MANAGER->initializeGLUT(argc, argv);

	// Create batch visualization window.
	w_conv10 = new WindowGrayscaleBatch<float>("Lin1 x", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 50, 256, 256);
	w_conv11 = new WindowGrayscaleBatch<float>("Lin1 dx", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 50, 256, 256);
	w_conv12 = new WindowGrayscaleBatch<float>("Lin1 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 50, 256, 256);
	w_conv13 = new WindowGrayscaleBatch<float>("Lin1 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 50, 256, 256);
	w_conv14 = new WindowGrayscaleBatch<float>("Lin1 y", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1074, 50, 256, 256);
	w_conv15 = new WindowGrayscaleBatch<float>("Lin1 dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 50, 256, 256);

	w_conv20 = new WindowGrayscaleBatch<float>("Lin1 inverse neuron activation", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 336, 256, 256);
	w_conv21 = new WindowGrayscaleBatch<float>("Lin1 inverse output activation", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 336, 256, 256);
	/*w_conv22 = new WindowGrayscaleBatch<float>("Conv2 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 336, 256, 256);
	w_conv23 = new WindowGrayscaleBatch<float>("Conv2 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 336, 256, 256);
	w_conv24 = new WindowGrayscaleBatch<float>("Conv2 y", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1074, 336, 256, 256);
	w_conv25 = new WindowGrayscaleBatch<float>("Conv2 dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 336, 256, 256);

	w_conv30 = new WindowGrayscaleBatch<float>("L1 x", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 50, 622, 256, 256);
	w_conv31 = new WindowGrayscaleBatch<float>("L1 dx", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 316, 622, 256, 256);
	w_conv32 = new WindowGrayscaleBatch<float>("L1 W", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 562, 622, 256, 256);
	w_conv33 = new WindowGrayscaleBatch<float>("L1 dW", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 818, 622, 256, 256);
	w_conv34 = new WindowGrayscaleBatch<float>("SM y", WindowGrayscaleBatch<float>::Norm_None, WindowGrayscaleBatch<float>::Grid_Both, 1074, 622, 256, 256);
	w_conv35 = new WindowGrayscaleBatch<float>("SM dy", WindowGrayscaleBatch<float>::Norm_HotCold, WindowGrayscaleBatch<float>::Grid_Both, 1330, 622, 256, 256);*/

	// Chart.
	w_chart = new WindowCollectorChart<float>("Statistics", 60, 878, 512, 256);
	collector_ptr= std::make_shared < mic::data_io::DataCollector<std::string, float> >( );
	w_chart->setDataCollectorPtr(collector_ptr);

	// Create data containers.
	collector_ptr->createContainer("Loss", mic::types::color_rgba(255, 0, 0, 180));
	collector_ptr->createContainer("Reconstruction Error", mic::types::color_rgba(255, 255, 255, 180));

	boost::thread batch_thread(boost::bind(&batch_function));

	// Start visualization thread.
	VGL_MANAGER->startVisualizationLoop();

	LOG(LINFO) << "Waiting for threads to join...";
	// End test thread.
	batch_thread.join();
	LOG(LINFO) << "Threads joined - ending application";
}//: main