#include "ConvexHull.h"
#include "DualCoding/PolygonData.h"

Shape<PolygonData> MyPolygonData::ConvexHull(std::vector<Point> pts, ShapeSpace &ShS, const fmat::Transform &tmatinv) {
    int const npts = pts.size();
    std::vector<convexHullPoint> points(npts);
    points.clear();
    int pivot_x = -1, pivot_y = 100000;
    for (int i=0;i<npts;i++) {
        //    cout << sketch[i] << " : " << neighbs[i] << endl;
        int const x = pts[i].coordX();
        int const y = pts[i].coordY();
        points.push_back(convexHullPoint(x,y,0));
        if ( y < pivot_y || (y == pivot_y && x > pivot_x) ) {
            pivot_x = x;
            pivot_y = y;
        }
    }

    // sort points by angle from the pivot, and if colinear, take closer point first
    for (int i=0; i<npts; i++)
        points[i].angle  = (float) -atan2((float) (pivot_y - points[i].y), (float) (pivot_x - points[i].x));
    std::sort(points.begin(),points.end(),convexHullPoint::pointCompare(convexHullPoint(pivot_x,pivot_y,0)));

    // push points onto a stack to form the convex hull
    vector<convexHullPoint> hull(npts);
    hull.clear();
    hull.push_back(points[0]);
    hull.push_back(points[1]);
    for ( int i=2; i<npts; i++ ) {
        int last = hull.size() - 1;
        int o = convexHullPoint::crossProduct(hull[last-1],hull[last],points[i]);
        if ( o == 0 )
            hull[last] = points[i];
        else if ( o < 0 )
            hull.push_back(points[i]);
        else {
            while ( o >= 0 && hull.size() > 2 ) {
                hull.pop_back();
                last--;
                o = convexHullPoint::crossProduct(hull[last-1],hull[last],points[i]);
            }
            hull.push_back(points[i]);
        }
    }
    int const nhull = hull.size();
    // build a polygon to represent the hull
    vector<Point> vertices;
    const ReferenceFrameType_t reftype = ShS.getRefFrameType();
    for (int i=0; i<nhull; i++) {
        fmat::Column<3> v = tmatinv * fmat::pack<float>(hull[i].x, hull[i].y,  0);
        vertices.push_back(Point(v, reftype));
    }
    vertices.push_back(vertices.front());  // force closed polygon: don't trust tryClose()
    return Shape<PolygonData>(new PolygonData(ShS,vertices,true));
}