#pragma once

#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <limits>
#include <set>
#include <string>
#include <utility>
#include <vector>

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>

#include <QColor>
#include <QImage>
#include <QPageSize>
#include <QPainter>
#include <QPoint>
#include <QPrinter>
#include <QSizeF>
#include <QString>

#include <q_gil_converter.hpp>

namespace vi
{
    namespace image_segmentation
    {
        using vertex_index  = unsigned;
        using edge_distance = double;
        using segment_index = unsigned;

        enum struct image_direction : unsigned
        {
            unassigned = 0U, none, north, east, south, west
        };

        const auto SEGMENT_COLORS = std::vector<QColor>{
            QColor{178,223,138},
            QColor{106,61,154},
            QColor{188,128,189},
            QColor{128,177,211},
            QColor{141,211,199},
            QColor{255,255,153},
            QColor{202,178,214},
            QColor{51,160,44},
            QColor{190,186,218},
            QColor{177,89,40},
            QColor{227,26,28},
            QColor{31,120,180},
            QColor{166,206,227},
            QColor{255,237,111},
            QColor{251,154,153},
            QColor{255,127,0},
            QColor{253,191,111},
            QColor{251,128,114},
            QColor{252,205,229},
            QColor{255,255,179},
            QColor{204,235,197},
            QColor{179,222,105},
            QColor{253,180,98}
        };

        template <typename pixel_type>
        inline
        double
        compute_pixel_distance(const pixel_type& a, const pixel_type& b)
        {
            using namespace boost::gil;

            return std::sqrt(
                std::pow(static_cast<double>(get_color(a, red_t()))   - static_cast<double>(get_color(b, red_t())),   2) +
                std::pow(static_cast<double>(get_color(a, green_t())) - static_cast<double>(get_color(b, green_t())), 2) +
                std::pow(static_cast<double>(get_color(a, blue_t()))  - static_cast<double>(get_color(b, blue_t())),  2));
        }

        double
        compute_overall_deviation(
            const boost::gil::rgb8_view_t&    image_view,
            const std::vector<segment_index>& segmentation,
            const int                         segment_count)
        {
            struct segment_color_sums_info
            {
                std::size_t              count{};
                double                   red_sum{};
                double                   green_sum{};
                double                   blue_sum{};
                boost::gil::rgb8_pixel_t centroid{};
            };

            double overall_deviation = 0.0;

            std::vector<segment_color_sums_info> segments_color_sums(segment_count);

            auto       pixel       = image_view.begin();
            auto       segment     = segmentation.begin();
            const auto segment_end = segmentation.end();

            while (segment != segment_end)
            {
                auto& segment_color_sums = segments_color_sums[*segment - 1];

                segment_color_sums.count     += 1;
                segment_color_sums.red_sum   += static_cast<double>(boost::gil::get_color(*pixel, boost::gil::red_t()));
                segment_color_sums.green_sum += static_cast<double>(boost::gil::get_color(*pixel, boost::gil::green_t()));
                segment_color_sums.blue_sum  += static_cast<double>(boost::gil::get_color(*pixel, boost::gil::blue_t()));

                ++pixel;
                ++segment;
            }

            for (auto& segment_color_sums : segments_color_sums)
            {
                segment_color_sums.centroid = boost::gil::rgb8_pixel_t{
                    static_cast<unsigned char>(std::round(segment_color_sums.red_sum   / segment_color_sums.count)),
                    static_cast<unsigned char>(std::round(segment_color_sums.green_sum / segment_color_sums.count)),
                    static_cast<unsigned char>(std::round(segment_color_sums.blue_sum  / segment_color_sums.count))};
            }

            pixel   = image_view.begin();
            segment = segmentation.begin();

            while (segment != segment_end)
            {
                const auto& segment_color_sums = segments_color_sums[*segment - 1];

                overall_deviation += compute_pixel_distance(*pixel, segment_color_sums.centroid);

                ++pixel;
                ++segment;
            }

            return overall_deviation;
        }

        double
        compute_edge_value(
            const boost::gil::rgb8_view_t&    image_view,
            const std::vector<segment_index>& segmentation,
            const int                         segment_count)
        {
            double edge_value = 0.0;

            const auto width  = static_cast<std::size_t>(image_view.width());
            const auto height = static_cast<std::size_t>(image_view.height());

            auto pixel_current   = image_view.begin();
            auto pixel_next      = pixel_current + 1;
            auto pixel_below     = pixel_current + width;
            auto segment_current = segmentation.begin();
            auto segment_next    = segment_current + 1;
            auto segment_below   = segment_current + width;

            for (std::size_t y = 0; y < height - 1; ++y)
            {
                for (std::size_t x = 0; x < width - 1; ++x)
                {
                    if (*segment_current != *segment_below)
                    {
                        edge_value += 2.0 * compute_pixel_distance(*pixel_current, *pixel_below);
                    }

                    if (*segment_current != *segment_next)
                    {
                        edge_value += 2.0 * compute_pixel_distance(*pixel_current, *pixel_next);
                    }

                    ++pixel_current;
                    ++pixel_next;
                    ++pixel_below;

                    ++segment_current;
                    ++segment_next;
                    ++segment_below;
                }

                if (*segment_current != *segment_below)
                {
                    edge_value += 2.0 * compute_pixel_distance(*pixel_current, *pixel_below);
                }

                ++pixel_current;
                ++pixel_next;
                ++pixel_below;

                ++segment_current;
                ++segment_next;
                ++segment_below;
            }

            for (std::size_t x = 0; x < width - 1; ++x)
            {
                if (*segment_current != *segment_next)
                {
                    edge_value += 2.0 * compute_pixel_distance(*pixel_current, *pixel_next);
                }

                ++pixel_current;
                ++pixel_next;
                ++pixel_below;

                ++segment_current;
                ++segment_next;
                ++segment_below;
            }

            return edge_value;
        }

        double
        compute_connectivity_measure(
            const boost::gil::rgb8_view_t&    image_view,
            const std::vector<segment_index>& segmentation,
            const int                         segment_count)
        {
            double connectivity_measure = 0.0;

            constexpr double disconnected_neighbor_penalties[] =
            {
                0.0,
                1.0,
                1.0 + 1.0 / 2.0,
                1.0 + 1.0 / 2.0 + 1.0 / 3.0,
                1.0 + 1.0 / 2.0 + 1.0 / 3.0 + 1.0 / 4.0
            };

            const auto width  = static_cast<std::size_t>(image_view.width());
            const auto height = static_cast<std::size_t>(image_view.height());

            auto segment_current = segmentation.begin();
            auto segment_above   = segment_current;
            auto segment_below   = segment_current + width;

            connectivity_measure += disconnected_neighbor_penalties[
                ((*segment_current != *(segment_current + 1)) ? 1 : 0) +
                ((*segment_current != *(segment_below      )) ? 1 : 0)];

            ++segment_current;
            ++segment_below;

            for (std::size_t x = 0; x < width - 2; ++x)
            {
                connectivity_measure += disconnected_neighbor_penalties[
                    ((*segment_current != *(segment_current - 1)) ? 1 : 0) +
                    ((*segment_current != *(segment_current + 1)) ? 1 : 0) +
                    ((*segment_current != *(segment_below      )) ? 1 : 0)];

                ++segment_current;
                ++segment_below;
            }

            connectivity_measure += disconnected_neighbor_penalties[
                ((*segment_current != *(segment_current - 1)) ? 1 : 0) +
                ((*segment_current != *(segment_below      )) ? 1 : 0)];

            ++segment_current;
            ++segment_below;

            for (std::size_t y = 0; y < height - 2; ++y)
            {
                connectivity_measure += disconnected_neighbor_penalties[
                    ((*segment_current != *(segment_above      )) ? 1 : 0) +
                    ((*segment_current != *(segment_current + 1)) ? 1 : 0) +
                    ((*segment_current != *(segment_below      )) ? 1 : 0)];

                ++segment_current;
                ++segment_below;
                ++segment_above;

                for (std::size_t x = 0; x < width - 2; ++x)
                {
                    connectivity_measure += disconnected_neighbor_penalties[
                        ((*segment_current != *(segment_above      )) ? 1 : 0) +
                        ((*segment_current != *(segment_current - 1)) ? 1 : 0) +
                        ((*segment_current != *(segment_current + 1)) ? 1 : 0) +
                        ((*segment_current != *(segment_below      )) ? 1 : 0)];

                    ++segment_current;
                    ++segment_below;
                    ++segment_above;
                }

                connectivity_measure += disconnected_neighbor_penalties[
                    ((*segment_current != *(segment_above      )) ? 1 : 0) +
                    ((*segment_current != *(segment_current - 1)) ? 1 : 0) +
                    ((*segment_current != *(segment_below      )) ? 1 : 0)];

                ++segment_current;
                ++segment_below;
                ++segment_above;
            }

            connectivity_measure += disconnected_neighbor_penalties[
                ((*segment_current != *(segment_above      )) ? 1 : 0) +
                ((*segment_current != *(segment_current + 1)) ? 1 : 0)];

            ++segment_current;
            ++segment_above;

            for (std::size_t x = 0; x < width - 2; ++x)
            {
                connectivity_measure += disconnected_neighbor_penalties[
                    ((*segment_current != *(segment_above      )) ? 1 : 0) +
                    ((*segment_current != *(segment_current - 1)) ? 1 : 0) +
                    ((*segment_current != *(segment_current + 1)) ? 1 : 0)];

                ++segment_current;
                ++segment_above;
            }

            connectivity_measure += disconnected_neighbor_penalties[
                ((*segment_current != *(segment_above      )) ? 1 : 0) +
                ((*segment_current != *(segment_current - 1)) ? 1 : 0)];

            return connectivity_measure;
        }

        void
        render(const boost::gil::rgb8_view_t&    image_view,
               const std::vector<segment_index>& segmentation,
               const std::string&                filename,
               const bool                        render_image,
               const bool                        render_borders,
               const bool                        render_segments)
        {
            const auto width  = static_cast<std::size_t>(image_view.width());
            const auto height = static_cast<std::size_t>(image_view.height());

            QPrinter printer{};
            printer.setOutputFormat(QPrinter::PdfFormat);
            printer.setOutputFileName(filename.c_str());
            printer.setPageMargins(0, 0, 0, 0, QPrinter::Inch);

            printer.setPageSize(QPageSize(
                QSizeF(static_cast<double>(width), static_cast<double>(height)),
                QPageSize::Inch,
                QString(""),
                QPageSize::SizeMatchPolicy::ExactMatch));

            QPainter painter{&printer};

            painter.scale(static_cast<double>(printer.resolution()),
                          static_cast<double>(printer.resolution()));

            if (render_image)
            {
                const QImage input_image_q = q_gil::gil_view_to_qimage(image_view);
                painter.drawImage(QPoint(0, 0), input_image_q);
            }

            if (render_segments)
            {
                for (std::size_t y = 0; y < height; ++y)
                {
                    for (std::size_t x = 0; x < width; ++x)
                    {
                        const auto index = y * width + x;
                        const auto segment = segmentation[index];

                        // Stupid hack to render without dividers:
                        painter.fillRect(QRectF{static_cast<double>(x) - 0.05,
                                                static_cast<double>(y) - 0.05,
                                                1.1,
                                                1.1},
                                         SEGMENT_COLORS[segment % SEGMENT_COLORS.size()]);
                    }
                }
            }

            if (render_borders)
            {
                painter.setPen(
                    QPen{QBrush((render_image || render_segments) ? QColor{42, 254, 39} : Qt::black),
                         0.2,
                         Qt::SolidLine,
                         Qt::RoundCap});

                for (std::size_t y = 0; y < height; ++y)
                {
                    for (std::size_t x = 0; x < width; ++x)
                    {
                        if (x < width - 1 && segmentation[y * width + x] != segmentation[y * width + x + 1])
                        {
                            painter.drawLine(QPointF(static_cast<double>(x) + 1.0, static_cast<double>(y)),
                                             QPointF(static_cast<double>(x) + 1.0, static_cast<double>(y) + 1.0));
                        }

                        if (y < height - 1 && segmentation[y * width + x] != segmentation[(y + 1) * width + x])
                        {
                            painter.drawLine(QPointF(static_cast<double>(x),       static_cast<double>(y) + 1.0),
                                             QPointF(static_cast<double>(x) + 1.0, static_cast<double>(y) + 1.0));
                        }
                    }
                }

                painter.drawRect(QRectF(0.0, 0.0, static_cast<double>(width), static_cast<double>(height)));
            }
        }

