/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/dimensionality.h>
#include <gauss/internal/scopedHostPtr.h>

#include <algorithm>
#include <boost/math/distributions/normal.hpp>
#include <cmath>
#include <iterator>
#include <limits>
#include <map>
#include <numeric>
#include <set>
#include <stdexcept>
#include <utility>
#include <vector>

using namespace gauss::dimensionality;

namespace {

struct VisvalingamSummaryPoint {
    float x;
    float y;
    int64_t area;
};

std::vector<float> computeBreakpoints(int alphabet_size, float mean_value, float std_value) {
    std::vector<float> res;
    boost::math::normal dist(mean_value, std_value);

    for (int i = 1; i < alphabet_size; i++) {
        float value = static_cast<float>(quantile(dist, (float)i * (1 / (float)alphabet_size)));
        res.push_back(value);
    }
    return res;
}

std::vector<int> generateAlphabet(int alphabet_size) {
    std::vector<int> res;
    for (int i = 0; i < alphabet_size; i++) {
        res.push_back(i);
    }
    return res;
}

double PerpendicularDistance(const Point &pt, const Point &lineStart, const Point &lineEnd) {
    double dx = static_cast<double>(lineEnd.first) - lineStart.first;
    double dy = static_cast<double>(lineEnd.second) - lineStart.second;

    // Normalise
    double mag = pow(pow(dx, 2.0) + pow(dy, 2.0), 0.5);
    if (mag > 0.0) {
        dx /= mag;
        dy /= mag;
    }

    double pvx = static_cast<double>(pt.first) - lineStart.first;
    double pvy = static_cast<double>(pt.second) - lineStart.second;

    // Get dot product (project pv onto normalized direction)
    double pvdot = dx * pvx + dy * pvy;

    // Scale line direction vector
    double dsx = pvdot * dx;
    double dsy = pvdot * dy;

    // Subtract this from pv
    double ax = pvx - dsx;
    double ay = pvy - dsy;

    return pow(pow(ax, 2.0) + pow(ay, 2.0), 0.5);
}

/**
 * @brief We compute the vertical distance of the point p w.r.t. the line that connects start and end points.
 * We use the point-slope equiation to solve it.
 */
float verticalDistance(const Point &p, const Point &start, const Point &end) {
    float dy = end.second - start.second;
    float dx = end.first - start.first;
    float m = dy / dx;
    float line_y = (p.first - start.first) * m + start.second;
    return (p.second - line_y) * (p.second - line_y);
}

/**
 * @brief This function just inserts a point in the given position of a vector.
 */
void insertPointBetweenSelected(const Point &p, int position, std::vector<Point> &selected) {
    auto it = selected.begin();
    selected.insert(it + position, p);
}

/**
 * @brief This function just checks if the given point is already in the vector.
 */
bool isPointInDesiredList(const Point &point, const std::vector<Point> &selected) {
    auto it =
        std::find_if(selected.cbegin(), selected.cend(), [&point](const Point &p) { return p.first == point.first; });
    return it != selected.end();
}

/**
 * @brief This function calculates the segment indices that we need to compare the point.
 */
std::pair<int, int> getSegmentFromSelected(const Point &point, const std::vector<Point> &selected) {
    int lower = 0, upper = 0;
    // We do not check the first element, as it is fixed and set to the first element of the time series
    size_t i = 0;
    bool rebased = false;
    while (!rebased && (i < selected.size() - 1)) {
        // We check points until point.first > selected[i]
        if (point.first > selected[i].first) {
            lower = static_cast<int>(i);
            upper = static_cast<int>(i + 1);
        } else {
            rebased = true;
        }
        i++;
    }
    return std::make_pair(lower, upper);
}

template <typename T>
af::array PAA_CPU(const af::array &a, int bins) {
    af::array result;
    auto n = a.dims(0);

    auto reducedColumn = std::vector<T>(bins);

    // Find out the number of elements per bin
    T elemPerBin = static_cast<T>(n) / static_cast<T>(bins);

    // For each column
    for (int i = 0; i < a.dims(1); i++) {
        auto column = gauss::utils::makeScopedHostPtr(a.col(i).host<T>());
        T start = 0.0;
        T end = elemPerBin - 1;

        // For each column
        for (int j = 0; j < bins; j++) {
            T avg = 0.0;
            int count = 0;

            // Compute avg for this segment
            for (int k = start; k <= end; k++) {
                avg = avg + column[k];
                count++;
            }
            avg = avg / count;
            reducedColumn[j] = avg;

            // Compute next segment
            start = std::ceil(end);
            end = end + elemPerBin;
            end = (end > n) ? n : end;
        }

        // First Column
        af::array col(bins, 1, reducedColumn.data());
        if (i == 0) {
            result = col;
        } else {
            result = af::join(1, result, col);
        }
    }
    return result;
}

float calculateError(const std::vector<Point> &ts, int start, int end) {
    Point p1 = ts[start];
    Point p2 = ts[end];

    // We use the point-slope equation for the middle points between start and end: y = mx - mx_1 + y_1
    // where m = (y_2 - y_1) / (x_2 - x_1)
    float m = (p2.second - p1.second) / (p2.first - p1.first);

    return std::accumulate(ts.cbegin() + start, ts.cbegin() + end + 1, 0.0f, [&p1, m](float acc, Point p) {
        return acc + std::pow(p.second - (m * (p.first - p1.first) + p1.second), 2);
    });
}

Segment merge(Segment s1, Segment s2) { return {s1.first, s2.second}; }

int64_t computeTriangleArea(const VisvalingamSummaryPoint &a, const VisvalingamSummaryPoint &b,
                            const VisvalingamSummaryPoint &c, const long scale = 1e9) {
    float f1 = a.x * (b.y - c.y);
    float f2 = b.x * (c.y - a.y);
    float f3 = c.x * (a.y - b.y);
    return static_cast<int64_t>(std::abs((static_cast<double>(f1) + f2 + f3) / 2.0f) * scale);
}

template <typename Iter, typename Distance>
Iter shiftIterator(Iter iter, Distance positions) {
    std::advance(iter, positions);
    return iter;
}

class mapComparator {
   public:
    bool operator()(const std::pair<int64_t, int64_t> &p1, const std::pair<int64_t, int64_t> &p2) const {
        return ((p1.first < p2.first) || ((p1.first == p2.first) && (p1.second < p2.second)));
    }
};

void recomputeAreaNeighbor(std::map<int64_t, VisvalingamSummaryPoint>::iterator &iterator_point,
                           std::set<std::pair<int64_t, int64_t>, mapComparator> &point_indexer,
                           std::map<int64_t, VisvalingamSummaryPoint> &points, const int64_t scale) {
    auto im1m1 = shiftIterator(iterator_point, -1);
    auto im1p1 = shiftIterator(iterator_point, 1);
    auto original_position_minus1 = iterator_point->first;

    auto old_area_minus1 = iterator_point->second.area;
    auto new_area_minus1 = computeTriangleArea(im1m1->second, iterator_point->second, im1p1->second, scale);
    points[iterator_point->first] =
        VisvalingamSummaryPoint{iterator_point->second.x, iterator_point->second.y, new_area_minus1};

    auto it = point_indexer.find(std::make_pair(old_area_minus1, original_position_minus1));
    point_indexer.erase(it);

    point_indexer.insert(std::pair<int64_t, int64_t>(std::make_pair(new_area_minus1, original_position_minus1)));
}

}  // namespace

std::vector<Point> gauss::dimensionality::PAA(const std::vector<Point> &points, int bins) {
    float xrange = points.back().first - points.front().first;
    float width_bin = xrange / bins;
    float reduction = bins / xrange;

    std::vector<float> sum(bins, 0.0);
    std::vector<int> counter(bins, 0);

    // Iterating over the time series
    for (const auto &p : points) {
        auto pos = static_cast<size_t>(std::min(p.first * reduction, (float)(bins - 1)));
        sum[pos] += p.second;
        counter[pos] += 1;
    }

    std::vector<Point> result;
    result.reserve(bins);
    // Compute the average per bin
    for (int i = 0; i < bins; ++i) {
        result.emplace_back((width_bin * i) + (width_bin / 2.0f), sum[i] / counter[i]);
    }
    return result;
}

af::array gauss::dimensionality::PAA(const af::array &a, int bins) {
    // Resulting array
    af::array result;

    // Check dimensions are divisible, if not, call CPU version
    if (a.dims(0) % bins == 0) {
        auto n = a.dims(0);
        auto elem_row = n / bins;

        af::array b = af::moddims(a, elem_row, bins, a.dims(1));
        af::array addition = af::sum(b, 0);
        result = af::reorder(addition / elem_row, 1, 2, 0, 3);
    } else {
        // Call the CPU version
        if (a.type() == af::dtype::f64) {
            result = PAA_CPU<double>(a, bins);
        } else if (a.type() == af::dtype::f32) {
            result = PAA_CPU<float>(a, bins);
        }
    }

    return result;
}

