#include "pch.h"
#include "Minkowski.h"
#include <boost/polygon/polygon.hpp>

#undef min
#undef max

typedef boost::polygon::point_data<int> point;
typedef boost::polygon::polygon_set_data<int> polygon_set;
typedef boost::polygon::polygon_with_holes_data<int> polygon;
typedef std::pair<point, point> edge;

using namespace MinkowskiCpp;
using namespace Platform;
using namespace boost::polygon::operators;
using namespace std;

Minkowski::Minkowski()
{
}

void convolve_two_segments(std::vector<point>& figure, const edge& a, const edge& b) {
    using namespace boost::polygon;
    figure.clear();
    figure.push_back(point(a.first));
    figure.push_back(point(a.first));
    figure.push_back(point(a.second));
    figure.push_back(point(a.second));
    convolve(figure[0], b.second);
    convolve(figure[1], b.first);
    convolve(figure[2], b.first);
    convolve(figure[3], b.second);
}

template <typename itrT1, typename itrT2>
void convolve_two_point_sequences(polygon_set& result, itrT1 ab, itrT1 ae, itrT2 bb, itrT2 be) {
    using namespace boost::polygon;
    if (ab == ae || bb == be)
        return;
    point first_a = *ab;
    point prev_a = *ab;
    std::vector<point> vec;
    polygon poly;
    ++ab;
    for (; ab != ae; ++ab) {
        point first_b = *bb;
        point prev_b = *bb;
        itrT2 tmpb = bb;
        ++tmpb;
        for (; tmpb != be; ++tmpb) {
            convolve_two_segments(vec, std::make_pair(prev_b, *tmpb), std::make_pair(prev_a, *ab));
            set_points(poly, vec.begin(), vec.end());
            result.insert(poly);
            prev_b = *tmpb;
        }
        prev_a = *ab;
    }
}

template <typename itrT>
void convolve_point_sequence_with_polygons(polygon_set& result, itrT b, itrT e, const std::vector<polygon>& polygons) {
    using namespace boost::polygon;
    for (std::size_t i = 0; i < polygons.size(); ++i) {
        convolve_two_point_sequences(result, b, e, begin_points(polygons[i]), end_points(polygons[i]));
        for (polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(polygons[i]);
            itrh != end_holes(polygons[i]); ++itrh) {
            convolve_two_point_sequences(result, b, e, begin_points(*itrh), end_points(*itrh));
        }
    }
}

void convolve_two_polygon_sets(polygon_set& result, const polygon_set& a, const polygon_set& b) {
    using namespace boost::polygon;
    result.clear();
    std::vector<polygon> a_polygons;
    std::vector<polygon> b_polygons;
    a.get(a_polygons);
    b.get(b_polygons);
    for (std::size_t ai = 0; ai < a_polygons.size(); ++ai) {
        convolve_point_sequence_with_polygons(result, begin_points(a_polygons[ai]),
            end_points(a_polygons[ai]), b_polygons);
        for (polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(a_polygons[ai]);
            itrh != end_holes(a_polygons[ai]); ++itrh) {
            convolve_point_sequence_with_polygons(result, begin_points(*itrh),
                end_points(*itrh), b_polygons);
        }
        for (std::size_t bi = 0; bi < b_polygons.size(); ++bi) {
            polygon tmp_poly = a_polygons[ai];
            result.insert(convolve(tmp_poly, *(begin_points(b_polygons[bi]))));
            tmp_poly = b_polygons[bi];
            result.insert(convolve(tmp_poly, *(begin_points(a_polygons[ai]))));
        }
    }
}

double inputscale;

using namespace boost::polygon;

vector<double> Avec;
vector<double> Bvec;
vector<vector<double>> chlds;
vector<vector<double>> retpnts;
vector<vector<double>> retholes;


void MinkowskiCpp::Minkowski::getResults(Platform::WriteOnlyArray<double>^ data, Platform::WriteOnlyArray<double>^ holesData)
{
    int index = 0;
    for (int i = 0; i < retpnts.size(); i++) {
        for (int j = 0; j < retpnts[i].size(); j++) {
            data[index] = retpnts[i][j];
            index++;
        }
    }

    index = 0;
    for (int i = 0; i < retholes.size(); i++) {
        for (int j = 0; j < retholes[i].size(); j++) {
            holesData[index] = retholes[i][j];
            index++;
        }
    }
}

void MinkowskiCpp::Minkowski::getSizes1(Platform::WriteOnlyArray<int>^ sizes)
{
    sizes[0] = retpnts.size();
    sizes[1] = retholes.size();
}

void MinkowskiCpp::Minkowski::getSizes2(Platform::WriteOnlyArray<int>^ sizes1, Platform::WriteOnlyArray<int>^ sizes2)
{
    for (int i = 0; i < retpnts.size(); i++) {
        sizes1[i] = retpnts[i].size();
    }
    for (int i = 0; i < retholes.size(); i++) {
        sizes2[i] = retholes[i].size();
    }
}

void MinkowskiCpp::Minkowski::setData(int cntA,
    const Platform::Array<double>^ pntsA,
    int holesCnt,
    const Platform::Array<int>^ holesSizes,
    const Platform::Array<double>^ holesPoints,
    int cntB,
    const Platform::Array<double>^ pntsB)
{
    Avec.clear();
    Bvec.clear();
    chlds.clear();

    retpnts.clear();
    retholes.clear();

    for (int i = 0; i < cntA; i++) {
        Avec.push_back(pntsA[i]);
    }
    for (int i = 0; i < cntB; i++) {
        Bvec.push_back(pntsB[i]);
    }

    int index = 0;
    for (int i = 0; i < holesCnt; i++) {
        chlds.push_back(vector<double>());
        for (int j = 0; j < holesSizes[i]; j++) {
            chlds.back().push_back(holesPoints[index]);
            index++;
        }
    }
}