        void
        inline
        trace_segment(std::vector<segment_index>&         segments,
                      const std::vector<image_direction>& graph,
                      const std::size_t                   width,
                      const std::size_t                   height,
                      const std::size_t                   x,
                      const std::size_t                   y,
                      const segment_index                 segment,
                      const image_direction               direction = image_direction::none)
        {
            const auto index = y * width + x;

            if (!segments[index])
            {
                segments[index] = segment;

                if ((y > 0 && direction != image_direction::south) &&
                    (graph[index] == image_direction::north || graph[index - width] == image_direction::south))
                {
                    trace_segment(segments, graph, width, height, x, y - 1, segment, image_direction::north);
                }

                if ((y < height - 1 && direction != image_direction::north) &&
                    (graph[index] == image_direction::south || graph[index + width] == image_direction::north))
                {
                    trace_segment(segments, graph, width, height, x, y + 1, segment, image_direction::south);
                }

                if ((x > 0 && direction != image_direction::east) &&
                    (graph[index] == image_direction::west || graph[index - 1] == image_direction::east))
                {
                    trace_segment(segments, graph, width, height, x - 1, y, segment, image_direction::west);
                }

                if ((x < width - 1 && direction != image_direction::west) &&
                    (graph[index] == image_direction::east || graph[index + 1] == image_direction::west))
                {
                    trace_segment(segments, graph, width, height, x + 1, y, segment, image_direction::east);
                }
            }
        }

        std::pair<std::vector<segment_index>, segment_index>
        compile_segmentation_graph(const std::vector<image_direction>& graph,
                                   const std::size_t                   width,
                                   const std::size_t                   height)
        {
            auto segmentation    = std::vector<segment_index>(graph.size());
            auto segment_counter = segment_index{1U};

            for (std::size_t y = 0; y < height; ++y)
            {
                for (std::size_t x = 0; x < width; ++x)
                {
                    const auto index = y * width + x;

                    if (!segmentation[index])
                    {
                        const auto segment = segment_counter++;
                        trace_segment(segmentation, graph, width, height, x, y, segment);
                    }
                }
            }

            return std::make_pair(std::move(segmentation), segment_counter - 1U);
        }

        struct image_distances
        {
            std::size_t         image_width;
            std::size_t         image_height;
            std::vector<double> costs;

            image_distances(const boost::gil::rgb8c_view_t& view)
                : image_width(static_cast<std::size_t>(view.width())),
                  image_height(static_cast<std::size_t>(view.height())),
                  costs((image_width * 2 - 1) * (image_height * 2 - 1),
                        std::numeric_limits<double>::quiet_NaN())
            {
                for (unsigned y = 0; y < image_height; ++y)
                {
                    for (unsigned x = 0; x < image_width; ++x)
                    {
                        if (x > 0)
                        {
                            (*this)(x, y, image_direction::west) = compute_pixel_distance(view(x - 1, y), view(x, y));
                        }

                        if (y > 0)
                        {
                            (*this)(x, y, image_direction::north) = compute_pixel_distance(view(x, y - 1), view(x, y));
                        }
                    }
                }
            }

            inline
            std::size_t
            computePixelDirectionIndex(const std::size_t x, const std::size_t y, const image_direction direction) const
            {
                switch (direction)
                {
                    case image_direction::north:
                        return (y * 2 - 1) * image_width + x * 2;
                    case image_direction::east:
                        return y * 2 * image_width + x * 2 + 1;
                    case image_direction::south:
                        return (y * 2 + 1) * image_width + x * 2;
                    case image_direction::west:
                        return y * 2 * image_width + x * 2 - 1;
                    default:
                        std::exit(-1);
                }
            }

            inline
            double
            maximum() const
            {
                auto max_value = std::numeric_limits<double>::min();

                for (const auto value : costs)
                {
                    if (!std::isnan(value) && value > max_value)
                    {
                        max_value = value;
                    }
                }

                return max_value;
            }

            inline
            double&
            operator()(const std::size_t x, const std::size_t y, const image_direction direction)
            {
                return costs[computePixelDirectionIndex(x, y, direction)];
            }

            inline
            const double&
            operator()(const std::size_t x, const std::size_t y, const image_direction direction) const
            {
                return costs[computePixelDirectionIndex(x, y, direction)];
            }
        };

        struct vertex_cost_comparator
        {
            vertex_cost_comparator(const std::vector<double>& costs)
                : costs{&costs} {}

            inline
            bool
            operator()(const vertex_index vertex_a, const vertex_index vertex_b) const
            {
                const auto cost_a = (*costs)[vertex_a];
                const auto cost_b = (*costs)[vertex_b];
                return cost_a != cost_b ? cost_a < cost_b : vertex_a < vertex_b;
            }

            const std::vector<double>* costs;
        };

        struct edge_distance_comparator
        {
            inline
            bool
            operator()(const std::pair<vertex_index, edge_distance>& a,
                       const std::pair<vertex_index, edge_distance>& b)
            {
                return a.second > b.second;
            }
        };

        template <typename frontier_type>
        inline
        bool
        update_cheapest(
            const std::vector<image_direction>& graph,
            const vertex_index                  parent_vertex,
            const vertex_index                  target_vertex,
            const image_direction               target_edge_direction,
            const double                        distance,
            frontier_type&                      frontier,
            std::vector<double>&                lowest_vertex_costs,
            std::vector<image_direction>&       lowest_cost_edges)
        {
            if (graph[target_vertex] == image_direction::unassigned)
            {
                if (distance < lowest_vertex_costs[target_vertex])
                {
                    if (frontier.count(target_vertex))
                    {
                        frontier.erase(target_vertex);
                    }

                    lowest_vertex_costs[target_vertex] = distance;
                    lowest_cost_edges[target_vertex]   = target_edge_direction;

                    frontier.insert(target_vertex);
                }
            }

            return false;
        }

        template <typename random_generator_type>
        std::vector<image_direction>
        build_minimum_spanning_tree(random_generator_type& random_generator,
                                    const image_distances& distances,
                                    const int              remove_edge_count = 0)
        {
            const auto width  = distances.image_width;
            const auto height = distances.image_height;
            const auto count  = width * height;

            auto graph               = std::vector<image_direction>(count, image_direction::unassigned);
            auto lowest_vertex_costs = std::vector<double>(count, std::numeric_limits<double>::max());
            auto lowest_cost_edges   = std::vector<image_direction>(count, image_direction::unassigned);
            auto highest_cost_edges  = std::set<
                std::pair<vertex_index, edge_distance>, edge_distance_comparator>{};

            vertex_cost_comparator comparator{lowest_vertex_costs};
            std::set<vertex_index, vertex_cost_comparator> frontier{comparator};

            const auto initial_vertex = std::uniform_int_distribution<unsigned>{
                0U, static_cast<unsigned>(count) - 1U}(random_generator);

            frontier.insert(initial_vertex);

            while (!frontier.empty())
            {
                const auto vertex_it = frontier.begin();
                const auto vertex    = *vertex_it;

                frontier.erase(vertex_it);

                const auto cheapest_edge = lowest_cost_edges[vertex];
                graph[vertex] = cheapest_edge != image_direction::unassigned ? cheapest_edge : image_direction::none;

                const auto cheapest_distance = cheapest_edge != image_direction::unassigned ? lowest_vertex_costs[vertex] : 0.0;

                if (remove_edge_count)
                {
                    if (highest_cost_edges.size() < remove_edge_count)
                    {
                        highest_cost_edges.insert(std::make_pair(vertex, cheapest_distance));
                    }
                    else if (cheapest_distance > highest_cost_edges.rend()->second)
                    {
                        highest_cost_edges.erase(--highest_cost_edges.end());
                        highest_cost_edges.insert(std::make_pair(vertex, cheapest_distance));
                    }
                }

                const auto row    = vertex / width;
                const auto column = vertex % width;

                if (row > 0)
                {
                    update_cheapest(graph,
                                    vertex,
                                    vertex - width,
                                    image_direction::south,
                                    distances(column, row, image_direction::north),
                                    frontier,
                                    lowest_vertex_costs,
                                    lowest_cost_edges);
                }

                if (row < height - 1)
                {
                    update_cheapest(graph,
                                    vertex,
                                    vertex + width,
                                    image_direction::north,
                                    distances(column, row, image_direction::south),
                                    frontier,
                                    lowest_vertex_costs,
                                    lowest_cost_edges);
                }

                if (column > 0)
                {
                    update_cheapest(graph,
                                    vertex,
                                    vertex - 1,
                                    image_direction::east,
                                    distances(column, row, image_direction::west),
                                    frontier,
                                    lowest_vertex_costs,
                                    lowest_cost_edges);
                }

                if (column < width - 1)
                {
                    update_cheapest(graph,
                                    vertex,
                                    vertex + 1,
                                    image_direction::west,
                                    distances(column, row, image_direction::east),
                                    frontier,
                                    lowest_vertex_costs,
                                    lowest_cost_edges);
                }
            }

            for (const auto& highest_cost_edge : highest_cost_edges)
            {
                graph[highest_cost_edge.first] = image_direction::none;
            }

            return graph;
        }

        struct moea
        {
            using genotype_type = std::pair<std::vector<vi::image_segmentation::image_direction>, vi::image_segmentation::segment_index>;

            template <typename random_generator_type>
            static
            std::pair<genotype_type, genotype_type>
            crossover_operator(random_generator_type& random_generator,
                               const genotype_type&   parent_a,
                               const genotype_type&   parent_b)
            {
                auto& parent_a_sequence = parent_a.first;
                auto& parent_b_sequence = parent_b.first;

                const auto sequence_length = static_cast<unsigned>(parent_a_sequence.size());

                auto children = std::make_pair(
                    std::make_pair(std::vector<image_direction>(sequence_length, image_direction::unassigned), segment_index{}),
                    std::make_pair(std::vector<image_direction>(sequence_length, image_direction::unassigned), segment_index{}));

                auto& child_a_sequence = children.first.first;
                auto& child_b_sequence = children.second.first;

                const auto crossover_point = std::uniform_int_distribution<unsigned>{
                    0U, sequence_length}(random_generator);

                std::copy(parent_a_sequence.begin(),
                          parent_a_sequence.begin() + crossover_point,
                          child_a_sequence.begin());

                std::copy(parent_b_sequence.begin() + crossover_point,
                          parent_b_sequence.end(),
                          child_a_sequence.begin() + crossover_point);

                std::copy(parent_b_sequence.begin(),
                          parent_b_sequence.begin() + crossover_point,
                          child_b_sequence.begin());

                std::copy(parent_a_sequence.begin() + crossover_point,
                          parent_a_sequence.end(),
                          child_b_sequence.begin() + crossover_point);

                return children;
            }