af::array gauss::dimensionality::PIP(const af::array &ts, int numberIPs) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    auto n = ts.dims(0);
    auto end = n - 1;

    if (n < 2) {
        throw std::invalid_argument("We can't delete all those important points");
    } else if (n <= numberIPs) {
        return ts;
    }

    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    // Converting c-arrays to vector of points
    std::vector<Point> points;
    for (int i = 0; i < n; i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    // Allocating vectors for selected points
    std::vector<Point> selected;
    selected.emplace_back(points[0]);
    selected.emplace_back(points[end]);

    // we have to find (numberIPs - 2) points, as we have already included P[0] and P[end].
    // Number of passes over the collection
    for (int p = 0; p < (numberIPs - 2); p++) {
        float dmax = -1.0;
        int index = -1;
        int position = -1;

        // Find the next PIP
        for (int i = 1; i < end; i++) {
            // We first check if the point is already in the list.
            if (!isPointInDesiredList(points[i], selected)) {
                // segment contains the indices of the selected points which are the boundaries for the point i.
                std::pair<int, int> segment = getSegmentFromSelected(points[i], selected);
                float d = verticalDistance(points[i], selected[segment.first], selected[segment.second]);
                // We store the point with the maximum distance to the line that connects the segment.
                if (d > dmax) {
                    index = i;
                    dmax = d;
                    position = segment.second;
                }
            }
        }
        insertPointBetweenSelected(points[index], position, selected);
    }

    // Converting from vector to array
    std::vector<float> x;
    x.reserve(selected.size());
    std::vector<float> y;
    y.reserve(selected.size());
    for (auto &i : selected) {
        x.emplace_back(i.first);
        y.emplace_back(i.second);
    }

    // from c-array to af::array
    af::array tsx(selected.size(), 1, x.data());
    af::array tsy(selected.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::PLABottomUp(const std::vector<Point> &ts, float maxError) {
    std::vector<Segment> segments;
    segments.reserve(ts.size());

    // Allocating vector of segments
    for (size_t i = 0; i < ts.size() - 1; i = i + 2) {
        segments.emplace_back(i, i + 1);
    }

    std::vector<float> mergeCost;
    mergeCost.reserve(segments.size());
    for (size_t i = 0; i < segments.size() - 1; i++) {
        mergeCost.emplace_back(calculateError(ts, segments[i].first, segments[i + 1].second));
    }

    // Calculate minimum, calculating in advance
    auto minCost = std::min_element(std::begin(mergeCost), std::end(mergeCost));
    while ((segments.size() > 2) && (*minCost < maxError)) {
        // We have to merge
        auto index = std::distance(std::begin(mergeCost), minCost);

        // Merge candidate segments
        segments[index] = merge(segments[index], segments[index + 1]);

        // Delete fused segment
        segments.erase(segments.begin() + index + 1);
        mergeCost.erase(mergeCost.begin() + index + 1);

        // Calculate new cost
        mergeCost[index] = calculateError(ts, segments[index].first, segments[index + 1].second);
        mergeCost[index - 1] = calculateError(ts, segments[index - 1].first, segments[index].second);

        // Calculate new minimum
        minCost = std::min_element(std::begin(mergeCost), std::end(mergeCost));
    }

    // Build a polyline from a set of segments
    std::vector<Point> result;
    result.reserve(segments.size());
    for (auto &segment : segments) {
        result.emplace_back(ts[segment.first]);
        result.emplace_back(ts[segment.second]);
    }

    return result;
}

af::array gauss::dimensionality::PLABottomUp(const af::array &ts, float maxError) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(ts.dims(0));

    // Creating a vector of Points
    for (int i = 0; i < ts.dims(0); i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    std::vector<Point> reducedPoints = PLABottomUp(points, maxError);
    std::vector<float> x;
    x.reserve(reducedPoints.size());
    std::vector<float> y;
    y.reserve(reducedPoints.size());

    // Converting from vector to array
    for (const auto &point : reducedPoints) {
        x.emplace_back(point.first);
        y.emplace_back(point.second);
    }

    // from c-array to af::array
    af::array tsx(reducedPoints.size(), 1, x.data());
    af::array tsy(reducedPoints.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::PLASlidingWindow(const std::vector<Point> &ts, float maxError) {
    std::vector<Segment> segments;

    size_t anchor = 0;
    size_t i;

    // We haven´t explored the whole time series
    while (anchor < (ts.size() - 1)) {
        i = 1;
        while (((anchor + i) < ts.size()) &&
               (calculateError(ts, static_cast<int>(anchor), static_cast<int>(anchor + i)) < maxError)) {
            i = i + 1;
        }

        if ((anchor + i) == (ts.size() - 1)) {
            segments.emplace_back(anchor, anchor + i);
        } else {
            segments.emplace_back(anchor, anchor + i - 1);
        }
        anchor += i;
    }

    // Build a polyline from a set of segments
    std::vector<Point> result;
    result.reserve(segments.size());
    for (auto &segment : segments) {
        result.emplace_back(ts[segment.first]);
        result.emplace_back(ts[segment.second]);
    }

    return result;
}

af::array gauss::dimensionality::PLASlidingWindow(const af::array &ts, float maxError) {
    if (ts.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }
    // Extracting info from af::array
    auto h_x = gauss::utils::makeScopedHostPtr(ts.col(0).host<float>());
    auto h_y = gauss::utils::makeScopedHostPtr(ts.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(ts.dims(0));

    // Creating a vector of Points
    for (int i = 0; i < ts.dims(0); i++) {
        points.emplace_back(h_x[i], h_y[i]);
    }

    std::vector<Point> reducedPoints = PLASlidingWindow(points, maxError);
    std::vector<float> x;
    x.reserve(reducedPoints.size());
    std::vector<float> y;
    y.reserve(reducedPoints.size());
    // Converting from vector to array
    for (const auto &point : reducedPoints) {
        x.emplace_back(point.first);
        y.emplace_back(point.second);
    }

    // from c-array to af::array
    af::array tsx(reducedPoints.size(), 1, x.data());
    af::array tsy(reducedPoints.size(), 1, y.data());
    af::array res = af::join(1, tsx, tsy);

    return res;
}

std::vector<Point> gauss::dimensionality::ramerDouglasPeucker(const std::vector<Point> &pointList, double epsilon) {
    std::vector<Point> out;

    if (pointList.size() < 2) throw std::invalid_argument("Not enough points to simplify ...");

    // Find the point with the maximum distance from line between start and end
    double dmax = 0.0;
    size_t index = 0;
    size_t end = pointList.size() - 1;

    for (size_t i = 1; i < end; i++) {
        double d = PerpendicularDistance(pointList[i], pointList[0], pointList[end]);
        if (d > dmax) {
            index = i;
            dmax = d;
        }
    }

    // If max distance is greater than epsilon, recursively simplify
    if (dmax > epsilon) {
        std::vector<Point> firstLine(pointList.begin(), pointList.begin() + index + 1);
        std::vector<Point> lastLine(pointList.begin() + index, pointList.end());
        auto recResults1 = ramerDouglasPeucker(firstLine, epsilon);
        auto recResults2 = ramerDouglasPeucker(lastLine, epsilon);

        // Build the result list
        out.assign(recResults1.begin(), recResults1.end() - 1);
        out.insert(out.end(), recResults2.begin(), recResults2.end());
        if (out.size() < 2) throw std::runtime_error("Problem assembling output");
    } else {
        // Just return start and end points
        out.clear();
        out.emplace_back(pointList[0]);
        out.emplace_back(pointList[end]);
    }
    return out;
}

af::array gauss::dimensionality::ramerDouglasPeucker(const af::array &pointList, double epsilon) {
    if (pointList.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }

    auto x = gauss::utils::makeScopedHostPtr(pointList.col(0).host<float>());
    auto y = gauss::utils::makeScopedHostPtr(pointList.col(1).host<float>());

    std::vector<Point> points;
    points.reserve(pointList.dims(0));
    for (int i = 0; i < pointList.dims(0); i++) {
        points.emplace_back(x[i], y[i]);
    }

    std::vector<Point> rPoints = ramerDouglasPeucker(points, epsilon);
    af::array out = af::constant(0, rPoints.size(), 2);

    std::vector<float> vx;
    vx.reserve(rPoints.size());
    std::vector<float> vy;
    vy.reserve(rPoints.size());

    for (const auto &rPoint : rPoints) {
        vx.emplace_back(rPoint.first);
        vy.emplace_back(rPoint.second);
    }

    af::array ox(rPoints.size(), vx.data());
    af::array oy(rPoints.size(), vy.data());

    return af::join(1, ox, oy);
}

af::array gauss::dimensionality::SAX(const af::array &a, int alphabet_size) {
    if (a.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Khiva array with two columns expected (x axis and y axis).");
    }

    af::array result = af::constant(0.0, a.dims());
    // Let's store the x-axis.
    result(af::span, 0) += a.col(0);

    // Let's compute the y-axis.
    for (int k = 1; k < a.dims(1); k++) {
        af::array ts = a.col(k);
        auto mean_value = af::mean<float>(ts);
        auto std_value = af::stdev<float>(ts);
        dim_t n = ts.dims(0);
        std::vector<int> aux(n, 0);

        if (std_value > 0) {
            std::vector<float> breakingPoints = computeBreakpoints(alphabet_size, mean_value, std_value);
            std::vector<int> alphabet = generateAlphabet(alphabet_size);
            auto a_h = gauss::utils::makeScopedHostPtr(ts.host<float>());

            // Iterate across elements of ts
            for (int i = 0; i < n; i++) {
                size_t j = 0;
                while ((j < breakingPoints.size()) && (a_h[i] > breakingPoints[j])) {
                    j++;
                }
                aux[i] = alphabet[j];
            }
        }

        // from c-array to af::array
        af::array res(aux.size(), 1, aux.data());
        result(af::span, k) += res;
    }

    return result;
}

std::vector<Point> gauss::dimensionality::visvalingam(const std::vector<Point> &pointList, int64_t numPoints, int64_t scale) {
    std::map<int64_t, VisvalingamSummaryPoint> points;
    std::set<std::pair<int64_t, int64_t>, mapComparator> point_indexer;
    int64_t counter = 0;

    std::transform(
        pointList.cbegin(), pointList.cend(), std::inserter(points, points.end()), [&counter](const Point &point) {
            return std::make_pair(
                counter++, VisvalingamSummaryPoint{point.first, point.second, std::numeric_limits<int64_t>::max()});
        });

    auto points_to_be_deleted = pointList.size() - numPoints;
    auto point_iterator = point_indexer.begin();

    // Precompute areas
    for (auto it = shiftIterator(points.begin(), 1); it != shiftIterator(points.end(), -1); ++it) {
        auto area = computeTriangleArea(shiftIterator(it, -1)->second, it->second, shiftIterator(it, 1)->second, scale);
        it->second.area = area;
        point_iterator = point_indexer.insert(point_iterator, std::make_pair(area, it->first));
    }

    // One point to be deleted on each iteration
    for (size_t iter = 0; iter < points_to_be_deleted; iter++) {
        auto min_index_iterator = point_indexer.begin();
        int64_t min_element = min_index_iterator->second;
        point_indexer.erase(min_index_iterator);

        auto iterator_point = points.find(min_element);
        auto iterator_point_minus1 = shiftIterator(iterator_point, -1);
        auto iterator_point_plus1 = shiftIterator(iterator_point, 1);

        points.erase(iterator_point);

        if (iterator_point_minus1->first > 0) {
            recomputeAreaNeighbor(iterator_point_minus1, point_indexer, points, scale);
        }

        if (iterator_point_plus1->first < counter - 1) {
            recomputeAreaNeighbor(iterator_point_plus1, point_indexer, points, scale);
        }
    }

    std::vector<Point> out_vector;
    out_vector.reserve(numPoints);
    std::transform(points.cbegin(), points.cend(), std::back_inserter(out_vector),
                   [](const std::pair<int64_t, VisvalingamSummaryPoint> &p) {
                       return Point{p.second.x, p.second.y};
                   });

    return out_vector;
}

af::array gauss::dimensionality::visvalingam(const af::array &pointList, int numPoints) {
    if (pointList.dims(1) != 2) {
        throw std::invalid_argument("Invalid dims. Array with two columns expected (x axis and y axis).");
    }

    std::vector<Point> points;
    points.reserve(pointList.dims(0));
    auto x = gauss::utils::makeScopedHostPtr(pointList.col(0).host<float>());
    auto y = gauss::utils::makeScopedHostPtr(pointList.col(1).host<float>());

    for (int i = 0; i < pointList.dims(0); i++) {
        points.emplace_back(x[i], y[i]);
    }

    std::vector<Point> rPoints = visvalingam(points, numPoints);
    af::array out = af::constant(0, rPoints.size(), 2);

    std::vector<float> vx;
    vx.reserve(rPoints.size());
    std::vector<float> vy;
    vy.reserve(rPoints.size());

    for (const auto &rPoint : rPoints) {
        vx.emplace_back(rPoint.first);
        vy.emplace_back(rPoint.second);
    }

    af::array ox(rPoints.size(), vx.data());
    af::array oy(rPoints.size(), vy.data());

    return af::join(1, ox, oy);
}


/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2018, Rawashdeh Research Group
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **********************************************************************/
// Author: Mohamed Aladem

#include "lvt_motion_model.h"
#include <Eigen/Geometry>

lvt_motion_model::lvt_motion_model()
{
    reset();
}

void lvt_motion_model::reset()
{
    m_last_q.setIdentity();
    m_angular_velocity.setIdentity();
    m_last_position.setZero();
    m_linear_velocity.setZero();
}

lvt_pose lvt_motion_model::predict_next_pose(const lvt_pose &current_pose)
{
    // compute new linear velocity
    lvt_vector3 new_lin_velocity = current_pose.get_position() - m_last_position;
    new_lin_velocity = (new_lin_velocity + m_linear_velocity) * 0.5; // smooth velocity over time

    // compute new angular velocity
    lvt_quaternion current_q = current_pose.get_orientation_quaternion();
    lvt_quaternion ang_vel_diff = current_q * m_last_q.inverse();
    lvt_quaternion new_ang_vel = ang_vel_diff.slerp(0.5, m_angular_velocity);
    new_ang_vel.normalize();

    // Update state
    m_last_q = current_q;
    m_angular_velocity = new_ang_vel;
    m_last_position = current_pose.get_position();
    m_linear_velocity = new_lin_velocity;

    // Integrate to compute predictions
    lvt_vector3 int_pos = m_last_position + m_linear_velocity;
    lvt_quaternion int_q = current_q * new_ang_vel;
    int_q.normalize();
    return lvt_pose(int_pos, int_q);
}


// Compile command: g++ -Wall -Wextra -std=c++17 -O2 -pthread -I/usr/include/python3.8 -I/usr/include/eigen3 -o triangular_cone_newdisloc triangular_cone_newdisloc.cpp -lpython3.8
#include <iostream>
#include <fstream>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
#include <utility>
#include <functional>
#include <thread>
#include <map>
#include <string>
#include <algorithm>
#include <ctime>
#include <iomanip>
#include <filesystem>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::Matrix2cd;
using Eigen::Vector2cd;
using std::vector;
using std::sqrt;
using std::cos;
using std::acos;
using std::sin;
using std::atan2;
using namespace std::complex_literals;

typedef std::complex<double> cd;
typedef vector<vector<vector<cd>>> grid_t;
typedef std::function<Matrix2cd(int, double, double)> gen_coin_t;

// Settings here
int num_steps = 1000;
double dy = sqrt(3);
bool plotCone = true; // Set to true to plot the results in the cone coordinates
int ymin = -200, ymax = 200;
int xmin = 3*ymin/2-ymin%2-1, xmax = 3*ymax/2+ymax%2+1;
int ntriangles_x = xmax-xmin+1;
int ntriangles_y = ymax-ymin+1;
int center[2] = {-xmin, -ymin};
std::string prefix;
int initialState = 0;
vector<std::string> initialStateName = {"square", "shifttop", "shiftdl", "center", "bothsides", "everywhere"};

std::time_t now = time(0);
std::tm *ltm = localtime(&now);

grid_t zerogrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, 0. + 0i)));
}
grid_t nangrid() {
    return vector<vector<vector<cd>>>(ntriangles_x, vector<vector<cd>> (ntriangles_y, vector<cd> (3, std::nan("") + 0i)));
}

grid_t grid = zerogrid();

Matrix2cd U, C0;
Matrix2cd sigmay, sigmaz;

double sumAmplitudes() {
    double ret = 0.0;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd val = grid[x+center[0]][y+center[1]][side];
                ret += std::real(val*std::conj(val));
            }
    return ret;
}