void MinkowskiCpp::Minkowski::calculateNFP()
{

    polygon_set a, b, c;
    std::vector<polygon> polys;
    std::vector<point> pts;



    // get maximum bounds for scaling factor
    unsigned int len = Avec.size() / 2;
    double Amaxx = 0;
    double Aminx = 0;
    double Amaxy = 0;
    double Aminy = 0;
    for (unsigned int i = 0; i < len; i++) {
        double x1 = Avec[i * 2];
        double y1 = Avec[i * 2 + 1];
        Amaxx = (std::max)(Amaxx, (double)x1);
        Aminx = (std::min)(Aminx, (double)x1);
        Amaxy = (std::max)(Amaxy, (double)y1);
        Aminy = (std::min)(Aminy, (double)y1);
    }

    len = Bvec.size() / 2;
    double Bmaxx = 0;
    double Bminx = 0;
    double Bmaxy = 0;
    double Bminy = 0;
    for (unsigned int i = 0; i < len * 2; i += 2) {
        double x1 = Bvec[i];
        double y1 = Bvec[i + 1];
        Bmaxx = (std::max)(Bmaxx, (double)x1);
        Bminx = (std::min)(Bminx, (double)x1);
        Bmaxy = (std::max)(Bmaxy, (double)y1);
        Bminy = (std::min)(Bminy, (double)y1);
    }

    double Cmaxx = Amaxx + Bmaxx;
    double Cminx = Aminx + Bminx;
    double Cmaxy = Amaxy + Bmaxy;
    double Cminy = Aminy + Bminy;

    double maxxAbs = (std::max)(Cmaxx, std::fabs(Cminx));
    double maxyAbs = (std::max)(Cmaxy, std::fabs(Cminy));

    double maxda = (std::max)(maxxAbs, maxyAbs);
    int maxi = std::numeric_limits<int>::max();

    if (maxda < 1) {
        maxda = 1;
    }

    // why 0.1? dunno. it doesn't screw up with 0.1
    inputscale = (0.1f * (double)(maxi)) / maxda;

    //double scale = 1000;
    len = Avec.size() / 2;

    for (unsigned int i = 0; i < len; i++) {
        double x1 = Avec[i * 2];
        double y1 = Avec[i * 2 + 1];
        int x = (int)(inputscale * (double)x1);
        int y = (int)(inputscale * (double)y1);

        pts.push_back(point(x, y));
    }

    polygon poly;
    boost::polygon::set_points(poly, pts.begin(), pts.end());
    a += poly;

    // subtract holes from a here...

    len = chlds.size();

    for (unsigned int i = 0; i < len; i++) {
        vector<double> hole = chlds[i];
        pts.clear();
        unsigned int hlen = hole.size() / 2;
        for (unsigned int j = 0; j < hlen; j++) {
            double x1 = hole[j * 2];
            double y1 = hole[j * 2 + 1];
            int x = (int)(inputscale * (double)x1);
            int y = (int)(inputscale * (double)y1);
            pts.push_back(point(x, y));
        }
        boost::polygon::set_points(poly, pts.begin(), pts.end());
        a -= poly;
    }

    //and then load points B
    pts.clear();
    len = Bvec.size() / 2;

    //javascript nfps are referenced with respect to the first point
    double xshift = 0;
    double yshift = 0;

    for (unsigned int i = 0; i < len; i++) {
        double x1 = Bvec[i * 2];
        double y1 = Bvec[i * 2 + 1];

        int x = -(int)(inputscale * (double)x1);
        int y = -(int)(inputscale * (double)y1);
        pts.push_back(point(x, y));

        if (i == 0) {
            xshift = (double)x1;
            yshift = (double)y1;
        }
    }

    boost::polygon::set_points(poly, pts.begin(), pts.end());
    b += poly;

    polys.clear();

    convolve_two_polygon_sets(c, a, b);
    c.get(polys);

    for (unsigned int i = 0; i < polys.size(); ++i) {

        vector<double> pointlist = vector<double>();

        int j = 0;

        for (polygon_traits<polygon>::iterator_type itr = polys[i].begin(); itr != polys[i].end(); ++itr) {

            //  std::cout << (double)(*itr).get(boost::polygon::HORIZONTAL) / inputscale << std::endl;
            double x1 = ((double)(*itr).get(boost::polygon::HORIZONTAL)) / inputscale + xshift;
            double y1 = ((double)(*itr).get(boost::polygon::VERTICAL)) / inputscale + yshift;

            pointlist.push_back(x1);
            pointlist.push_back(y1);

            j++;
        }
        retpnts.push_back(pointlist);
        // holes

        int k = 0;
        for (polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(polys[i]); itrh != end_holes(polys[i]); ++itrh) {
            vector<double> child = vector<double>();


            int z = 0;
            for (polygon_traits<polygon>::iterator_type itr2 = (*itrh).begin(); itr2 != (*itrh).end(); ++itr2) {

                double x1 = ((double)(*itr2).get(boost::polygon::HORIZONTAL)) / inputscale + xshift;
                double y1 = ((double)(*itr2).get(boost::polygon::VERTICAL)) / inputscale + yshift;

                child.push_back(x1);
                child.push_back(y1);

                z++;
            }
            retholes.push_back(child);

            k++;
        }
    }
}