            static
            boost::gil::rgb8_image_t
            load_image(const std::string& image_filename)
            {
                boost::gil::rgb8_image_t image{};
                boost::gil::jpeg_read_image(image_filename, image);
                return image;
            }

            template <typename random_generator_type>
            static
            void
            mutate_operator(random_generator_type& random_generator,
                            genotype_type&         genotype)
            {
                const auto index = std::uniform_int_distribution<unsigned>{
                    0U, static_cast<unsigned>(genotype.first.size()) - 1U}(random_generator);

                const auto value = static_cast<image_direction>(
                    std::uniform_int_distribution<unsigned>{
                        static_cast<unsigned>(image_direction::none),
                        static_cast<unsigned>(image_direction::west)}(random_generator));

                genotype.first[index] = value;
            }

            boost::gil::rgb8_image_t input_image;
            std::size_t              input_image_width;
            std::size_t              input_image_height;
            image_distances          input_image_distances;
            bool                     evaluate_overall_deviation;
            bool                     evaluate_edge_value;
            bool                     evaluate_connectivity_measure;

            moea(const std::string& input_image_filename,
                 const bool         evaluate_overall_deviation,
                 const bool         evaluate_edge_value,
                 const bool         evaluate_connectivity_measure)
                : input_image{load_image(input_image_filename)},
                  input_image_width{static_cast<std::size_t>(input_image.width())},
                  input_image_height{static_cast<std::size_t>(input_image.height())},
                  input_image_distances{boost::gil::view(input_image)},
                  evaluate_overall_deviation{evaluate_overall_deviation},
                  evaluate_edge_value{evaluate_edge_value},
                  evaluate_connectivity_measure{evaluate_connectivity_measure}
            {
            }

            std::vector<double>
            evaluate(genotype_type& genotype)
            {
                std::vector<double> objective_values{};

                std::vector<segment_index> segmentation{};
                segment_index              segment_count{};

                std::tie(segmentation, segment_count) = compile_segmentation_graph(
                    genotype.first, input_image_width, input_image_height);

                genotype.second = segment_count;

                if (evaluate_overall_deviation)
                {
                    objective_values.push_back(
                        compute_overall_deviation(
                            boost::gil::view(input_image),
                            segmentation,
                            segment_count));
                }

                if (evaluate_edge_value)
                {
                    objective_values.push_back(
                        compute_edge_value(
                            boost::gil::view(input_image),
                            segmentation,
                            segment_count));
                }

                if (evaluate_connectivity_measure)
                {
                    objective_values.push_back(
                        compute_connectivity_measure(
                            boost::gil::view(input_image),
                            segmentation,
                            segment_count));
                }

                return objective_values;
            }

            template <typename random_generator_type>
            genotype_type
            generate(random_generator_type& random_generator)
            {
                return std::make_pair(build_minimum_spanning_tree(
                    random_generator, input_image_distances), segment_index{});
            }
        };
    }
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2015 Andres Hernandez

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file hybridsimulatedannealing.hpp
\brief Implementation based on:
Very Fast Simulated Re-Annealing, Lester Ingber,
Mathl. Comput. Modelling, 967-973, 1989
*/

#ifndef quantlib_optimization_hybridsimulatedannealing_hpp
#define quantlib_optimization_hybridsimulatedannealing_hpp

#include <ql/math/optimization/problem.hpp>
#include <ql/math/optimization/constraint.hpp>
#include <ql/experimental/math/hybridsimulatedannealingfunctors.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    /*! Method is fairly straightforward:
    1) Sampler provides a probability density (based on current value) for the parameters. Each
    iteration a new draw is made from it to find a new point
    2) Probability determines whether the new point, obtained from Sampler, is accepted or not
    3) Temperature is a schedule T(k) for the iteration k, which affects the Sampler and Probability
    4) Reannealing is a departure from the traditional Boltzmann Annealing method: it rescales
    the iteration k independently for each dimension so as to improve convergence

    The hybrid in the name is because one can provide it a local optimizer for use whenever any new
    best point is found or at every accepted point, in which case is used is chose by the user.

    Class Sampler must implement the following interface:
    \code
    void operator()(Array &newPoint, const Array &currentPoint, const Array &temp) const;
    \endcode
    Class Probability must implement the following interface:
    \code
    bool operator()(Real currentValue, Real newValue, const Array &temp) const;
    \endcode
    Class Temperature must implement the following interface:
    \code
    void operator()(Array &newTemp, const Array &currTemp, const Array &steps) const;
    \endcode
    Class Reannealing must implement the following interface:
    \code
    void operator()(Array & steps, const Array &currentPoint,
    Real aCurrentValue, const Array & currTemp) const;
    \endcode
    */
    template <class Sampler, class Probability, class Temperature, class Reannealing = ReannealingTrivial>
    class HybridSimulatedAnnealing : public OptimizationMethod {
      public:
        enum LocalOptimizeScheme {
            NoLocalOptimize,
            EveryNewPoint,
            EveryBestPoint
        };
        enum ResetScheme {
            NoResetScheme,
            ResetToBestPoint,
            ResetToOrigin
        };

        HybridSimulatedAnnealing(const Sampler &sampler,
            const Probability &probability,
            const Temperature &temperature,
            const Reannealing &reannealing = ReannealingTrivial(),
            Real startTemperature = 200.0,
            Real endTemperature = 0.01,
            Size reAnnealSteps = 50,
            ResetScheme resetScheme = ResetToBestPoint,
            Size resetSteps = 150,
            boost::shared_ptr<OptimizationMethod> localOptimizer
            = boost::make_shared<LevenbergMarquardt>(),
            LocalOptimizeScheme optimizeScheme = EveryBestPoint)
            : sampler_(sampler), probability_(probability),
            temperature_(temperature), reannealing_(reannealing),
            startTemperature_(startTemperature), endTemperature_(endTemperature),
            reAnnealSteps_(reAnnealSteps == 0 ? QL_MAX_INTEGER : reAnnealSteps), resetScheme_(resetScheme),
            resetSteps_(resetSteps == 0 ? QL_MAX_INTEGER : resetSteps), localOptimizer_(localOptimizer),
            optimizeScheme_(localOptimizer ? optimizeScheme : NoLocalOptimize) {}

        EndCriteria::Type minimize(Problem &P, const EndCriteria &endCriteria);
    private:
        Sampler sampler_;
        Probability probability_;
        Temperature temperature_;
        Reannealing reannealing_;
        Real startTemperature_;
        Real endTemperature_;
        Size reAnnealSteps_;
        ResetScheme resetScheme_;
        Size resetSteps_;
        boost::shared_ptr<OptimizationMethod> localOptimizer_;
        LocalOptimizeScheme optimizeScheme_;
    };

    template <class Sampler, class Probability, class Temperature, class Reannealing>
    EndCriteria::Type HybridSimulatedAnnealing<Sampler, Probability, Temperature, Reannealing>::minimize(Problem &P, const EndCriteria &endCriteria) {
        EndCriteria::Type ecType = EndCriteria::None;
        P.reset();
        reannealing_.setProblem(P);
        Array x = P.currentValue();
        Size n = x.size();
        Size k = 1;
        Size kStationary = 1;
        Size kReAnneal = 1;
        Size kReset = 1;
        Size maxK = endCriteria.maxIterations();
        Size maxKStationary = endCriteria.maxStationaryStateIterations();
        bool temperatureBreached = false;
        Array currentTemperature(n, startTemperature_);
        Array annealStep(n, 1.0);
        Array bestPoint(x);
        Array currentPoint(x);
        Array startingPoint(x);
        Array newPoint(x);
        Real bestValue = P.value(bestPoint);
        Real currentValue = bestValue;
        Real startingValue = bestValue; //to reset to starting point if desired
        while (k <= maxK && kStationary <= maxKStationary && !temperatureBreached)
        {
            //Draw a new sample point
            sampler_(newPoint, currentPoint, currentTemperature);
            try{
                //Evaluate new point
                Real newValue = P.value(newPoint);

                //Determine if new point is accepted
                if (probability_(currentValue, newValue, currentTemperature)) {
                    if (optimizeScheme_ == EveryNewPoint) {
                        P.setCurrentValue(newPoint);
                        P.setFunctionValue(newValue);
                        localOptimizer_->minimize(P, endCriteria);
                        newPoint = P.currentValue();
                        newValue = P.functionValue();
                    }
                    currentPoint = newPoint;
                    currentValue = newValue;
                }

                //Check if we have a new best point
                if (newValue < bestValue) {
                    if (optimizeScheme_ == EveryBestPoint) {
                        P.setCurrentValue(newPoint);
                        P.setFunctionValue(newValue);
                        localOptimizer_->minimize(P, endCriteria);
                        newPoint = P.currentValue();
                        newValue = P.functionValue();
                    }
                    kStationary = 0;
                    bestValue = newValue;
                    bestPoint = newPoint;
                }
            } catch(...){
                //Do nothing, move on to new draw
            }
            //Increase steps
            k++;
            kStationary++;
            for (Size i = 0; i < annealStep.size(); i++)
                annealStep[i]++;

            //Reanneal if necessary
            if (kReAnneal == reAnnealSteps_) {
                kReAnneal = 0;
                reannealing_(annealStep, currentPoint, currentValue, currentTemperature);
            }
            kReAnneal++;

            //Reset if necessary
            if (kReset == resetSteps_) {
                kReset = 0;
                switch (resetScheme_) {
                case NoResetScheme:
                    break;
                case ResetToOrigin:
                    currentPoint = startingPoint;
                    currentValue = startingValue;
                    break;
                case ResetToBestPoint:
                    currentPoint = bestPoint;
                    currentValue = bestValue;
                    break;
                }
            }
            kReset++;

            //Update the current temperature according to current step
            temperature_(currentTemperature, currentTemperature, annealStep);

            //Check if temperature condition is breached
            for (Size i = 0; i < n; i++)
                temperatureBreached = temperatureBreached && currentTemperature[i] < endTemperature_;
        }

        //Change end criteria type if appropriate
        if (k > maxK)
            ecType = EndCriteria::MaxIterations;
        else if (kStationary > maxKStationary)
            ecType = EndCriteria::StationaryPoint;

        //Set result to best point
        P.setCurrentValue(bestPoint);
        P.setFunctionValue(bestValue);
        return ecType;
    }

    typedef HybridSimulatedAnnealing<SamplerGaussian, ProbabilityBoltzmannDownhill, TemperatureExponential, ReannealingTrivial> GaussianSimulatedAnnealing;
    typedef HybridSimulatedAnnealing<SamplerLogNormal, ProbabilityBoltzmannDownhill, TemperatureExponential, ReannealingTrivial> LogNormalSimulatedAnnealing;
    typedef HybridSimulatedAnnealing<SamplerMirrorGaussian, ProbabilityBoltzmannDownhill, TemperatureExponential, ReannealingTrivial> MirrorGaussianSimulatedAnnealing;
    typedef HybridSimulatedAnnealing<SamplerGaussian, ProbabilityBoltzmannDownhill, TemperatureExponential, ReannealingFiniteDifferences> GaussianSimulatedReAnnealing;
    typedef HybridSimulatedAnnealing<SamplerVeryFastAnnealing, ProbabilityBoltzmannDownhill, TemperatureVeryFastAnnealing, ReannealingTrivial> VeryFastSimulatedAnnealing;
    typedef HybridSimulatedAnnealing<SamplerVeryFastAnnealing, ProbabilityBoltzmannDownhill, TemperatureVeryFastAnnealing, ReannealingFiniteDifferences> VeryFastSimulatedReAnnealing;
}