// Simplifies initialization for some initial states
void normalizeGrid() {
    double target = sumAmplitudes();
    double mul = 1/sqrt(target);
    if(target == 0)
        return;
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++)
                grid[x+center[0]][y+center[1]][side] *= mul;
}

inline int modulo(int a, int b) {
    return (a%b+b)%b;
}

void init_HQ() {
    sigmaz <<
        1,0,
        0,-1;
    sigmay <<
        0, -1.0i,
        1.0i, 0;
    C0 = std::exp(1.0i*M_PI/3.0) * ((1.0i*M_PI/3.0)*sigmaz).exp();
    double alpha = -std::acos(sqrt(5)/3);
    Matrix2cd U0 = (-1.0i*alpha*sigmay/2.0).exp();
    U = U0*C0*C0;
}

inline int sign(double d) {
    if(d>=0)
        return 1;
    else
        return -1;
}

inline double sq(double d) {
    return d*d;
}

inline double correct_fmod(const double a, const double b) {
    return std::fmod(std::fmod(a,b)+b, b);
}

inline double principal_measure(const double theta) {
    double ret = correct_fmod(theta, 2*M_PI);
    if(ret > M_PI)
        ret -= 2*M_PI;
    return ret;
}

inline double sqmodulus(const std::complex<double> comp) {
    return std::real(comp*std::conj(comp));
}

grid_t shift(grid_t grid) {
    grid_t ngrid = zerogrid();
    for(int x = xmin; x <= xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            for(int i = 0; i < 3; i++) {
                int iprec = ((i-1)%3+3)%3;
                ngrid[x+center[0]][y+center[1]][i] = grid[x+center[0]][y+center[1]][iprec];
            }
        }
    }
    return ngrid;
}

std::pair<double, double> real_coords(int iside, int x, int y, bool show = false) {
    double dec;
    if(show)
        dec = .4;
    else
        dec = .5;
    double xcoord, ycoord;
    // Dislocation: particular case
    if(x == 0 && y >= 0) { // Vertical line
        if((x+y)%2 == 0) { // Tip down
            if(iside == 0) {
                xcoord = x-dec;
                ycoord = (y+.5)*dy;
            }
            else if(iside == 1) {
                xcoord = x;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x-dec;
                ycoord = (y+.5+dec)*dy;
            }
        }
        else { // Tip up
            if(iside == 0) {
                xcoord = x;
                ycoord = (y+.5)*dy;
            }
            else if(iside == 1) {
                xcoord = x-dec;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x-dec;
                ycoord = (y+.5-dec)*dy;
            }
        }
    }
    else if(y >= 0 && (x == 3*y+1 || x == 3*y+2)) { // Diagonal line
        if((x+y)%2 == 0) { // Tip down
            if(iside == 0) {
                xcoord = x-dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else if(iside == 1) {
                xcoord = x+dec;
                ycoord = (y+.5)*dy;
            }
            else {
                xcoord = x+dec/2.;
                ycoord = (y+.5+dec/2.)*dy;
            }
        }
        else { // Tip up
            if(iside == 0) {
                xcoord = x+dec+dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else if(iside == 1) {
                xcoord = x-dec/2.;
                ycoord = (y+.5-dec/2.)*dy;
            }
            else {
                xcoord = x;
                ycoord = (y+.5-dec)*dy;
            }
        }
    }
    else if((x+y)%2==0) {
        if(iside == 0) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5+dec)*dy;
        }
    }
    else {
        if(iside == 0) {
            xcoord = x+dec;
            ycoord = (y+.5)*dy;
        }
        else if(iside == 1) {
            xcoord = x-dec;
            ycoord = (y+.5)*dy;
        }
        else {
            xcoord = x;
            ycoord = (y+.5-dec)*dy;
        }
    }
    return std::make_pair(xcoord, ycoord);
}

