#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <cmath>
#include <omp.h> // Include only if using OpenMP

#define LD_CONFIDENCE_THRESH 0.5
#define MIN_KPS_PER_LANE_LINE 3
#define N_POINTS 1000000

typedef struct {
    double x;
    double y;
    double score;
} key_point_t;

/* Original quadratic regression function */
void quadratic_regression(const key_point_t *__restrict__ points, int n, double *__restrict__ a, double *__restrict__ b, double *__restrict__ c) {
    double sum_x = 0.0, sum_x2 = 0.0, sum_x3 = 0.0, sum_x4 = 0.0;
    double sum_y = 0.0, sum_xy = 0.0, sum_x2y = 0.0;
    int valid_points = 0;
    for (int i = 0; i < n; i++) {
        /* Check if the point is valid */
        if ((points[i].x != 0 || points[i].y != 0) && (points[i].score > LD_CONFIDENCE_THRESH)) {
            double x = points[i].x;
            double y = points[i].y;
            /* Calculate powers of x on the fly (may do redundant multiplications) */
            sum_x  += x;
            sum_x2 += x * x;
            sum_x3 += x * x * x;
            sum_x4 += x * x * x * x;
            sum_y  += y;
            sum_xy += x * y;
            sum_x2y += (x * x) * y;
            valid_points++;
        }
    }
    if (valid_points <= MIN_KPS_PER_LANE_LINE)
        return;
    double determinant = (valid_points * (sum_x2 * sum_x4 - sum_x3 * sum_x3)) -
                         (sum_x * (sum_x * sum_x4 - sum_x2 * sum_x3)) +
                         (sum_x2 * (sum_x * sum_x3 - sum_x2 * sum_x2));
    if (determinant == 0)
        return;
    *a = ((valid_points * (sum_x2 * sum_x2y - sum_x3 * sum_xy)) -
          (sum_x * (sum_x * sum_x2y - sum_xy * sum_x2)) +
          (sum_y * (sum_x * sum_x3 - sum_x2 * sum_x2))) / determinant;
    *b = ((valid_points * (sum_xy * sum_x4 - sum_x2y * sum_x3)) -
          (sum_y * (sum_x * sum_x4 - sum_x2 * sum_x3)) +
          (sum_x2 * (sum_x * sum_x2y - sum_xy * sum_x2))) / determinant;
    *c = ((sum_y * (sum_x2 * sum_x4 - sum_x3 * sum_x3)) -
          (sum_x * (sum_xy * sum_x4 - sum_x2y * sum_x3)) +
          (sum_x2 * (sum_xy * sum_x3 - sum_x2y * sum_x2))) / determinant;
}

/* Fill an array of key_point_t with random data.
   We generate x and y values between -50 and 50, and a score between 0.5 and 1.0 to ensure the point is valid. */
void fill_points(key_point_t *points, int n) {
    for (int i = 0; i < n; i++) {
        points[i].x = ((double)rand() / RAND_MAX) * 100.0 - 50.0;
        points[i].y = ((double)rand() / RAND_MAX) * 100.0 - 50.0;
        points[i].score = ((double)rand() / RAND_MAX) * 0.5 + 0.5;
    }
}

int main() {
    key_point_t *points = (key_point_t *)malloc(N_POINTS * sizeof(key_point_t));
    if (!points) {
        fprintf(stderr, "Memory allocation failed\n");
        return 1;
    }
    srand((unsigned int)time(NULL));
    fill_points(points, N_POINTS);

    double a1 = 0, b1 = 0, c1 = 0;
    double a2 = 0, b2 = 0, c2 = 0;
    clock_t start, end;
    double time_original;

    /* Time the original algorithm */
    start = clock();
    quadratic_regression(points, N_POINTS, &a1, &b1, &c1);
    end = clock();
    time_original = ((double)(end - start)) / CLOCKS_PER_SEC;


    printf("Original: a = %.6f, b = %.6f, c = %.6f, time = %f seconds\n", a1, b1, c1, time_original);

    free(points);
    return 0;
}