#endif


// ------------------------------------------------------------------------
//  Copyright (C)
//  Federico Perazzi <perazzif@inf.ethz.ch>
//  April 2016
// ------------------------------------------------------------------------
#include <polycont/cpp/mask2poly.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/python.hpp>
#include <boost/numpy.hpp>

// djikstra
#include <Eigen/Dense>

#include <boost/config.hpp>
#include <iostream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

namespace py = boost::python;
namespace np = boost::numpy;

#include <Eigen/Dense>
#include "cpp/mask2polycont.hpp"

using namespace std;
using namespace PolyCont;

//void mexFunction( int nlhs, mxArray *plhs[],
              //int nrhs, const mxArray*prhs[] )
//{

struct _ContContainer: public ContContainer
{
	py::list contour_coords;

};
_ContContainer _mask2poly(const np::ndarray& pymask, float tolerance) {

		np::dtype dtype = np::dtype::get_builtin<bool>();

		// Required transposition to match matlab impl
		np::ndarray _pymask = np::zeros(py::make_tuple(pymask.shape(1), pymask.shape(0)),
				np::dtype::get_builtin<bool>());

		// Transpose
		for(int i=0; i < _pymask.shape(0); ++i) {
			for(int j=0; j < _pymask.shape(1); ++j) {
				_pymask[i][j] = pymask[j][i];
			}
		}

		Eigen::Map<Eigen::Array<bool,Eigen::Dynamic,Eigen::Dynamic> >
						mask((bool*)_pymask.get_data(),_pymask.shape(1),_pymask.shape(0));

    /* Output contours */
    _ContContainer all_conts;

    /*-------------------------------------------*/
    /*         Call the actual function          */
    /*-------------------------------------------*/
    mask2polycont(mask, all_conts, tolerance);

		for (std::size_t ii=0; ii<all_conts.size(); ++ii) {

		dtype = np::dtype::get_builtin<int>();
			np::ndarray curr_poly = np::empty(
					py::make_tuple(all_conts[ii].size(),2),dtype);
			for (std::size_t jj=0; jj<all_conts[ii].size(); ++jj) {
				curr_poly[jj][0] = all_conts[ii][jj].Y;
				curr_poly[jj][1] = all_conts[ii][jj].X;
			}
			all_conts.contour_coords.append(curr_poly);
		}

		return all_conts;
}

np::ndarray _get_longest_cont(const py::list& contour_coords) {

	int max_id     = -1;
	int max_length = 0;

	for(int jj = 0; jj < py::len(contour_coords); ++jj) {
		np::ndarray arr = boost::python::extract<np::ndarray>(contour_coords[jj]);
		if(max_length <= arr.shape(0)) {
			max_id     = jj;
			max_length = arr.shape(0);
		}
	}
	return boost::python::extract<np::ndarray>(contour_coords[max_id]);;
}

np::ndarray _contour_upsample(const np::ndarray cont, float cont_th) {

	np::ndarray _cont = cont.astype(np::dtype::get_builtin<float>());

	struct Point {float x,y;};
	std::vector<Point> diff(_cont.shape(0)-1);

	const float* ptr_cont = (float*) _cont.get_data();

	for(int i=1; i < cont.shape(0); ++i) {
		diff[i-1].x = ptr_cont[i*2+0] - ptr_cont[(i-1)*2+0];
		diff[i-1].y = ptr_cont[i*2+1] - ptr_cont[(i-1)*2+1];
	}

	std::vector<float> nv(diff.size());

	// Compute length of each segment
	for(int i=0; i<diff.size(); ++i) {
		nv[i] = sqrt(diff[i].x*diff[i].x + diff[i].y*diff[i].y);
	}

	std::vector<Point> up_cont;
	up_cont.push_back(Point{ptr_cont[0],ptr_cont[1]});

	// Now upsample contour
	for(int i=0; i<nv.size(); ++i) {
		if(nv[i] > cont_th) {
			int n_segm = ceil((float)nv[i]/(float)cont_th);
			Point curr_point = up_cont.back();
			Point vec{diff[i].x/(float)n_segm,diff[i].y/(float)n_segm};

			for(int j=0; j < n_segm-1; ++j) {
				curr_point.x += vec.x;
				curr_point.y += vec.y;
				up_cont.push_back(curr_point);
			}
		}
		up_cont.push_back(Point{ptr_cont[(i+1)*2+0],ptr_cont[(i+1)*2+1]});
	}
	// Copy results
	np::ndarray res = np::zeros(py::make_tuple(up_cont.size(), 2),
				np::dtype::get_builtin<float>());

	for(int i = 0; i < up_cont.size(); ++i) {
		res[i][0] = up_cont[i].x;
		res[i][1] = up_cont[i].y;
	}
	return res;
}
// ------------------------------------------------------------------------
// Jordi Pont-Tuset - http://jponttuset.github.io/
// April 2016
// ------------------------------------------------------------------------
// This file is part of the DAVIS package presented in:
//   Federico Perazzi, Jordi Pont-Tuset, Brian McWilliams,
//   Luc Van Gool, Markus Gross, Alexander Sorkine-Hornung
//   A Benchmark Dataset and Evaluation Methodology for Video Object Segmentation
//   CVPR 2016
// Please consider citing the paper if you use this code.
// ------------------------------------------------------------------------

using namespace boost;
using namespace std;

/* Typedefs from Boost */
typedef adjacency_list < listS, vecS, directedS,
        no_property, property < edge_weight_t, double > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef std::pair<int, int> Edge;

std::vector<vertex_descriptor> run_one_dijkstra(std::list<Edge>& edge_list, std::list<double>& edge_costs, const int num_nodes, int orig)
{
    /* Define the graph and the containers for the distances and parent nodes */
    graph_t g(edge_list.begin(), edge_list.end(), edge_costs.begin(), num_nodes);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
    std::vector<vertex_descriptor> parents(num_vertices(g));
    std::vector<double>              dists(num_vertices(g));

    /* Run Dijkstra to all nodes */
    vertex_descriptor orig_vertex = vertex(orig, g);
    dijkstra_shortest_paths(g, orig_vertex,
            predecessor_map(boost::make_iterator_property_map(parents.begin(), get(vertex_index, g))).
            distance_map(boost::make_iterator_property_map(dists.begin(), get(vertex_index, g))));

    return parents;
}

py::tuple _match_dijkstra(const np::ndarray& prhs) {

		np::ndarray _prhs = np::zeros(py::make_tuple(prhs.shape(1),prhs.shape(0)),
				np::dtype::get_builtin<double>());

		// Transpose
		for(int i=0; i < _prhs.shape(0); ++i) {
			for(int j=0; j < _prhs.shape(1); ++j) {
				_prhs[i][j] = prhs[j][i];
			}
		}
    /* Cost matrix (doubles) */
    Eigen::Map<Eigen::Array<double,Eigen::Dynamic,Eigen::Dynamic> >
            costs_matrix((double *)_prhs.get_data(),_prhs.shape(1),_prhs.shape(0));
		Eigen::Array<double,Eigen::Dynamic,Eigen::Dynamic> costs(costs_matrix);

		//[> Sizes of the graph <]
		const int n1 = costs.rows();
		const int n2 = costs.cols();
		const int num_nodes = n1*n2;

		//[> Create a look-up table for node indices <]
		Eigen::Array<std::size_t,Eigen::Dynamic,Eigen::Dynamic> sub2ind(n1,n2);
		size_t curr_id = 0;
		for (size_t xx=0; xx<n1; ++xx)
				for (size_t yy=0; yy<n2; ++yy)
						sub2ind(xx,yy) = curr_id++;

		//[> Inverted LUT <]
		vector<pair<size_t,size_t>> ind2sub(num_nodes);
		for (size_t xx=0; xx<n1; ++xx)
				for (size_t yy=0; yy<n2; ++yy)
						ind2sub[sub2ind(xx,yy)] = make_pair(xx,yy);

		//[> Define the edges of the graph <]
		std::list<Edge> edge_list;
		std::list<double> edge_costs;
		for (std::size_t xx=0; xx<n1; ++xx)
		{
				for (std::size_t yy=0; yy<n2; ++yy)
				{
						//[> Down <]
						if (yy>0)
						{
								edge_list.emplace_back(sub2ind(xx,yy-1),sub2ind(xx,yy));
								edge_costs.emplace_back(costs(xx,yy));
						}
						//[> Left <]
						if (xx>0)
						{
								edge_list.emplace_back(sub2ind(xx-1,yy),sub2ind(xx,yy));
								edge_costs.emplace_back(costs(xx,yy));
						}

						//[> Down-left <]
						if ((yy>0) && (xx>0))
						{
								edge_list.emplace_back(sub2ind(xx-1,yy-1),sub2ind(xx,yy));
								edge_costs.emplace_back(costs(xx,yy));
						}
				}
		}

		//[> Origin and destination nodes - First we assume the 0-0 matching <]
		int orig = 0;
		int dest = num_nodes-1;
		std::vector<vertex_descriptor> parents = run_one_dijkstra(edge_list,edge_costs,num_nodes,orig);

	 //Get path to destination by scanning predecessors
	 //We also get the minimum-cost node
		std::vector<size_t> opt_path;
		size_t curr_predecessor = dest;
		double min_cost = numeric_limits<double>::max();
		size_t min_id = curr_predecessor;
		while(curr_predecessor!=orig)
		{
				double curr_cost = costs(ind2sub[curr_predecessor].first,ind2sub[curr_predecessor].second);
				if (curr_cost<min_cost)
				{
						min_cost = curr_cost;
						min_id = curr_predecessor;
				}
				opt_path.emplace_back(curr_predecessor);
				curr_predecessor = parents[curr_predecessor];
		}
		opt_path.emplace_back(curr_predecessor);
		int min_xx = ind2sub[min_id].first;
		int min_yy = ind2sub[min_id].second;

		np::ndarray outputMatrix = np::zeros(py::make_tuple(opt_path.size(),2),
				np::dtype::get_builtin<double>());

		//[> Output pairs - +1 for Matlab <]
		for (size_t i=0; i<opt_path.size(); i++) {
				outputMatrix[i][0] = ind2sub[opt_path[i]].first ;
				outputMatrix[i][1] = ind2sub[opt_path[i]].second;
		}

		return py::make_tuple(outputMatrix,min_xx,min_yy);
}

BOOST_PYTHON_MODULE(tstab) {
	// Initialize numpy
	np::initialize();

	py::class_<_ContContainer,std::shared_ptr<_ContContainer>>(
			"ContContainer")
		.def_readwrite("contour_coords",&_ContContainer::contour_coords)
		.def_readwrite("im_sx",&_ContContainer::im_sx)
		.def_readwrite("im_sy",&_ContContainer::im_sy)
		.def_readonly("is_hole",&_ContContainer::is_hole);

	py::def("mask2poly",_mask2poly);
	py::def("get_longest_cont",_get_longest_cont);
	py::def("contour_upsample",_contour_upsample);
	py::def("match_dijkstra",_match_dijkstra);

	boost::python::class_<std::vector<bool>>("PyVecBool")
			.def(boost::python::vector_indexing_suite<std::vector<bool>,true >());
}