std::pair<double, double> cone_coords(int iside, int x, int y, bool show = false) {
    double rx, ry;
    std::tie(rx, ry) = real_coords(iside, x, y, show);
    double r = 5./6.*sqrt(rx*rx+ry*ry);
    double theta_before = atan2(ry, rx);
    double theta_rotate = principal_measure(theta_before+4*M_PI/6.);
    double theta = 6./5.*theta_rotate;
    return std::make_pair(r*cos(theta), r*sin(theta));
}

const int DELTAS[][2] = {{1,0}, {-1,0}, {0,-1}};
const int NUM_THREADS = 8;

/**
 * For the new dislocation:
 * - triangles (0,y), y >= 0, have triangle (3*y//2 + y%2 + 1, y//2) as a neighbor.
 *   Since we only consider tip up triangles, with (x+y)%2 == 1, we simply need to know that
 *   triangles (0, y), y >= 0 and y odd, have side 2 of triangle (3*(y-1)/2+2, (y-1)/2) as a neighbor at side 0.
 * - triangles (3y+1, y), y >= 0 have side 1 of triangle (0, 2y) as a neighbor at side 1.
 */
void applyCoinsPartial(grid_t &ngrid, grid_t &grid, const Matrix2cd &coin, int loc_xmin, int loc_xmax) {
    for(int x = loc_xmin; x < loc_xmax; x++) {
        for(int y = ymin; y <= ymax; y++) {
            if((x+y)%2==0 || (x>0 && y>(x-1)/3))
                continue;
            for(int iside = 0; iside < 3; iside++) {
                int otherside = iside;
                cd thisval = grid[x+center[0]][y+center[1]][iside];
                int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                if(xo > xmax || xo < xmin || yo > ymax || yo < ymin) { // No propagation at borders
                    ngrid[x+center[0]][y+center[1]][iside] = thisval;
                    continue;
                }
                if(x == 0 && y >= 0 && iside == 0) {
                    xo = 3*(y-1)/2+2;
                    yo = (y-1)/2;
                    otherside = 2;
                }
                else if(y >= 0 && x == 3*y+1 && iside == 1) {
                    xo = 0;
                    yo = 2*y;
                    otherside = 1;
                }
                cd otherval = grid[xo+center[0]][yo+center[1]][otherside];
                Vector2cd vect;
                vect << thisval, otherval;
                Vector2cd newvect = coin*vect;
                ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                ngrid[xo+center[0]][yo+center[1]][otherside] = newvect(1);
            }
        }
    }
}

grid_t applyCoins(grid_t grid, const Matrix2cd &coin, bool multithread = true) {
    grid_t ngrid = nangrid();
    if(!multithread) {
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                if((x+y)%2==0 || (x>0 && y>(x-1)/3)) // dislocation : remove a 60-degree part of the lattie
                    continue;
                for(int iside = 0; iside < 3; iside++) {
                    int otherside = iside;
                    cd thisval = grid[x+center[0]][y+center[1]][iside];
                    int xo = x + DELTAS[iside][0], yo = y + DELTAS[iside][1];
                    if(xo > xmax || xo < xmin || yo > ymax || yo < ymin) { // No propagation at borders
                        ngrid[x+center[0]][y+center[1]][iside] = thisval;
                        continue;
                    }
                    if(x == 0 && y >= 0 && iside == 0) {
                        xo = 3*(y-1)/2+2;
                        yo = (y-1)/2;
                        otherside = 2;
                    }
                    else if(y >= 0 && x == 3*y+1 && iside == 1) {
                        xo = 0;
                        yo = 2*y;
                        otherside = 1;
                    }
                    cd otherval = grid[xo+center[0]][yo+center[1]][otherside];
                    Vector2cd vect;
                    vect << thisval, otherval;
                    Vector2cd newvect = coin*vect;
                    ngrid[x+center[0]][y+center[1]][iside] = newvect(0);
                    ngrid[xo+center[0]][yo+center[1]][otherside] = newvect(1);
                }
            }
        }
    }
    else {
        std::thread threads[NUM_THREADS];
        int delta_x = ntriangles_x/NUM_THREADS;
        for(int iThread = 0; iThread < NUM_THREADS-1; iThread++) {
            threads[iThread] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(coin), xmin+iThread*delta_x, xmin+(iThread+1)*delta_x);
        }
        threads[NUM_THREADS-1] = std::thread(applyCoinsPartial, std::ref(ngrid), std::ref(grid), std::ref(coin), xmin+(NUM_THREADS-1)*delta_x, xmax+1);
        for(int iThread = 0; iThread < NUM_THREADS; iThread++)
            threads[iThread].join();
    }
    // We may miss some sides of 1 (tip up) triangles which are not adjacent to a valid 0 triangle
    for(int x = xmin; x <= xmax; x++)
        for(int y = ymin; y <= ymax; y++)
            for(int side = 0; side < 3; side++) {
                cd &newval = ngrid[x+center[0]][y+center[1]][side];
                if(std::isnan(std::real(newval)))
                    newval = grid[x+center[0]][y+center[1]][side];
            }
    return ngrid;
}

void plot(int iGrid = -1) {
    PyObject *fig;
    // Code to write the plot as Python matplotlib instructions. Use this if there are too many visible points to plot, since matplotlib-cpp segfaults in that case
    /*std::ostringstream scriptpath;
    scriptpath << prefix << "/script_" << iGrid << ".py";
    std::ofstream pyscript(scriptpath.str());
    pyscript << "from matplotlib import pyplot as plt\n";
    pyscript << "plt.figure(figsize=(10,10))\n";
    pyscript << "plt.xlim(" << xmin << "," << xmax << ")\n";
    pyscript << "plt.ylim(" << ymin*dy << "," << ymax*dy << ")\n";*/
    if(plotCone) {
        fig = plt::figure_size(1000,1000);
        plt::xlim(xmin, xmax);
        plt::ylim(ymin*dy, ymax*dy);
        plt::set_aspect_equal();
        vector<double> xlist, ylist, colorlist;
        vector<double> listvals;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    listvals.push_back(col);
                }
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        double threshold = maxi/10;
        for(int x = xmin; x <= xmax; x++) {
            for(int y = ymin; y <= ymax; y++) {
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    if(col > threshold) {
                        double rx, ry;
                        std::tie(rx, ry) = cone_coords(iside, x, y, true);
                        xlist.push_back(rx);
                        ylist.push_back(ry);
                        colorlist.push_back(col);
                    }
                }
            }
        }
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= 0.6;
        // Continued
        /*
        pyscript << "plt.scatter([";
        for(double d : xlist)
            pyscript << d << ",";
        pyscript << "],[";
        for(double d : ylist)
            pyscript << d << ",";
        pyscript << "], c=[";
        for(double c : colorlist)
            pyscript << c << ",";
        pyscript << "], cmap=\"gist_heat_r\", vmin=0, vmax=" << maxi << ")\n";*/
        plt::scatter_colored(xlist, ylist, colorlist, 1, {{"cmap","gist_heat_r"}, {"vmin", "0"}, {"vmax", std::to_string(maxi)}});
    }
    else {
        fig = plt::figure_size(1000,1000);
        plt::set_aspect_equal();
        vector<vector<double>> imgrid(ymax-ymin+1, vector<double>(xmax-xmin+1, 0.0));
        vector<double> listvals;
        for(int y = ymin; y <= ymax; y++) {
            for(int x = xmin; x <= xmax; x++) {
                double sum = 0.0;
                for(int iside = 0; iside < 3; iside++) {
                    cd val = grid[x+center[0]][y+center[1]][iside];
                    double col = std::real(val*std::conj(val));
                    sum += col;
                }
                listvals.push_back(sum);
                imgrid[y+center[1]][x+center[0]] = sum;
            }
        }
        std::sort(listvals.rbegin(), listvals.rend());
        double maxi = (listvals[0]+listvals[1])/2;
        if(maxi == 0.0)
            maxi = 1.0;
        maxi *= .6;

        double minx = (xmin-.5);
        double maxx = (xmax+.5);
        double miny = dy*(ymin-.5);
        double maxy = dy*(ymax+.5);
        plt::imshow(imgrid, {minx, maxx, miny, maxy}, {{"origin", "lower"}, {"cmap", "gist_heat_r"}, {"vmin", "0.0"}, {"vmax", std::to_string(maxi)}});
        plt::plot({0.0, 0.0}, {0.0, maxy}, {{"color","red"}});
        plt::plot({0.0, maxx}, {0.0, maxx*std::tan(M_PI/6)}, {{"color","red"}});
    }
    // Continued
    /*pyscript << "plt.savefig(\"" << prefix << "_" << iGrid << ".png\")\n";
    pyscript.flush();
    pyscript.close();*/
    std::ostringstream filename;
    filename << prefix << "_" << iGrid << ".png";
    plt::save(prefix + "/" + filename.str());
    plt::clf();
    plt::close(fig);
    Py_DECREF(fig);

    // Plot around dislocation line
    vector<double> xlist2, ylist2;
    for(int y = 0; y <= ymax; y++) {
        vector<int> klist;
        if(y%2)
            klist = {2, 0, 1};
        else
            klist = {0, 1, 2};
        for(int k : klist) {
            double rx, ry;
            std::tie(rx, ry) = real_coords(k, 0, y);
            xlist2.push_back(ry);
            cd val = grid[center[0]][center[1]+y][k];
            double col = std::real(val*std::conj(val));
            ylist2.push_back(col);
        }
    }
    plt::plot(xlist2, ylist2);
    std::ostringstream filename_disloc;
    filename_disloc << prefix << "_dislocation_" << iGrid << ".png";
    plt::save(prefix + "/" + filename_disloc.str());
    plt::clf();
    plt::close();
}

