#include <stdlib.h>
// #include "debug.h"
#define DIM 2

void copyCoordinate(double* from, double* to) {
    for (int i = 0; i < DIM; i++) {
        to[i] = from[i];
    }
}

// extends array and copies point on the end. Returns new length
int push(double* array, int length, double* point) {
    int newLength = length + DIM;
    // realloc(array, newLength * sizeof(double));
    copyCoordinate(point, array + length);
    return newLength;
}

// square distance between 2 points
double getSqDist(double* p1, double* p2) {
    double dx = p1[0] - p2[0],
        dy = p1[1] - p2[1];
    return dx * dx + dy * dy;
}


// square distance from a point to a segment
double getSqSegDist(double* p, double* p1, double* p2) {
    double x = p1[0],
           y = p1[1],
           dx = p2[0] - x,
        dy = p2[1] - y;

    if (dx != 0 || dy != 0) {
        double t = ((p[0] - x) * dx + (p[1] - y) * dy) / (dx * dx + dy * dy);

        if (t > 1) {
            x = p2[0];
            y = p2[1];

        } else if (t > 0) {
            x += dx * t;
            y += dy * t;
        }
    }

    dx = p[0] - x;
    dy = p[1] - y;

    return dx * dx + dy * dy;
}

// basic distance-based simplification
int simplifyRadialDist(double* points, int length, double sqTolerance, double* result) {
    double* prevPoint = points;
    double* point;
    int resultLength = push(result, 0, points);

    for (int i = DIM; i < length; i += DIM) {
        point = points + i;
        if (getSqDist(point, prevPoint) > sqTolerance) {
            resultLength = push(result, resultLength, point);
            prevPoint = point;
        }
    }

    if (prevPoint != point) {
        resultLength = push(result, resultLength, point);
    }

    return resultLength;
}

int simplifyDPStep(double* points, int first, int last, double sqTolerance, double* simplified, int lengthOfSimplified) {
    double maxSqDist = sqTolerance;
    int index;

    for (int i = first + DIM; i < last; i += DIM) {
        double sqDist = getSqSegDist(points + i, points + first, points + last);

        if (sqDist > maxSqDist) {
            index = i;
            maxSqDist = sqDist;
        }
    }

    if (maxSqDist > sqTolerance) {
        if (index - first > DIM) lengthOfSimplified = simplifyDPStep(points, first, index, sqTolerance, simplified, lengthOfSimplified);
        lengthOfSimplified = push(simplified, lengthOfSimplified, points + index);
        if (last - index > DIM) lengthOfSimplified = simplifyDPStep(points, index, last, sqTolerance, simplified, lengthOfSimplified);
    }
    return lengthOfSimplified;
}

// simplification using Ramer-Douglas-Peucker algorithm
int simplifyDouglasPeucker(double* points, int length, double sqTolerance, double* simplified) {
    int lengthOfSimplified;
    lengthOfSimplified = push(simplified, 0, points);
    lengthOfSimplified = simplifyDPStep(points, 0, length - DIM, sqTolerance, simplified, lengthOfSimplified);
    lengthOfSimplified = push(simplified, lengthOfSimplified, points + length - DIM);
    return lengthOfSimplified;
}

int* simplify(double * points, int length, double tolerance, int highestQuality) {
    double sqTolerance = tolerance * tolerance;
    double* resultRdDistance = NULL;
    double* result = NULL;
    int resultLength;

    if (!highestQuality) {
        resultRdDistance = malloc(length * sizeof(double));
        length = simplifyRadialDist(points, length, sqTolerance, resultRdDistance);
        points = resultRdDistance;
    }

    result = malloc(length * sizeof(double));
    resultLength = simplifyDouglasPeucker(points, length, sqTolerance, result);
    free(resultRdDistance);

    int* resultInfo = malloc(2);
    resultInfo[0] = (int) result;
    resultInfo[1] = resultLength;
    return resultInfo;
}