#include "truncate.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include "all.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <cmath>
namespace HT
{
    void truncate(PASTNode astnode, ParsersHelper& ph)
    {
        auto & secondCh = *astnode->ch.rbegin();
        validateEach(astnode, ph, "truncate", 1, [](PASTNode an)
                    {
                    return an->token.tokenType == Complex && boost::get<ComplexType>(an->token.info).isReal();
                    });
        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        if (!cast.exact())
        astnode->token.info = ComplexType(
                    std::trunc(
                        cast.toinexact().getRealD()
                        ));
        else
        {
            auto rat = cast.getRealR();
            auto ans = rat.getUp() / rat.getDown();
            ans.setSign(rat.getSign());
            astnode->token.info = ComplexType(ans);
        }

        astnode->remove();
    }

}


// BLAS level 3

//#define F_USE_STD_VECTOR

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/std/vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;

using std::cout;
using std::endl;

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<double, ublas::row_major> m_t;
#else
typedef ublas::matrix<double, ublas::column_major, std::vector<double> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (4, 4);
  init_m (a, kpp (1));
  print_m (a, "a");
  cout << endl;

  m_t b (4, 6);
  init_m (b, cls1());
  print_m (b, "b");
  cout << endl;

  m_t c (4, 6);

  // c = a b
  blas::gemm ( 1.0, a, b, 0.0, c);
  print_m (c, "c = a b");
  cout << endl;
  blas::gemm ( 1.0, a, b, 0.0, c);
  print_m (c, "c = a b");
  cout << endl;

  init_m (c, const_val<double> (1));
  print_m (c, "c");
  cout << endl;
  // c = 2 a b + 0.5 c
  blas::gemm (2.0, a, b, 0.05, c);
  print_m (c, "c = 2 a b + 0.05 c");
  cout << endl;

  m_t d (6, 4);

  // d = b^T a^T
  blas::gemm ( 1.0, bindings::trans(b), bindings::trans(a), 0.0, d);
  print_m (d, "d = b^T a^T");
  cout << endl;

  // c = a^T b
  blas::gemm (1.0, bindings::trans(a), b, 0.0, c);
  print_m (c, "c = a^T b");
  cout << endl;

  // d = b^T a
  blas::gemm ( 1.0, bindings::trans(b), a, 0.0, d);
  print_m (d, "d = b^T a");
  cout << endl;

  init_m (d, const_val<double> (0));
  ublas::matrix_range<m_t> br (b, ublas::range (0, 4), ublas::range (0, 4));
  ublas::matrix_range<m_t> dr (d, ublas::range (1, 5), ublas::range (0, 4));

  // d[1..5][0..4] = a b[0..4][0..4]
  blas::gemm ( 1.0, a, br, 0.0, dr);
  print_m (d, "d[1..5][0..4] = a b[0..4][0..4]");
  cout << endl;

  // d[1..5][0..4] = b[0..4][0..4] a
  blas::gemm ( 1.0, br, a, 0.0, dr);
  print_m (d, "d[1..5][0..4] = b[0..4][0..4] a");
  cout << endl;

  // d[1..5][0..4] = b[0..4][0..4] a^T
  blas::gemm ( 1.0, br, bindings::trans(a), 0.0, dr);
  print_m (d, "d[1..5][0..4] = b[0..4][0..4] a^T");
  cout << endl;

  // d[1..5][0..4] = a b[0..4][0..4]^T
  blas::gemm ( 1.0, a, bindings::trans(br), 0.0, dr);
  print_m (d, "d[1..5][0..4] = a b[0..4][0..4]^T");
  cout << endl;

}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#include <Eigen/Dense>
#include <iostream>
#include <fstream>

#include <boost/program_options.hpp>

#include <dpMM/normalSphere.hpp>

using namespace Eigen;
using std::string;

namespace po = boost::program_options;

int main(int argc, char** argv)
{
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("seed", po::value<int>(), "seed for random number generator")
    ("N,N", po::value<int>(), "number of input datapoints")
    ("D,D", po::value<int>(), "number of dimensions of the data")
    ("K,K", po::value<int>(), "number of initial clusters ")
    ("nu,nu", po::value<double>(), "nu parameter of IW from which "
      "sigmas are sampled")
    ("minAngle,a", po::value<double>(), "min angle between means on sphere")
    ("delta,d", po::value<double>(), "delta of NIW")
    ("output,o", po::value<string>(),
      "path to output labels and data .csv file (rows: time; cols: different "
      "datapoints)")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

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

  uint64_t seed = time(0);
  if(vm.count("seed"))
    seed = static_cast<uint64_t>(vm["seed"].as<int>());
  boost::mt19937 rndGen(seed);
  uint32_t K=5;
  if (vm.count("K")) K = vm["K"].as<int>();
  uint32_t N=100;
  if (vm.count("N")) N = vm["N"].as<int>();
  uint32_t D=3;
  if (vm.count("D")) D = vm["D"].as<int>();
  string pathOut ="./rndSphereData";
  if(vm.count("output"))
    pathOut = vm["output"].as<string>();
  cout<<"output to "<<pathOut<<endl;

  double nu = static_cast<double>(D)+2.; // 100 for spherical
  if (vm.count("nu")) nu = vm["nu"].as<double>();
  if(nu < static_cast<double>(D)+1.+1e-8)
    nu = static_cast<double>(D)+1.+1e-8;
  double minAngle = 6.;
  if (vm.count("minAngle")) minAngle = vm["minAngle"].as<double>();
  double delta = 6.;
  if (vm.count("delta")) delta = vm["delta"].as<double>();

  MatrixXd Delta(D-1,D-1);
  Delta = MatrixXd::Identity(D-1,D-1);
  Delta *= nu*(delta*PI/180.)*(delta*PI/180.);

//  double nu = D+0.1
//  MatrixXd Delta(D,D);
//  Delta.setIdentity();
//  Delta *= nu * (12.*PI)/180.0;
  MatrixXd x(D,N);
  VectorXu z(N);
  sampleClustersOnSphere<double>(Delta,nu,x,z,K,minAngle);
  for(uint32_t i=0; i<N-1; ++i)
    if(fabs(x.col(i).norm()-1.0) > 1e-2)
    {
      cout<<"@"<<i<<":"<<x.col(i).norm()<<endl;
      cout<<" error in generating data"<<endl;
      return 0;
    }

  std::ofstream fout;
  fout.open((pathOut+".csv").data(),std::ofstream::out);
  for(uint32_t d=0; d<D; ++d)
  {
    for(uint32_t i=0; i<N-1; ++i)
      fout<<x(d,i)<<" ";
    fout<<x(d,N-1)<<endl;
  }
  fout.close();
  fout.open((pathOut+"_gt.lbl").data(),std::ofstream::out);
    for(uint32_t i=0; i<N-1; ++i)
      fout<<z(i)<<" ";
    fout<<z(N-1)<<endl;
  fout.close();

}


/*=============================================================================
    Copyright (c) 2001-2007 Hartmut Kaiser
    Copyright (c) 2020 Nikita Kniazev
    http://spirit.sourceforge.net/

    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)
=============================================================================*/

#include <boost/preprocessor/cat.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_ast.hpp>
#include <boost/spirit/include/classic_tree_to_xml.hpp>

#include <boost/core/lightweight_test.hpp>

#include <iostream>
#include <iterator>
#include <fstream>
#include <ostream>
#include <string>

using namespace BOOST_SPIRIT_CLASSIC_NS;

///////////////////////////////////////////////////////////////////////////////
struct calculator : public grammar<calculator>
{
    static const int integerID = 1;
    static const int factorID = 2;
    static const int termID = 3;
    static const int expressionID = 4;

    template <typename ScannerT>
    struct definition
    {
        definition(calculator const& /*self*/)
        {
            //  Start grammar definition
            integer     =   leaf_node_d[ lexeme_d[
                                (!ch_p('-') >> +digit_p)
                            ] ];

            factor      =   integer
                        |   inner_node_d[ch_p('(') >> expression >> ch_p(')')]
                        |   (root_node_d[ch_p('-')] >> factor);

            term        =   factor >>
                            *(  (root_node_d[ch_p('*')] >> factor)
                              | (root_node_d[ch_p('/')] >> factor)
                            );

            expression  =   term >>
                            *(  (root_node_d[ch_p('+')] >> term)
                              | (root_node_d[ch_p('-')] >> term)
                            );
            //  End grammar definition

            // turn on the debugging info.
            BOOST_SPIRIT_DEBUG_RULE(integer);
            BOOST_SPIRIT_DEBUG_RULE(factor);
            BOOST_SPIRIT_DEBUG_RULE(term);
            BOOST_SPIRIT_DEBUG_RULE(expression);
        }

        rule<ScannerT, parser_context<>, parser_tag<expressionID> >   expression;
        rule<ScannerT, parser_context<>, parser_tag<termID> >         term;
        rule<ScannerT, parser_context<>, parser_tag<factorID> >       factor;
        rule<ScannerT, parser_context<>, parser_tag<integerID> >      integer;

        rule<ScannerT, parser_context<>, parser_tag<expressionID> > const&
        start() const { return expression; }
    };
};

///////////////////////////////////////////////////////////////////////////////
/// a streambuf implementation that sinks characters to output iterator
template <typename OutputIterator, typename Char>
struct psbuf : std::basic_streambuf<Char>
{
    template <typename T>
    psbuf(T& sink) : sink_(sink) {}

    // silence MSVC warning C4512: assignment operator could not be generated
    BOOST_DELETED_FUNCTION(psbuf& operator=(psbuf const&))

protected:
    typename psbuf::int_type overflow(typename psbuf::int_type ch) BOOST_OVERRIDE
    {
        if (psbuf::traits_type::eq_int_type(ch, psbuf::traits_type::eof()))
            return psbuf::traits_type::not_eof(ch);

        *sink_ = psbuf::traits_type::to_char_type(ch);
        ++sink_;
        return ch;
    }

private:
    OutputIterator sink_;
};

///////////////////////////////////////////////////////////////////////////////
#define EXPECTED_XML_OUTPUT "<?xml version=\"1.0\" encoding=\"ISO-8859-1\"?>\n\
<!DOCTYPE parsetree SYSTEM \"parsetree.dtd\">\n\
<!-- 1+2 -->\n\
<parsetree version=\"1.0\">\n\
    <parsenode>\n\
        <value>+</value>\n\
        <parsenode>\n\
            <value>1</value>\n\
        </parsenode>\n\
        <parsenode>\n\
            <value>2</value>\n\
        </parsenode>\n\
    </parsenode>\n\
</parsetree>\n"

#define EXPECTED_XML_OUTPUT_WIDE BOOST_PP_CAT(L, EXPECTED_XML_OUTPUT)

bool test(wchar_t const *text)
{
    typedef std::basic_string<wchar_t>::iterator iterator_t;

    std::basic_string<wchar_t> input(text);
    calculator calc;
    tree_parse_info<iterator_t> ast_info =
        ast_parse(iterator_t(input.begin()), iterator_t(input.end()),
            calc >> end_p, space_p);

    std::basic_string<wchar_t> out;
    {
        psbuf<std::back_insert_iterator<std::wstring>, wchar_t> buf(out);
        std::wostream outsink(&buf);
        basic_tree_to_xml<wchar_t>(outsink, ast_info.trees, input);
    }
    return out == EXPECTED_XML_OUTPUT_WIDE;
}