vector<double> dislocAmplitudes;

void step_walk(int step = -1) {
    std::cerr << "Begin step " << step << std::endl;
    grid = applyCoins(grid, U);
    for(int j = 0; j < 3; j++) {
        grid = applyCoins(grid, U*C0*U.adjoint());
        grid = shift(grid);
    }
    grid = applyCoins(grid, U.adjoint());
    double sumDisloc = 0;
    for(int y = 0; y <= ymax; y++)
        for(int k = 0; k < 3; k++)
            sumDisloc += sqmodulus(grid[center[0]][y+center[1]][k]);
    for(int y = 0; y <= ymax && 3*y+1 <= xmax; y++)
        for(int k = 0; k < 3; k++)
            sumDisloc += sqmodulus(grid[center[0]+3*y+1][center[1]+y][k]);
    dislocAmplitudes.push_back(sumDisloc);
    std::cerr << "Total amplitude: " << sumAmplitudes() << "\n";
    std::cerr << "End step " << step << std::endl;
}

void plotDislocAmplitude() {
    std::cerr << "Plotting dislocation amplitude" << std::endl;
    plt::plot(dislocAmplitudes);
    plt::save(prefix + "/" + prefix + "_disloc.png");
    plt::clf();
    plt::close();
}

void print_params() {
    std::ofstream ostream(prefix + "/settings.txt");
    ostream << "num_steps = " << num_steps << "\n";
    ostream << "initial state: " << initialStateName[initialState] << "\n";
    if(plotCone)
        ostream << "Cone coordinates\n";
    else
        ostream << "Unfolded cone\n";
    ostream << "xmin = " << xmin << "\n";
    ostream << "xmax = " << xmax << "\n";
    ostream << "ymin = " << ymin << "\n";
    ostream << "ymax = " << ymax << "\n";
    ostream << "U=\n" << U << "\n";
    ostream << "C0=\n" << C0 << std::endl;
    ostream.close();
}

void listInitialStates() {
    for(size_t i = 0; i < initialStateName.size(); i++)
        std::cerr << "- " << i << ": " << initialStateName[i] << "\n";
}

int main(int argc, char **argv)
{
    if(argc <= 2) {
        std::cerr << "Usage: " << std::string(argv[0]) << " <initial state> <plot cone>\n";
        std::cerr << "Initial states:\n";
        listInitialStates();
        std::cerr << "<plot cone> should be 1 if the figure should be plotted in cone coordinates, 0 otherwise.\n";
        return 1;
    }
    initialState = std::atoi(argv[1]);
    if(initialState < 0 || initialState >= (int)initialStateName.size()) {
        std::cerr << "Invalid initial state " << initialState << ". List of possible states:\n";
        listInitialStates();
        return 2;
    }
    plotCone = (bool) std::atoi(argv[2]);
    std::ostringstream str;
    str <<
        "simul_cone_newdisloc_" << initialStateName[initialState];
    if(plotCone)
        str << "_conecoord_";
    else
        str << "_altcoord_";
    str <<
        std::setw(4) << std::setfill('0') << ltm->tm_year+1900 << "-" <<
        std::setw(2) << ltm->tm_mon+1 << "-" <<
        std::setw(2) << ltm->tm_mday << "_" <<
        std::setw(2) << ltm->tm_hour << "-" <<
        std::setw(2) << ltm->tm_min << "-" <<
        std::setw(2) << ltm->tm_sec;
    prefix = str.str();
    std::filesystem::create_directory(prefix);
    init_HQ();
    print_params();
    // Initial state
    // A square
    if(initialState == 0) {
        for(int x = xmin/5; x <= xmax/5; x++)
            for(int y = ymin/5; y <= ymax/5; y++)
                for(int k = 0; k < 3; k++)
                    if(x <= 0 || y <= (x-1)/3)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    // Shifted
    else if(initialState == 1) {
        for(int k = 0; k < 3; k++)
            grid[center[0]][center[1]+ymax/2-1][k]=1/sqrt(3);
    }
    // Shifted (in another way)
    else if(initialState == 2) {
        for(int k = 0; k < 3; k++)
            grid[center[0]+xmin/4+1][center[1]+ymin/4+1][k]=1/sqrt(3);
    }
    // Centered
    else if(initialState == 3) {
        vector<vector<int> > centercoords = {{-1,-1},{-1,0},{0,-1},{0,0},{1,-1},{1,0}};
        for(vector<int> coord : centercoords)
            for(int k = 0; k < 3; k++)
                grid[center[0]+coord[0]][center[1]+coord[1]][k]=1/sqrt(3*centercoords.size());
    }
    // Shifted, both sides of the line
    else if(initialState == 4) {
        for(int k=0; k<3; k++) {
            grid[center[0]+10][center[1]+3][k] = 1/sqrt(6);
            grid[center[0]][center[1]+6][k] = 1/sqrt(6);
        }
    }
    // Everywhere, uniformly
    else if(initialState == 5) {
        for(int x = xmin; x <= xmax; x++)
            for(int y = ymin; y <= ymax; y++)
                if(x<=0 || y <= (x-1)/3)
                    for(int k = 0; k < 3; k++)
                        grid[center[0]+x][center[1]+y][k] = 1;
        normalizeGrid();
    }
    plot(0);
    for(int i = 0; i < num_steps; i++) {
        step_walk(i);
        if((i+1)%10 == 0)
            plot(i+1);
    }
    plotDislocAmplitude();

}


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

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <iostream>
#include <fstream>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>
#include <CGAL/IO/WKT.h>

namespace PS = CGAL::Polyline_simplification_2;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polygon_2<K>                                  Polygon_2;
typedef CGAL::Polygon_with_holes_2<K>                       Polygon_with_holes_2;

typedef PS::Vertex_base_2<K> Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> TDS;
typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, CGAL::Exact_predicates_tag> CDT;
typedef CGAL::Constrained_triangulation_plus_2<CDT>     CT;
typedef CT::Point                           Point;
typedef CT::Constraint_iterator             Constraint_iterator;
typedef CT::Vertices_in_constraint_iterator Vertices_in_constraint_iterator;
typedef CT::Points_in_constraint_iterator   Points_in_constraint_iterator;
typedef PS::Stop_below_count_ratio_threshold Stop;
typedef PS::Squared_distance_cost Cost;
int main(int argc, char* argv[])
{
  std::ifstream ifs( (argc==1)?"data/polygon.wkt":argv[1]);
  CT ct;
  Polygon_with_holes_2 P;
  while(CGAL::read_polygon_WKT(ifs, P)){
    const Polygon_2& poly = P.outer_boundary();
    ct.insert_constraint(poly);
    for(Polygon_with_holes_2::Hole_const_iterator it = P.holes_begin(); it != P.holes_end(); ++it){
      const Polygon_2& hole = *it;
      ct.insert_constraint(hole);
    }
  }
  PS::simplify(ct, Cost(), Stop(0.5));

  for(Constraint_iterator cit = ct.constraints_begin();
      cit != ct.constraints_end();
      ++cit) {
    std::cout << "simplified polyline" << std::endl;
    for(Points_in_constraint_iterator vit =
          ct.points_in_constraint_begin(*cit);
        vit != ct.points_in_constraint_end(*cit);
        ++vit)
      std::cout << *vit << std::endl;
  }
  return 0;
}

#else

int main()
{
  return 0;
}
#endif


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

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

#ifndef BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP
#define BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * The plane containing the Earth’s orbit around the Sun is called
 * the ecliptic and the other planets in our Solar System also move
 * in orbits close to this plane.
 *
 * When making calculations on objects in the Solar System it is
 * therefore often convenient to define positions with respect to
 * the ecliptic, that is, to use the ecliptic coordinate system.
 *
 * Ecliptic Latitude
 * The ecliptic plane is extended until it intersects the celestial
 * sphere to create an “ecliptic equator” as the reference point from
 * which Ecliptic Latitudes are measured. The Ecliptic Latitude, denoted
 * by β, is the angular distance that an object P lies above or below the
 * ecliptic plane and falls within the range ±90◦.
 * Latitudes above the ecliptic plane are positive angles while latitudes
 * below the ecliptic plane are negative angles. An object, such as the Sun,
 * whose orbit lies entirely within the ecliptic plane has an ecliptic latitude
 * of 0◦.
 *
 * Ecliptic Longitude
 * The Ecliptic Longitude, designated by λ, measures how far away an object is
 * from the First Point of Aries. The Ecliptic Longitude is in the range [0◦, 360◦]
 * and measured along the ecliptic toward the First Point of Aries.
 */

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename LatQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename LonQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct ecliptic_coord : public coord_sys
    <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
  ///@cond INTERNAL
  BOOST_STATIC_ASSERT_MSG(
      ((std::is_same<typename bu::get_dimension<LatQuantity>::type,
          bu::plane_angle_dimension>::value) &&
       (std::is_same<typename bu::get_dimension<LonQuantity>::type,
           bu::plane_angle_dimension>::value)),
      "Latitude and Longitude must be of plane angle type");
  BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                          "CoordinateType must be a floating-point type");
  ///@endcond
public:
    typedef LatQuantity quantity1;
    typedef LonQuantity quantity2;

    //Default constructor
    ecliptic_coord() {}

    ecliptic_coord
    (
            LatQuantity const &Lat,
            LonQuantity const &Lon
    )
    {
        this->set_lat_lon(Lat, Lon);
    }

    //Create a tuple of Ecliptic Latitude and Ecliptic Longitude
    std::tuple<LatQuantity, LonQuantity> get_lat_lon() const
    {
        return std::make_tuple(this->get_lat(), this->get_lon());
    }

    //Get Ecliptic Latitude
    LatQuantity get_lat() const
    {
        return static_cast<LatQuantity>
        (
            bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                    (bg::get<0>(this->point))
        );
    }

    //Get Ecliptic Longitude
    LonQuantity get_lon() const
    {
        return static_cast<LonQuantity>
        (
                bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                        (bg::get<1>(this->point))
        );
    }

    //Set value of Ecliptic Latitude and Ecliptic Longitude
    void set_lat_lon
    (
            LatQuantity const &Lat,
            LonQuantity const &Lon
    )
    {
        this->set_lat(Lat);
        this->set_lon(Lon);
    }

    //Set Ecliptic Latitude
    void set_lat(LatQuantity const &Lat)
    {
        bg::set<0>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lat).value()
            );
    }

    //Set Ecliptic Longitude
    void set_lon(LonQuantity const &Lon)
    {
        bg::set<1>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Lon).value()
            );
    }

}; //ecliptic_coord

//Make Ecliptic Coordinate
template
<
    typename CoordinateType,
    template<typename Unit2, typename CoordinateType_> class LatQuantity,
    template<typename Unit1, typename CoordinateType_> class LonQuantity,
    typename Unit1,
    typename Unit2
>
ecliptic_coord
<
    CoordinateType,
    LatQuantity<Unit2, CoordinateType>,
    LonQuantity<Unit1, CoordinateType>
> make_ecliptic_coord
(
    LatQuantity<Unit2, CoordinateType> const &Lat,
    LonQuantity<Unit1, CoordinateType> const &Lon
)
{
    return ecliptic_coord
        <
            CoordinateType,
            LatQuantity<Unit2, CoordinateType>,
            LonQuantity<Unit1, CoordinateType>
        > (Lat, Lon);
}

//Print Ecliptic Coordinates
template
<
    typename CoordinateType,
    class LatQuantity,
    class LonQuantity
>
std::ostream &operator << (std::ostream &out, ecliptic_coord
        <CoordinateType, LatQuantity, LonQuantity> const &point) {
    out << "Ecliptic Coordinate (Ecliptic Latitude: "
        << point.get_lat() << ", Ecliptic Longitude: "
        << point.get_lon() << ")";

    return out;
}

}}}

#endif //BOOST_ASTRONOMY_ECLIPTIC_COORD_HPP


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with DSGD. We first creates factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using DSGD.
 *
 * Run with: mpirun --hosts localhost,localhost dsgd
 * (make sure to use a production build, otherwise it will be slow)
 */
#include <iostream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateSl> Update;
typedef RegularizeTruncate<RegularizeSl> Regularize;
typedef SlLoss Loss;

int main(int argc, char* argv[]) {
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 5000000;
	mf_size_type r = 10;

	// parameters for distribution
	int tasksPerRank = 2;
	mf_size_type blocks1 = world.size() * tasksPerRank;
	mf_size_type blocks2 = world.size() * tasksPerRank;

	// parameters for SGD
	double epsMax = 0.001;
	mf_size_type epochs = 20;
	SgdOrder order = SGD_ORDER_WOR;
	StratumOrder stratumOrder = STRATUM_ORDER_WOR;
	Update update = Update((UpdateSl()), 0, 100);
	Regularize regularize((RegularizeSl()), 0, 100);
	Loss loss;

	// start mf library
	// TODO: need automatic registration
	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

		// TODO: distribute matrix generation
		// generate original factors by sampling from a uniform(0,1) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::uniform_real<>(0,1));
		generateRandom(hIn, random, boost::uniform_real<>(0,1));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and sample from a Poisson with mean equal to the entry
		// TODO: this generation process does not match the factorization model since we sample
		//       from the Poisson only at some entries of wh
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));

		// take a small sample and remove empty rows/columns
		ProjectedSparseMatrix Vsample;
		projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
		projectFrequent(Vsample, 0);
		LOG4CXX_INFO(logger, "Sample matrix: "
			<< Vsample.data.size1() << " x " << Vsample.data.size2()
			<< ", " << Vsample.data.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[0,1] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(0, 1));
		generateRandom(h, random, boost::uniform_real<>(0, 1));

		// distribute the input matrices
		DistributedSparseMatrix dv = distributeMatrix("V", blocks1, blocks2, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
					<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedDenseMatrix dw = distributeMatrix("W", blocks1, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks2, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize the DSGD
		Timer t;
		DsgdRunner dsgdRunner(random);
		DsgdJob<Update,Regularize> dsgdJob(dv, dw, dh, update, regularize, order,
				stratumOrder, false, tasksPerRank);
		DistributedDecayAuto<Update,Regularize,Loss> decay(dsgdJob, loss, Vsample, "decay", epsMax,
				world.size()*tasksPerRank);
		Trace trace;

		// run DSGD to try to reconstruct the original factors
		t.start();
		dsgdRunner.run(dsgdJob, loss, epochs, decay, trace);
		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/dsgd-gkl-trace.R");
		trace.toRfile("/tmp/dsgd-gkl-trace.R", "dsgd.gkl");
	}

	mfStop();
	mfFinalize();

	return 0;
}


/*
 Copyright (c) 2010, The Barbarian Group
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and
	the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
	the following disclaimer in the documentation and/or other materials provided with the distribution.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
*/

#include "cinder/PolyLine.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/multi.hpp>

namespace cinder {

template<typename T>
bool PolyLineT<T>::isClockwise( bool *isColinear ) const
{
	if( mPoints.size() < 3 ) {
		if( isColinear != nullptr ) *isColinear = true;
		return false;
	}

	size_t last = mPoints.size() - 1;
	// If the first and last point are the same (on a closed polygon), ignore one.
	if( mPoints.front() == mPoints.back() ) --last;

	// Find an extreme point since we know it will be on the hull...
	size_t smallest = 0;
	for( size_t i = 1; i <= last; ++i ) {
		if( mPoints[i].x < mPoints[smallest].x ) {
			smallest = i;
		}
		else if( mPoints[i].x == mPoints[smallest].x && mPoints[i].y < mPoints[smallest].y ) {
			smallest = i;
		}
	};
	// ...then get the next and previous point
	size_t prev = ( smallest == 0 )    ? last : ( smallest - 1 );
	size_t next = ( smallest == last ) ? 0    : ( smallest + 1 );
	T a = mPoints[next], b = mPoints[smallest], c = mPoints[prev];

	// The sign of the determinate indicates the orientation:
	//   positive is clockwise
	//   zero is colinear
	//   negative is counterclockwise
	double determinate = ( b.x - a.x ) * ( c.y - a.y ) - ( c.x - a.x ) * ( b.y - a.y );
	if( isColinear != nullptr ) *isColinear = determinate == 0.0;
	return determinate > 0.0;
}

template<typename T>
bool PolyLineT<T>::isCounterclockwise( bool *isColinear ) const
{
	bool colinear;
	bool clockwise = this->isClockwise( &colinear );
	if( isColinear != nullptr ) *isColinear = colinear;
	return colinear ? false : ! clockwise;
}

template<typename T>
T PolyLineT<T>::getPosition( float t ) const
{
	typedef typename T::value_type R;
	if( mPoints.size() <= 1 ) return T();
	if( t >= 1 ) return mPoints.back();
	if( t <= 0 ) return mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	R lerpT = ( t - span / (R)numSpans ) * numSpans;
	return mPoints[span] * ( 1 - lerpT ) + mPoints[span+1] * lerpT;
}

template<typename T>
T PolyLineT<T>::getDerivative( float t ) const
{
	typedef typename T::value_type R;
	if( mPoints.size() <= 1 ) return T();
	if( t >= 1 ) return mPoints.back() - mPoints[mPoints.size()-2];
	if( t <= 0 ) return mPoints[1] - mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	return mPoints[span+1] - mPoints[span];
}

template<typename T>
void PolyLineT<T>::scale( const T &scaleFactor, T scaleCenter )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt = scaleCenter + ( *ptIt - scaleCenter ) * scaleFactor;
}

template<typename T>
PolyLineT<T> PolyLineT<T>::scaled( const T &scaleFactor, T scaleCenter ) const
{
	PolyLineT<T> result( *this );
	result.scale( scaleFactor, scaleCenter );
	return result;
}

template<typename T>
void PolyLineT<T>::offset( const T &offsetBy )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt += offsetBy;
}

template<typename T>
PolyLineT<T> PolyLineT<T>::getOffset( const T &offsetBy ) const
{
	PolyLineT<T> result( *this );
	result.offset( offsetBy );
	return result;
}

template<typename T>
void PolyLineT<T>::reverse()
{
	std::reverse( mPoints.begin(), mPoints.end() );
}

template<typename T>
PolyLineT<T> PolyLineT<T>::reversed() const
{
	PolyLineT result( *this );
	std::reverse( result.mPoints.begin(), result.mPoints.end() );
	return result;
}

template<typename T>
T linearYatX( const glm::tvec2<T, glm::defaultp> p[2], T x )
{
	if( p[0].x == p[1].x ) 	return p[0].y;
	return p[0].y + (p[1].y - p[0].y) * (x - p[0].x) / (p[1].x - p[0].x);
}