bool test(char const *text)
{
    typedef std::string::iterator iterator_t;

    std::string input(text);
    calculator calc;
    tree_parse_info<iterator_t> ast_info =
        ast_parse(iterator_t(input.begin()), iterator_t(input.end()),
            calc >> end_p, space_p);

    std::string out;
    {
        psbuf<std::back_insert_iterator<std::string>, char> buf(out);
        std::ostream outsink(&buf);
        basic_tree_to_xml<char>(outsink, ast_info.trees, input);
    }
    return out == EXPECTED_XML_OUTPUT;
}

int main()
{
    BOOST_TEST(test("1+2"));
    if (std::has_facet<std::ctype<wchar_t> >(std::locale()))
    {
        BOOST_TEST(test(L"1+2"));
    }
    return boost::report_errors();
}


#ifndef TYPES_HPP_
#define TYPES_HPP_

#include <Eigen/Eigen>

typedef Eigen::ArrayXXf Image;
typedef Eigen::ArrayXXf Kernel;

enum Filter { BLUR, DER_X, DER_Y, DER_MAG };

#endif


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <gtest/gtest.h>
#include <limits>
#include <vector>

class BernoulliLogitTestRig : public VectorIntRNGTestRig {
 public:
  BernoulliLogitTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1},
                            {-5.7, -1.0, 0.0, 0.2, 1.0, 10.0}, {-3, -2, 0, 1},
                            {}, {}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& t, const T2&, const T3&, T_rng& rng) const {
    return stan::math::bernoulli_logit_rng(t, rng);
  }

  template <typename T1>
  double pmf(int y, T1 t, double, double) const {
    return std::exp(stan::math::bernoulli_logit_lpmf(y, t));
  }
};

TEST(ProbDistributionsBernoulliLogit, errorCheck) {
  check_dist_throws_all_types(BernoulliLogitTestRig());
}

TEST(ProbDistributionsBernoulliLogit, distributionCheck) {
  check_counts_real(BernoulliLogitTestRig());
}

TEST(ProbDistributionsBernoulliLogit, error_check) {
  boost::random::mt19937 rng;

  EXPECT_NO_THROW(stan::math::bernoulli_logit_rng(-3.5, rng));
  EXPECT_THROW(
      stan::math::bernoulli_logit_rng(stan::math::positive_infinity(), rng),
      std::domain_error);
}

TEST(ProbDistributionsBernoulliLogit, logitChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  // number of samples
  int N = 10000;

  // logit-transformed probability
  double parameter = -0.5;
  // actual probability
  double prob = stan::math::inv_logit(-0.5);

  std::vector<double> expected;
  expected.push_back(N * (1 - prob));
  expected.push_back(N * prob);

  std::vector<int> counts(2);
  for (int i = 0; i < N; ++i) {
    ++counts[stan::math::bernoulli_logit_rng(parameter, rng)];
  }

  assert_chi_squared(counts, expected, 1e-6);
}


/*(utf8)
Simple moving median filter
*/

#include <cmath>
#include <boost/container/flat_map.hpp>

#include "filter_base.hpp"

namespace value_filters
{

template <typename T = double>
class floating_median : public filter_base<T>
{

public:
  floating_median() {}

  T operator()(T x)
  {
    if (!std::isnan(x) || !std::isinf(x))
    {
      buffer[x] = date;

      if (buffer.size() > capacity)
        for (auto it = buffer.begin(); it < buffer.end(); it++)
          if (it->second <= (date - capacity))
          {
            buffer.erase(it);
            break;
          }

      date++;
    }

    if (buffer.size() % 2 == 0)
    {
      int middle = buffer.size() / 2;
      auto mid1 = buffer.nth(middle--)->first;
      auto mid2 = buffer.nth(middle)->first;
      return (mid1 + mid2) / 2.;
    }
    else
    {
      return buffer.nth(buffer.size() / 2)->first;
    }
  }

  void set_amount(double amt)
  {
    if (amt < 1)
      amt = 1;

    capacity = amt;

    if (buffer.size() > capacity)
      for (auto it = buffer.begin(); it < buffer.end(); it++)
        if (it->second <= (date - capacity))
          buffer.erase(it);
  }

  void update()
  {
  }

private:
  boost::container::flat_map<T, int> buffer{};

  int date{0}, capacity{static_cast<int>(SCALED_AMOUNT)};
};

}


// util.hpp
#ifndef COURSERA_UTIL_HPP
#define COURSERA_UTIL_HPP

#include <cassert>
#include <cmath>
#include <string>
#include <vector>
#include <armadillo>

template <typename T>
bool is_almost_equal(const T a, const T b, const uint8_t decimal_accuracy)
{
    const T ac = std::round(a * (10 ^ decimal_accuracy)) / (10 ^ decimal_accuracy);
    const T bc = std::round(b * (10 ^ decimal_accuracy)) / (10 ^ decimal_accuracy);

    return ac == bc;
}

template <typename T>
bool compare_mat(const T &mat_a, const T &mat_b, const uint8_t decimal_accuracy)
{
    using namespace std;
    using namespace arma;

    static_assert(is_base_of<mat, T>::value || is_base_of<fmat, T>::value, "T must extend Armadillo Mat<double> || Mat<float>");

    if (mat_a.n_cols != mat_b.n_cols || mat_a.n_rows != mat_b.n_rows)
    {
        throw invalid_argument("sizes of Mats differ");
    }

    for (size_t r = 0; r < mat_a.n_rows; r++)
    {
        for (size_t c = 0; c < mat_a.n_cols; c++)
        {
            const double a = round(mat_a(r, c) * (10 ^ decimal_accuracy)) / (10 ^ decimal_accuracy);
            const double b = round(mat_b(r, c) * (10 ^ decimal_accuracy)) / (10 ^ decimal_accuracy);
            if (a != b)
            {
                return false;
            }
        }
    }
    return true;
}

template <typename T>
void parse_mat(T *targetmat, const std::string source)
{
    using namespace std;
    using namespace arma;

    static_assert(is_base_of<mat, T>::value || is_base_of<fmat, T>::value, "T must extend Armadillo Mat<double> || Mat<float>");

    if (!(source.front() == '[' && source.back() == ']'))
    {
        throw invalid_argument("can not parse Mat");
    }

    size_t cols = 0, rows = 0;
    vector<double> v;

    size_t semi_pos = 0, lsemi_pos = 0;
    do
    {
        size_t c = 0;
        size_t col_pos = semi_pos, lcol_pos = lsemi_pos;
        semi_pos = source.find(';', semi_pos + 1);
        do
        {
            const auto srclen = (semi_pos != string::npos ? semi_pos : source.length()) - 1;
            col_pos = source.substr(0, srclen).find(',', col_pos + 1);
            if (col_pos != string::npos)
            {
                v.push_back(stod(source.substr(lcol_pos + 1, col_pos - lcol_pos)));
                c++;
            }
            else if (semi_pos != string::npos)
            {
                // get single/last column
                v.push_back(stod(source.substr(lcol_pos + 1, semi_pos - lcol_pos)));
                c++;
            }
            else
            {
                // get last column in last row
                v.push_back(stod(source.substr(lcol_pos + 1, source.length() - 1)));
                c++;
            }
            lcol_pos = col_pos;
        } while (col_pos != string::npos);
        lsemi_pos = semi_pos;
        if (cols == 0)
        {
            cols = c;
        }
        else if (c != cols)
        {
            string msg = "can not parse Mat at row " + to_string(rows);
            throw invalid_argument(msg.c_str());
        }
        rows++;
    } while (semi_pos != string::npos);

    targetmat->set_size(rows, cols);

    for (size_t r = 0, i = 0; r < rows; r++)
    {
        for (size_t c = 0; c < cols; c++, i++)
        {
            (*targetmat)(r, c) = v[i];
        }
    }
}

#endif // COURSERA_UTIL_HPP


#include <iostream>
#include <armadillo.h>
#include <digitRecognition.h>
//#include <mkl.h>
#include <iomanip>

using namespace arma;

//support functions
//cost = fmincg2(10000, nn_params2, k, n , num_labels, X, y, 0.01)
//real function fmincg2(length, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda)

void fmincg(double& finalcost, const int length,mat& nn_params,const int input_layer_size,const int hidden_layer_size,const int num_labels,mat& inputdata,mat& y,const double lambda){
// Copyright (C) 2001 and 2002 by Carl Edward Rasmussen. Date 2002-02-13
// (C) Copyright 1999, 2000 & 2001, Carl Edward Rasmussen
//
// Permission is granted for anyone to copy, use, or modify these
// programs and accompanying documents for purposes of research or
// education, provided this copyright notice is retained, and note is
// made of any changes that have been made.
//
// These programs and documents are distributed without any warranty,
// express or implied.  As the programs were written for research
// purposes only, they have not been tested to the degree that would be
// advisable in any important application.  All use of these programs is
// entirely at the user's own risk.
//
// [ml-class] Changes Made:
// 1) Function name and argument specifications
// 2) Output display
//
// [John Shahbazian] Changes Made:
// 1) Ported to C++ using the Armadillo (http://arma.sourceforge.net/) library
// 2) Change the cost function call to internal.  Replace the
//    'costfunction' function to whatever you would like.  It returns
//    the cost as the result, and the gradient is returned through the first
//    argument as an intent(inout) (e.g. 'gradient#').
// 3) Changed the variable names to be readable.
//    f1 = cost1
//    df1 = gradient1
//    s = search_direction
//    d1 = slope1
//    z1 = point1
//    X0 = backup_params
//    f0 = cost_backup
//    df0 = gradient_backup

	const double RHO = 0.01;
	const double SIG = 0.5;
	const double INT = 0.1;
	const double EXT = 3.0;
	const int MAXEVALS = 20;
	const double RATIO = 100;

	double mintemp, minstuff, M, A, B;
	double fX = 0.0;
	int success;
	int i=0;
	int ls_failed = 0;

	mat backup_params = nn_params;
	mat gradient2(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat gradient3(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat gradient_backup(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat tmp(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	double limit;
	double point1, point2, point3;
	double cost1, cost2, cost3, cost_backup;
	double slope1, slope2, slope3;
	mat search_direction(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat stemp(nn_params.n_rows,nn_params.n_cols,fill::zeros);

	double sqrtnumber;
	//mat sd_calc_1,sd_calc_2,sd_calc_3;
	//double sd_calc_4;
	//mat sd_calc_5(nn_params.n_rows,nn_params.n_cols,fill::zeros);

	cost1 = 10000.0;  //lower is better, so init with high
	mat gradient1(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	costfunction(cost1, gradient1, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);
	//std::cout << "gradient1: " << gradient1(0,0) << endl;
	//pause();

	//i = i + (length<0);
	search_direction = -gradient1;

	mat slope_vector(1,1);
	slope_vector = -search_direction.t() * search_direction;
	slope1 = slope_vector(0,0);
	point1 = 1.0/(1.0 - slope1);
	//std::cout << "point1: " << point1 << endl;

	while(i < std::abs(length)){
		i = i + 1;
		//std::cout << "loop: " << i << endl;
		backup_params = nn_params;
		cost_backup = cost1;
		gradient_backup = gradient1;
		stemp = point1 * search_direction;
		nn_params = nn_params + stemp;
		//std::cout << "nn_params: " << nn_params.row(0) << endl;

		gradient2 = gradient1;

		cost2 = 10000.0;
		costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);

		//i = i + (length<0);
		//i++;
		//std::cout << "i: " << i << endl;
		slope_vector = gradient2.t() * search_direction;
		slope2 = slope_vector(0,0);
		//std::cout << "slope2: " << slope2 << endl;

		cost3 = cost1;
		slope3 = slope1;
		point3 = -point1;
		if(length>0)M=MAXEVALS;
		else M = std::min(MAXEVALS,-length-i);
		success=0;
		limit=-1;

		while(1){ //3.66252   3.66252  0 -5.40439
			//std::cout << "cost2: " << cost2 << " cost1: " << cost1 << " point1: " << point1 << " slope1: " << slope1 << endl;
			//std::cout << "cost2 vs stuff: " << cost2 << "   " << (cost1 + (point1 * RHO * slope1)) << endl;
			//std::cout << "slope2 vs stuff: " << slope2 << "   " << (-SIG * slope1) << endl;
			//std::cout << "M: " << M << endl;
			//pause();
			while(( (cost2 > (cost1 + (point1 * RHO * slope1))) || (slope2 > (-SIG * slope1)) ) && (M > 0) ){
				//std::cout << "here*******************" << endl;
				limit = point1;
				if(cost2 > cost1)
                    point2 = point3 - (0.5 * slope3 * point3 * point3)/(slope3 * point3 + cost2 - cost3);  //quadratic fit
                else{
                    A = 6*(cost2 - cost3)/point3 + 3*(slope2 + slope3);           //cubic fit
                    B = 3*(cost3 - cost2) - point3 * (slope3 + 2*slope2);
                    point2 = (std::sqrt(B*B - A*slope2*point3*point3) - B)/A;
				}
                if(std::isnan(point2) || (!std::isfinite(point2)))
                    point2 = point3 / 2;                         // if we had a numerical problem then bisect
                point2 = std::max( std::min(point2, (INT * point3)), ((1.0 - INT) * point3));  //don't accept too close to limits
                point1  = point1 + point2;                       // update the step
                stemp = point2 * search_direction;
                nn_params = nn_params + stemp;

                costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);

                M = M - 1.0;
				//std::cout << "i2: " << i << endl;
                //i = i + (length<0);                              // count epochs?!
				//std::cout << "i2: " << i << endl;
                slope_vector = gradient2.t() * search_direction;
                slope2 = slope_vector(0,0);                      //convert to scalar
                point3 = point3 - point2;                        // point3 is now relative to the location of point2

			}

            if ((cost2 > (cost1 + (point1*RHO*slope1)) ) || (slope2 > (-SIG * slope1) )){
				//std::cout << "Break -----------------" << endl;
				break;                                            // this is a failure

            }
            else if (slope2 > (SIG * slope1)){
				//std::cout << "Break -----------------" << endl;
                success = 1;
                break;                                            // success
			}
            else if (M == 0.0){
				//std::cout << "Break -----------------" << endl;
                break;                                            // failure
			}

            A = 6*(cost2 - cost3)/point3 + 3*(slope2 + slope3);  // make cubic extrapolation
            B = 3*(cost3 - cost2) - point3*(slope3 + 2*slope2);
            sqrtnumber = (B*B) - A*slope2*point3*point3;

			if((!std::isnormal(sqrtnumber)) || (sqrtnumber < 0.0) ){
				//std::cout << "sqrt ifs" << endl;
                if (limit < -0.5)                          // if we have no upper limit
                    point2 = point1  * (EXT - 1);                // the extrapolate the maximum amount
                else
                    point2 = (limit - point1) / 2;               // otherwise bisect
				//std::cout << "point2 sqrt ifs: " << point2 << endl;
            }
            else{
                point2 = (-slope2 * point3 * point3)/(B + std::sqrt(sqrtnumber));
                if ((limit > -0.5) && ((point2 + point1) > limit))          // extraplation beyond max?
                    point2 = (limit - point1)/2;                 // bisect
                else if ((limit < -0.5) && ((point2 + point1) > (point1 * EXT)))       // extrapolation beyond limit
                    point2 = point1 * (EXT - 1.0);               // set to extrapolation limit
                else if (point2 < (-point3 * INT))
                    point2 = -point3 * INT;
                else if ((limit > -0.5) && (point2 < (limit - point1)*(1.0 - INT)))   // too close to limit?
                    point2 = (limit - point1 ) * (1.0 - INT);
            }

            cost3 = cost2;
            slope3 = slope2;
            point3 = -point2;
            point1  = point1 + point2;

			//std::cout << "search_direction: " << endl << search_direction.rows(0, 10) << endl;
            stemp = point2 * search_direction;
			//std::cout << "point2: " << point2 << endl;

            nn_params = nn_params + stemp;                       // update current estimates
			//std::cout << "nn_paramsb: " << endl << nn_params.rows(0,10) << endl;

            costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);
			//std::cout << "cost2b: " << cost2 << endl;
            M = M - 1.0;
			//std::cout << "i: " << i << endl;
            //i = i + (length<0);                                  // count epochs?!
			//std::cout << "i: " << i << endl;
			slope_vector = gradient2.t() * search_direction;
            slope2 = slope_vector(0,0);                          //convert to scalar
			//std::cout << "slope2b: " << slope2 << endl;
			//pause();
		}

        if (success == 1){                                   // if line search succeeded
            cost1 = cost2;
            fX = cost1;
            std::cout << "Iteration: " << i << " | Cost: " << cost1 << endl;
            mat sd_calc_1 = gradient2.t() * gradient2;
            mat sd_calc_2 = gradient1.t() * gradient2;
            mat sd_calc_3 = gradient1.t() * gradient1;
            double sd_calc_4 = (sd_calc_1(0,0) - sd_calc_2(0,0)) / sd_calc_3(0,0);
            mat sd_calc_5 = sd_calc_4 * search_direction;

            search_direction = sd_calc_5 - gradient2;
            tmp = gradient1;
            gradient1 = gradient2;
            gradient2 = tmp;                                     // swap derivatives
			slope_vector = gradient1.t() * search_direction;
            slope2 = slope_vector(0,0);                          //convert to scalar
            if(slope2 > 0.0){                                  // new slope must be negative
                search_direction = -gradient1;                   // otherwise use steepest direction
				slope_vector = -search_direction.t() * search_direction;
                slope2 = slope_vector(0,0);                      //convert to scalar
            }
			mintemp = slope1 / (slope2);// - std::numeric_limits<double>::lowest());  //std::numeric_limits<double>::lowest() is min value double precision float //TODO: figure out why the min number is needed
            minstuff = std::min(RATIO, mintemp);
            point1  = point1 * minstuff;                         // slope ratio but max RATIO
            slope1 = slope2;
            ls_failed = 0;                                        // this line search did not fail
        }
        else{
            nn_params = backup_params;
            cost1 = cost_backup;
            gradient1 = gradient_backup;                         // restore point from before failed line search
            if (ls_failed == 1 || (i > std::abs(length)))       // line search failed twice in a row
				//std::cout << "Break -----------------" << endl;
                break;                                            // or we ran out of time, so we give up
            tmp = gradient1;
            gradient1 = gradient2;
            gradient2 = tmp;                                    // swap derivatives
            search_direction = -gradient1;                      // try steepest
            slope_vector = -search_direction.t() * search_direction;
            slope1 = slope_vector(0,0);                         // convert to scalar
            point1  = 1.0 / (1.0 - slope1);
            ls_failed = 1;                                      // this line search failed
        }

	}

	finalcost = fX;		//return finalcost
}

void costfunction(double& cost, mat& gradient, mat& nn_params,const int input_layer_size,const int hidden_layer_size,const int num_labels, const mat& inputdata, const mat& y,const double lambda){
	//std::cout << "in costfunction..." << endl;

	mat Theta1 = nn_params.submat(0, 0, hidden_layer_size*(input_layer_size + 1) - 1, 0);
	Theta1.reshape(hidden_layer_size, input_layer_size + 1);
	//std::cout << "Theta1 rows: " << Theta1.n_rows << ", cols: " << Theta1.n_cols << endl;

	mat Theta2 = nn_params.submat(hidden_layer_size*(input_layer_size + 1), 0, hidden_layer_size*(input_layer_size + 1) - 1 + num_labels*(hidden_layer_size + 1) - 1, 0);
	Theta2.reshape(num_labels, hidden_layer_size + 1);
	//std::cout << "Theta2 rows: " << Theta2.n_rows << ", cols: " << Theta2.n_cols << endl;

	//std::cout << "Theta2: " << endl << Theta2.col(0) << endl;
	//pause();

	//constexpr int training_size = 4000;    //m
	//constexpr int input_layer_size = 784;  //k
	//constexpr int hidden_layer_size = 500;  //n
	// l  y_train size

	int l = y.n_rows;
	int m = inputdata.n_rows;
	//int k = input_layer_size;

	//create the  0 1 0 0 0 0 0 0 0 0 style representative matrix
	mat y_representative(l, num_labels,fill::zeros);
	for (unsigned int i=0;i<=(y.n_rows - 1);i++ ){
		int row_value = y(i,0);
		y_representative(i,row_value) = 1.0;
	}
	//std::cout << "y_rep: " << y_representative.row(0) << endl;

	//setup a_2
	mat a_1(inputdata.n_rows,inputdata.n_cols + 1,fill::ones);
	a_1.submat(0,1,a_1.n_rows-1,a_1.n_cols-1) = inputdata;
	mat z_2(a_1.n_rows,Theta1.n_rows,fill::zeros);

	//std::cout << "a_1: " << a_1(0, 0) << endl;

	//std::cout << "Theta1: " << Theta1(1, 2) << endl;

	//std::cout << "Theta1.t(): " << theta1t.row(0) << endl;

	z_2 = a_1 * Theta1.t();
	//std::cout << "z_2: " << z_2.row(0) << endl;
	//pause();

	mat a_2=z_2;
	mat a_2_ones(z_2.n_rows,z_2.n_cols+1,fill::ones);

	sigmoid(a_2,a_2);
	//std::cout << "a_2: " << a_2.row(0) << endl;
	//pause();

	a_2_ones.submat(0,1,a_2_ones.n_rows-1,a_2_ones.n_cols-1)=a_2;
	//std::cout << "a_2_ones: " << a_2_ones.n_rows << endl;
	//std::cout << "a_2_ones: " << a_2_ones.row(0) << endl;
	//pause();

	mat theta2t = Theta2.t();
	//std::cout << "theta2t: " << theta2t.row(0) << endl;
	//pause();

	//setup a_3
	mat z_3(a_2_ones.n_rows,Theta2.n_rows,fill::zeros);
	z_3 = a_2_ones * Theta2.t();
	//std::cout << "z_3: " << z_3.row(0) << endl;
	//pause();

	mat a_3 = z_3;
	//std::cout << "a_3: " << a_3(0, 0) << endl;
	sigmoid(a_3,a_3);

	//mat test1;

	//test1 << 1.0 << 2.0 << 3.0 << endr
	//	  << 4.0 << 5.0 << 6.0 << endr;

	//mat test2 = test1;
	//mat test3 = test1;
	//mat test2l = test2.transform([](double val){ return (log(val)); });

	//test3 = test1 % test2l;
	//std::cout << test3 << endl;
	//pause();

	//test2l = log(test2);
	//test3 = test1 % test2l;
	//std::cout << test3 << endl;
	//pause();

	//calculate cost
	mat summation(z_3.n_rows,z_3.n_cols,fill::zeros);

//	mat a_3l1 = a_3;
//	a_3l1.transform([](double val){ return (log(val)); });

	//std::cout << "log(a_3): " << log(a_3.row(0)) << endl;
	//std::cout << "transform log(a_3): " << a_3l1.row(0) << endl;
	//pause();

//	mat a_3l2 = a_3;
//	a_3l2.transform([](double val){ return (log(1 - val)); });

	summation = -y_representative % log(a_3) - ((1 - y_representative) % log(1-a_3));
	//std::cout << "summation: " << summation.row(0) << endl;
	//pause();
	cost = accu(summation);
	//std::cout << "Cost: " << cost << " m: " << m << endl;
	cost /= m;
	//std::cout << "Cost: " << cost << endl;
	//pause();

	//setup delta_3
	mat delta_3 = y_representative;
	delta_3 = a_3 - y_representative;
	//std::cout << "delta_3: " << delta_3.row(0) << endl;
	//pause();

	//setup sigmoid of z_2
	mat z_2_ones(z_2.n_rows, z_2.n_cols + 1, fill::ones);
	z_2_ones.submat(0,1,z_2_ones.n_rows-1,z_2_ones.n_cols-1) = z_2;
	//std::cout << "z_2_ones: " << z_2_ones.row(0) << endl;
	//pause();

	sigmoidGradient(z_2_ones,z_2_ones);

	//std::cout << "sigmoid_z_2: " << z_2_ones.row(0) << endl;
	//pause();

	//setup delta_2
	mat delta_2(delta_3.n_rows,Theta2.n_cols);
	delta_2 = (delta_3 * Theta2) % z_2_ones;

	//std::cout << "delta_2: " << delta_2.row(0) << endl;
	//pause();

	//setup and combine gradients
	mat gradient_2(delta_3.n_cols,a_2_ones.n_cols,fill::zeros);
	mat gradient_1(delta_2.n_cols-1,a_1.n_cols,fill::zeros);

	gradient_2 = (delta_3.t() * a_2_ones) / m;
	//std::cout << "gradient_2: " << gradient_2.row(0) << endl;
	//pause();

	gradient_1 = (delta_2.cols(1,delta_2.n_cols-1).t() * a_1) / m;
	//std::cout << "gradient_1: " << gradient_1.row(0) << endl;
	//pause();
	gradient_2.cols(1,gradient_2.n_cols-1) += Theta2.cols(1,Theta2.n_cols-1) * (lambda/m);
	gradient_1.cols(1,gradient_1.n_cols-1) += Theta1.cols(1,Theta1.n_cols-1) * (lambda/m);
	gradient_1.reshape(gradient_1.n_rows*gradient_1.n_cols,1);
	gradient_2.reshape(gradient_2.n_rows*gradient_2.n_cols,1);
	gradient = join_cols(gradient_1,gradient_2);
	//std::cout << "gradient: " << gradient.col(0) << endl;
	//pause();

	//std::cout << "leaving costfunction..." << endl;
}

void predict(const mat& Theta1,const mat& Theta2,const mat& inputdata, mat& predictions){
    mat x_holder(inputdata.n_rows,inputdata.n_cols+1,fill::ones);
    mat pre_h1(x_holder.n_rows,Theta1.n_rows);
    mat h1_ones(x_holder.n_rows,Theta1.n_rows+1,fill::ones);
    mat h2(x_holder.n_rows,Theta2.n_rows,fill::zeros);

	//std::cout << "inputdata: " << inputdata.n_rows << " " << inputdata.n_cols << endl;
	// std::cout << "x_holder: " << x_holder.n_rows << " " << x_holder.n_cols << endl;
    x_holder.submat(0,1,x_holder.n_rows-1,x_holder.n_cols-1) = inputdata;

    // std::cout << "Theta1: " << Theta1.n_rows << " " << Theta1.n_cols << endl;

    pre_h1 = x_holder * Theta1.t();

	// std::cout << "pre_h1: " << pre_h1.n_rows << " " << pre_h1.n_cols << endl;

    sigmoid(pre_h1,pre_h1);
    h1_ones.submat(0,1,h1_ones.n_rows-1,h1_ones.n_cols-1) = pre_h1;

    // std::cout << "h1_ones: " << h1_ones.n_rows << " " << h1_ones.n_cols << endl;
    // std::cout << "Theta2: " << Theta2.n_rows << " " << Theta2.n_cols << endl;

    h2 = h1_ones * Theta2.t();
    sigmoid(h2,h2);

    predictions = arma::max(h2,0);

}

void sigmoid(const mat& input, mat& output){
    output = exp(-input);
    output.transform([](double val){return (1.0/(1.0 + val));});
}

void sigmoidGradient(const mat& input, mat& output){
    output = exp(-input);
    output.transform([](double val){return (1.0/(1.0 + val));});
    output.transform([](double val){return (val * (1.0 - val));});
}

// void pauseJNS(){
// 	std::cout << std::endl << "Press ENTER to continue...";
// 	std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
// }


#include <boost/math/distributions/negative_binomial.hpp>


#include <mesh_array.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <iostream>   // needed for debugging... This unit-test is not yet perfect (2015-12-11 Kenny)
#include <vector>
#include <cmath>

BOOST_AUTO_TEST_SUITE(mesh_array);

BOOST_AUTO_TEST_CASE(mesh_array_compute_surface_map_sphere)
{

  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;
  typedef MT::vector3_type       V;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  size_t const slices   = 12u;
  size_t const segments = 24u;
  T      const radius   = 12.0f;

  mesh_array::make_sphere<MT>(radius, slices, segments, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z;

  mesh_array::tetgen(surf, sX, sY, sZ, mesh, X, Y, Z );

  mesh_array::TetrahedronAttribute< mesh_array::TetrahedronSurfaceInfo ,mesh_array::T4Mesh> surface_map;

  mesh_array::compute_surface_map( mesh, X, Y, Z, surface_map );

  for (size_t idx = 0u; idx < mesh.tetrahedron_size(); ++idx)
  {
    mesh_array::Tetrahedron const tetrahedron = mesh.tetrahedron(idx);

    bool const opposite_i_on_surface = surface_map( tetrahedron ).m_i;
    bool const opposite_j_on_surface = surface_map( tetrahedron ).m_j;
    bool const opposite_k_on_surface = surface_map( tetrahedron ).m_k;
    bool const opposite_m_on_surface = surface_map( tetrahedron ).m_m;

    V const p_i =  V::make( X( tetrahedron.i() ), Y( tetrahedron.i() ), Z( tetrahedron.i() ) );
    V const p_j =  V::make( X( tetrahedron.j() ), Y( tetrahedron.j() ), Z( tetrahedron.j() ) );
    V const p_k =  V::make( X( tetrahedron.k() ), Y( tetrahedron.k() ), Z( tetrahedron.k() ) );
    V const p_m =  V::make( X( tetrahedron.m() ), Y( tetrahedron.m() ), Z( tetrahedron.m() ) );

    T const distance_i = fabs(radius  - tiny::norm(p_i) );
    T const distance_j = fabs(radius  - tiny::norm(p_j) );
    T const distance_k = fabs(radius  - tiny::norm(p_k) );
    T const distance_m = fabs(radius  - tiny::norm(p_m) );

    bool const i_on_surface = distance_i > 10e-5 ? false : true;
    bool const j_on_surface = distance_j > 10e-5 ? false : true;
    bool const k_on_surface = distance_k > 10e-5 ? false : true;
    bool const m_on_surface = distance_m > 10e-5 ? false : true;

    std::cout << "\t" << distance_i
              << " "  << distance_j
              << " "  << distance_k
              << " "  << distance_m
              << " "  << std::endl;

    std::cout << "\t"
              << i_on_surface
              << " "
              << j_on_surface
              << " "
              << k_on_surface
              << " "
              << m_on_surface
              << " "
              << std::endl;

    std::cout << "\t"
              << opposite_i_on_surface
              << " "
              << opposite_j_on_surface
              << " "
              << opposite_k_on_surface
              << " "
              << opposite_m_on_surface
              << " "
              << std::endl;
  }
}

BOOST_AUTO_TEST_SUITE_END();


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:	 NonDDREAMBayesCalibration
//- Description: Derived class for DREAM-based Bayesian inference
//- Owner:       Brian Adams
//- Checked by:
//- Version:

#ifndef NOND_DREAM_BAYES_CALIBRATION_H
#define NOND_DREAM_BAYES_CALIBRATION_H

#include "NonDBayesCalibration.hpp"
// for uniform PDF
#include <boost/math/distributions/uniform.hpp>
// for uniform samples (uniform_real is deprecated)
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>

namespace Dakota {

/// Bayesian inference using the DREAM approach

/** This class performed Bayesian calibration using the DREAM (Markov
    Chain Monte Carlo acceleration by Differential Evolution)
    implementation of John Burkhardt (FSU), adapted from that of
    Guannan Zhang (ORNL) */

class NonDDREAMBayesCalibration: public NonDBayesCalibration
{
public:

  //
  //- Heading: Constructors and destructor
  //

  /// standard constructor
  NonDDREAMBayesCalibration(ProblemDescDB& problem_db, Model& model);
  /// destructor
  ~NonDDREAMBayesCalibration();

  //
  //- Heading: Static callback functions required by DREAM
  //

  /// initializer for problem size characteristics in DREAM
  static void
  problem_size (int &chain_num, int &cr_num, int &gen_num, int &pair_num,
		int &par_num);

  /// Filename and data initializer for DREAM
  static void
  problem_value (std::string *chain_filename, std::string *gr_filename,
		 double &gr_threshold, int &jumpstep, double limits[],
		 int par_num, int &printstep, std::string *restart_read_filename,
		 std::string *restart_write_filename);

  /// Compute the prior density at specified point zp
  static double prior_density (int par_num, double zp[]);

  // NOTE: Memory is freed inside the dream core
  /// Sample the prior and return an array of parameter values
  static double* prior_sample (int par_num);

  // Called by chain_init and dream_algm
  //   par_num: number of parameters
  //   zp:      point at which to sample the likelihood
  //   returns: real valued log-likelihood
  /// Likelihood function for call-back from DREAM to DAKOTA for evaluation
  static double sample_likelihood (int par_num, double zp[]);

protected:

  //
  //- Heading: Virtual function redefinitions
  //

  void calibrate();
  //void print_results(std::ostream& s, short results_state = FINAL_RESULTS);

  // Member functions

  /// Callback to archive the chain from DREAM, potentially leaving it in u-space
  static void cache_chain(const double* const z);
  /// save the final x-space acceptance chain and corresponding function values
  void archive_acceptance_chain();

  //
  //- Heading: Data

  /// lower bounds on calibrated parameters
  RealVector paramMins;
  /// upper bounds on calibrated parameters
  RealVector paramMaxs;

  // DREAM Algorithm controls

  /// number of concurrent chains
  int numChains;
  /// number of generations
  int numGenerations;
  /// number of CR-factors
  int numCR;
  /// number of crossover chain pairs
  int crossoverChainPairs;
  /// threshold for the Gelmin-Rubin statistic
  Real grThreshold;
  /// how often to perform a long jump in generations
  int jumpStep;

  /// random number engine for sampling the prior
  boost::mt19937 rnumGenerator;

  // uniform prior PDFs for each variable
  //std::vector<boost::math::uniform> priorDistributions;
  // samplers for the uniform prior PDFs for each variable
  //std::vector<boost::uniform_real<double> > priorSamplers;

private:

  //
  // - Heading: Data
  //

  /// Pointer to current class instance for use in static callback functions
  static NonDDREAMBayesCalibration* nonDDREAMInstance;

};

} // namespace Dakota

#endif