template<typename T>
size_t linearCrossings( const glm::tvec2<T, glm::defaultp> p[2], const vec2 &pt )
{
	if( (p[0].x < pt.x && pt.x <= p[1].x ) ||
		(p[1].x < pt.x && pt.x <= p[0].x )) {
		if( pt.y > linearYatX<T>( p, pt.x ) )
			return 1;
	}
	return 0;
}

template<typename T>
bool PolyLineT<T>::contains( const vec2 &pt ) const
{
	if( mPoints.size() <= 2 )
		return false;

	size_t crossings = 0;
	for( size_t s = 0; s < mPoints.size() - 1; ++s ) {
		crossings += linearCrossings( &(mPoints[s]), pt );
	}

	T temp[2];
	temp[0] = mPoints[mPoints.size()-1];
	temp[1] = mPoints[0];
	crossings += linearCrossings( &(temp[0]), pt );

	return (crossings & 1) == 1;
}

template<typename T>
double PolyLineT<T>::calcArea() const
{
	double sum = 0;
	const size_t numPoints = mPoints.size();
	if( numPoints > 2 ) {
		for( size_t i = 0; i < numPoints - 1; ++i )
			sum += mPoints[i].x * mPoints[i+1].y - mPoints[i+1].x * mPoints[i].y;
		sum += mPoints[numPoints-1].x * mPoints[0].y - mPoints[0].x * mPoints[numPoints-1].y;
	}

	return glm::abs(sum * 0.5);
}

template<typename T>
T PolyLineT<T>::calcCentroid() const
{
	T result( 0 );

	const size_t numPoints = mPoints.size();
	double area = 0;
	if( numPoints > 2 ) {
		for( size_t i = 0; i < numPoints - 1; ++i ) {
			double subExpr = mPoints[i].x * mPoints[i+1].y - mPoints[i+1].x * mPoints[i].y;
			result.x += ( mPoints[i].x + mPoints[i+1].x ) * subExpr;
			result.y += ( mPoints[i].y + mPoints[i+1].y ) * subExpr;
			area += subExpr;
		}
		double subExpr = mPoints[numPoints-1].x * mPoints[0].y - mPoints[0].x * mPoints[numPoints-1].y;
		result.x += ( mPoints[numPoints-1].x + mPoints[0].x ) * subExpr;
		result.y += ( mPoints[numPoints-1].y + mPoints[0].y ) * subExpr;
		area += subExpr;

		result *= 1 / ( area * 3 );
	}

	return result;
}

namespace {
typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

template<typename T>
std::vector<PolyLineT<T> > convertBoostGeometryPolygons( std::vector<polygon> &polygons )
{
	std::vector<PolyLineT<T> > result;
	for( std::vector<polygon>::const_iterator outIt = polygons.begin(); outIt != polygons.end(); ++outIt ) {
		typedef polygon::inner_container_type::const_iterator RingIterator;
		typedef polygon::ring_type::const_iterator PointIterator;

		result.push_back( PolyLineT<T>() );
		for( PointIterator pt = outIt->outer().begin(); pt != outIt->outer().end(); ++pt )
			result.back().push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );

		for( RingIterator crunk = outIt->inners().begin(); crunk != outIt->inners().end(); ++crunk ) {
			PolyLineT<T> contour;
			for( PointIterator pt = crunk->begin(); pt != crunk->end(); ++pt )
				contour.push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );
			result.push_back( contour );
		}
	}

	return result;
}

template<typename T>
polygon convertPolyLinesToBoostGeometry( const std::vector<PolyLineT<T> > &a )
{
	polygon result;

	for( typename std::vector<T>::const_iterator ptIt = a[0].getPoints().begin(); ptIt != a[0].getPoints().end(); ++ptIt )
		result.outer().push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
	for( typename std::vector<PolyLineT<T> >::const_iterator plIt = a.begin() + 1; plIt != a.end(); ++plIt ) {
		polygon::ring_type ring;
		for( typename std::vector<T>::const_iterator ptIt = plIt->getPoints().begin(); ptIt != plIt->getPoints().end(); ++ptIt )
			ring.push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
		result.inners().push_back( ring );
	}

	boost::geometry::correct( result );

	return result;
}
} // anonymous namespace

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcUnion( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::union_( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcIntersection( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::intersection( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcXor( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::sym_difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLineT<T> > PolyLineT<T>::calcDifference( const std::vector<PolyLineT<T> > &a, std::vector<PolyLineT<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template class PolyLineT<vec2>;
template class PolyLineT<dvec2>;

} // namespace cinder


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

// a class to hold the coordinates of the straight line embedding
struct coord_t
{
    std::size_t x;
    std::size_t y;
};

int main(int argc, char** argv)
{
    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int > >
        graph;

    // Define the storage type for the planar embedding
    typedef std::vector< std::vector< graph_traits< graph >::edge_descriptor > >
        embedding_storage_t;
    typedef boost::iterator_property_map< embedding_storage_t::iterator,
        property_map< graph, vertex_index_t >::type >
        embedding_t;

    // Create the graph - a maximal planar graph on 7 vertices. The functions
    // planar_canonical_ordering and chrobak_payne_straight_line_drawing both
    // require a maximal planar graph. If you start with a graph that isn't
    // maximal planar (or you're not sure), you can use the functions
    // make_connected, make_biconnected_planar, and make_maximal planar in
    // sequence to add a set of edges to any undirected planar graph to make
    // it maximal planar.

    graph g(7);
    add_edge(0, 1, g);
    add_edge(1, 2, g);
    add_edge(2, 3, g);
    add_edge(3, 0, g);
    add_edge(3, 4, g);
    add_edge(4, 5, g);
    add_edge(5, 6, g);
    add_edge(6, 3, g);
    add_edge(0, 4, g);
    add_edge(1, 3, g);
    add_edge(3, 5, g);
    add_edge(2, 6, g);
    add_edge(1, 4, g);
    add_edge(1, 5, g);
    add_edge(1, 6, g);

    // Create the planar embedding
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

    boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
        boyer_myrvold_params::embedding = embedding);

    // Find a canonical ordering
    std::vector< graph_traits< graph >::vertex_descriptor > ordering;
    planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

    // Set up a property map to hold the mapping from vertices to coord_t's
    typedef std::vector< coord_t > straight_line_drawing_storage_t;
    typedef boost::iterator_property_map<
        straight_line_drawing_storage_t::iterator,
        property_map< graph, vertex_index_t >::type >
        straight_line_drawing_t;

    straight_line_drawing_storage_t straight_line_drawing_storage(
        num_vertices(g));
    straight_line_drawing_t straight_line_drawing(
        straight_line_drawing_storage.begin(), get(vertex_index, g));

    // Compute the straight line drawing
    chrobak_payne_straight_line_drawing(
        g, embedding, ordering.begin(), ordering.end(), straight_line_drawing);

    std::cout << "The straight line drawing is: " << std::endl;
    graph_traits< graph >::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    {
        coord_t coord(get(straight_line_drawing, *vi));
        std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                  << std::endl;
    }

    // Verify that the drawing is actually a plane drawing
    if (is_straight_line_drawing(g, straight_line_drawing))
        std::cout << "Is a plane drawing." << std::endl;
    else
        std::cout << "Is not a plane drawing." << std::endl;

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_HYPOT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  hypot generic tag

      Represents the hypot function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct hypot_ : ext::elementwise_<hypot_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<hypot_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_hypot_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::hypot_, Site> dispatching_hypot_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::hypot_, Site>();
    }
    template<class... Args>
    struct impl_hypot_;
  }
  /*!
    Computes \f$(x^2 + y^2)^{1/2}\f$

    @par semantic:
    For any given value @c x,  @c y of floating type @c T:

    @code
    T r = hypot(x, y);
    @endcode

    The code is similar to:

    @code
    T r = sqrt(sqr(x)+sqr(y));
    @endcode

    Provision are made to avoid overflow as possible and to compute
    @c hypot accurately.
    If these considerations can be put aside use
    @c fast_hypot.

    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::hypot_, hypot, 2)
} }

#endif


#ifndef MATHPARSER_HPP
#define MATHPARSER_HPP

/*=============================================================================
    Copyright (c) 2004 Angus Leeming

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#if defined PHOENIX_LIMIT && PHOENIX_LIMIT < 6
#undef PHOENIX_LIMIT
#endif

#ifndef PHOENIX_LIMIT
#define PHOENIX_LIMIT 6
#endif

#define YAC_SINGLE_COMPILATION_UNIT

#include "external/yac/yac_gnuplot_grammar.hpp"
#include "external/yac/yac_skip_grammar.hpp"
#include "external/yac/yac_virtual_machine.hpp"

#include <boost/spirit/iterator/file_iterator.hpp>
#include <boost/math/special_functions/acosh.hpp>
#include <boost/math/special_functions/asinh.hpp>
#include <boost/math/special_functions/atanh.hpp>

#include <iostream>
#include <sstream>
#include <string>

using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::string;

namespace spirit = boost::spirit;

namespace {

int    bitwise_and(int a, int b) { return a & b; }
int    bitwise_or(int a, int b) { return a | b; }
int    bitwise_xor(int a, int b) { return a ^ b; }
bool   logical_and(bool a, bool b) { return a && b; }
double logical_not(double a) { return !a; }
double equal(double a, double b) { return a == b; }
double not_equal(double a, double b) { return a != b; }
double less(double a, double b) { return a < b; }
double greater(double a, double b) { return a > b; }
double less_equal(double a, double b) { return a <= b; }
double greater_equal(double a, double b) { return a >= b; }
int    shift_left(int a, int b) { return a << b; }
int    shift_right(int a, int b) { return a >> b; }
double add(double a, double b) { return a + b; }
double subtract(double a, double b) { return a - b; }
double multiply(double a, double b) { return a * b; }
double divide(double a, double b) { return a / b; }
int    mod(int a, int b) { return a % b; }
double negate(double a) { return -a; }
double sgn(double a) { return a >= 0.0 ? 1.0 : -1.0; }

typedef int(* ifunc2_ptr_t)(int, int);
typedef bool(* bfunc2_ptr_t)(bool, bool);
typedef double(* dfunc2_ptr_t)(double, double);
typedef double(* dfunc1_ptr_t)(double);

} // namespace anon

namespace yac {

boost::shared_ptr<function> make_func(string const & name, dfunc1_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function1<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, dfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, bfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<bool>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, ifunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<int>(name, ptr));
}

class controller {
private:
	  virtual_machine vm;
	  std::vector<RealType> x_vector, y_vector;
public:
  RealType time;
  controller():
    time(0.)
    {
      std::vector<RealType> X;
      std::vector<RealType> Y;
      X.push_back(0.);
      Y.push_back(0.);
      x_vector = X;
      y_vector = Y;

      char const * const keywords[] = {
	"x", "y",
	"return",
	"pi",
	"abs", "acos", "acosh", "asin", "asinh", "atan",
	"atanh", "besj0", "besj1", "besy0", "besy1", "ceil",
	"cos", "cosh",
#if defined(__GNUC__)
	"erf", "erfc",
#endif
	"exp", "floor", "log", "log10", "sgn", "sin", "sinh",
	"sqrt", "tan", "tanh", "atan2",
      };
      std::size_t const keywords_size =
	sizeof(keywords) / sizeof(keywords[0]);

      for (std::size_t i = 0; i != keywords_size; ++i)
	name_grammar::reserved_keywords.add(keywords[i]);

      vm.global_vars.add
	("pi", 3.1415926536);

      vm.funcs.add
	("bitwise_and##2",   make_func("bitwise_and#",   &::bitwise_and))
	("bitwise_or##2",    make_func("bitwise_or#",    &::bitwise_or))
	("bitwise_xor##2",   make_func("bitwise_xor#",   &::bitwise_xor))
	("logical_and##2",   make_func("logical_and#",   &::logical_and))
	("logical_not##1",   make_func("logical_not#",   &::logical_not))
	("equal##2",         make_func("equal#",         &::equal))
	("not_equal##2",     make_func("not_equal#",     &::not_equal))
	("less##2",          make_func("less#",          &::less))
	("greater##2",       make_func("greater#",       &::greater))
	("less_equal##2",    make_func("less_equal#",    &::less_equal))
	("greater_equal##2", make_func("greater_equal#", &::greater_equal))
	("shift_left##2",    make_func("shift_left#",    &::shift_left))
	("shift_right##2",   make_func("shift_right#",   &::shift_right))
	("add##2",           make_func("add#",           &::add))
	("subtract##2",      make_func("subtract#",      &::subtract))
	("multiply##2",      make_func("multiply#",      &::multiply))
	("divide##2",        make_func("divide#",        &::divide))
	("mod##2",           make_func("mod#",           &::mod))
	("negate##1",        make_func("negate#",        &::negate))
	("abs#1",   make_func("abs",   &std::abs))
	("acos#1",  make_func("acos",  &std::acos))
	("asin#1",  make_func("asin",  &std::asin))
	("atan#1",  make_func("atan",  &std::atan))
	("acosh#1", make_func("acosh", &boost::math::acosh))
	("asinh#1", make_func("asinh", &boost::math::asinh))
	("atanh#1", make_func("atanh", &boost::math::atanh))
	("besj0#1", make_func("besj0", &j0))
	("besj1#1", make_func("besj1", &j1))
	("besy0#1", make_func("besy0", &y0))
	("besy1#1", make_func("besy1", &y1))
	("ceil#1",  make_func("ceil",  &std::ceil))
	("cos#1",   make_func("cos",   &std::cos))
	("cosh#1",  make_func("cosh",  &std::cosh))
#if defined(__GNUC__)
	("erf#1",   make_func("erf",   &erf))
	("erfc#1",  make_func("erfc",  &erfc))
#endif
	("exp#1",   make_func("exp",   &std::exp))
	("floor#1", make_func("floor", &std::floor))
	("log#1",   make_func("log",   &std::log))
	("log10#1", make_func("log10", &std::log10))
	("sgn#1",   make_func("sgn",   &sgn))
	("sin#1",   make_func("sin",   &std::sin))
	("sinh#1",  make_func("sinh",  &std::sinh))
	("sqrt#1",  make_func("sqrt",  &std::sqrt))
	("tan#1",   make_func("tan",   &std::tan))
	("tanh#1",  make_func("tanh",  &std::tanh))
	("atan2#2", make_func("atan2", &std::atan2));
    }

  controller(std::vector<RealType> &X, std::vector<RealType> &Y, RealType &time_)
    : x_vector(X), y_vector(Y), time(time_)
  {
        char const * const keywords[] = {
	  "x", "y", "t",
	  "return",
	  "pi",
            "abs", "acos", "acosh", "asin", "asinh", "atan",
            "atanh", "besj0", "besj1", "besy0", "besy1", "ceil",
            "cos", "cosh",
#if defined(__GNUC__)
            "erf", "erfc",
#endif
            "exp", "floor", "log", "log10", "sgn", "sin", "sinh",
            "sqrt", "tan", "tanh", "atan2",
        };
        std::size_t const keywords_size =
            sizeof(keywords) / sizeof(keywords[0]);

        for (std::size_t i = 0; i != keywords_size; ++i)
                name_grammar::reserved_keywords.add(keywords[i]);

        vm.global_vars.add
	  ("pi", 3.1415926536);

        vm.funcs.add
            ("bitwise_and##2",   make_func("bitwise_and#",   &::bitwise_and))
            ("bitwise_or##2",    make_func("bitwise_or#",    &::bitwise_or))
            ("bitwise_xor##2",   make_func("bitwise_xor#",   &::bitwise_xor))
            ("logical_and##2",   make_func("logical_and#",   &::logical_and))
            ("logical_not##1",   make_func("logical_not#",   &::logical_not))
            ("equal##2",         make_func("equal#",         &::equal))
            ("not_equal##2",     make_func("not_equal#",     &::not_equal))
            ("less##2",          make_func("less#",          &::less))
            ("greater##2",       make_func("greater#",       &::greater))
            ("less_equal##2",    make_func("less_equal#",    &::less_equal))
            ("greater_equal##2", make_func("greater_equal#", &::greater_equal))
            ("shift_left##2",    make_func("shift_left#",    &::shift_left))
            ("shift_right##2",   make_func("shift_right#",   &::shift_right))
            ("add##2",           make_func("add#",           &::add))
            ("subtract##2",      make_func("subtract#",      &::subtract))
            ("multiply##2",      make_func("multiply#",      &::multiply))
            ("divide##2",        make_func("divide#",        &::divide))
            ("mod##2",           make_func("mod#",           &::mod))
            ("negate##1",        make_func("negate#",        &::negate))
            ("abs#1",   make_func("abs",   &std::abs))
            ("acos#1",  make_func("acos",  &std::acos))
            ("asin#1",  make_func("asin",  &std::asin))
            ("atan#1",  make_func("atan",  &std::atan))
            ("acosh#1", make_func("acosh", &boost::math::acosh))
            ("asinh#1", make_func("asinh", &boost::math::asinh))
            ("atanh#1", make_func("atanh", &boost::math::atanh))
            ("besj0#1", make_func("besj0", &j0))
            ("besj1#1", make_func("besj1", &j1))
            ("besy0#1", make_func("besy0", &y0))
            ("besy1#1", make_func("besy1", &y1))
            ("ceil#1",  make_func("ceil",  &std::ceil))
            ("cos#1",   make_func("cos",   &std::cos))
            ("cosh#1",  make_func("cosh",  &std::cosh))
#if defined(__GNUC__)
            ("erf#1",   make_func("erf",   &erf))
            ("erfc#1",  make_func("erfc",  &erfc))
#endif
            ("exp#1",   make_func("exp",   &std::exp))
            ("floor#1", make_func("floor", &std::floor))
            ("log#1",   make_func("log",   &std::log))
            ("log10#1", make_func("log10", &std::log10))
            ("sgn#1",   make_func("sgn",   &sgn))
            ("sin#1",   make_func("sin",   &std::sin))
            ("sinh#1",  make_func("sinh",  &std::sinh))
            ("sqrt#1",  make_func("sqrt",  &std::sqrt))
            ("tan#1",   make_func("tan",   &std::tan))
            ("tanh#1",  make_func("tanh",  &std::tanh))
            ("atan2#2", make_func("atan2", &std::atan2));
    }

  void updateReservedVariables( std::vector<RealType> &X,
				std::vector<RealType> &Y )
  {
    x_vector = X;
    y_vector = Y;
  }

  void updateTime( RealType& t ) {
    time = t;
  }

    template <typename ItT>
    std::vector<RealType> parse( ItT first, ItT last )
  {
        using phoenix::arg1;
        using phoenix::var;

        typedef spirit::parse_info<ItT> parse_info_t;

        gnuplot_grammar calculator(vm.funcs, vm.global_vars);

        parse_info_t info = spirit::parse(first, last,
                                          calculator[var(vm.stk) = arg1],
                                          skip_grammar());

        if (info.full) {
	  std::vector<RealType> result = vectorizedEval(vm.stk,
						      x_vector,
						      y_vector, time );
	  return result;
        } else {
            cerr << "-------------------------\n"
                 << "Math parsing failed\n"
                 << "-------------------------\n";
	    return std::vector<RealType> ( x_vector.size(), 0.0 );
        }
    }
};

} // namespace yac

#endif // MATHPARSER_HPP