#include <Arduino.h>

void polyfit(float x[], float y[], int num_points, int degree, float coefficients[]) {
  // Check for invalid degree or insufficient data points
  if (degree >= num_points || degree < 1) {
    Serial.println("Invalid degree or insufficient data points.");
    return;
  }

  int matrix_size = degree + 1;

  // Initialize matrices
  float matrix[matrix_size][matrix_size];
  float right_side[matrix_size];

  // Populate matrices
  for (int i = 0; i < matrix_size; i++) {
    right_side[i] = 0;
    for (int j = 0; j < matrix_size; j++) {
      matrix[i][j] = 0;
      for (int k = 0; k < num_points; k++) {
        matrix[i][j] += pow(x[k], i + j);
      }
    }
  }

  for (int i = 0; i < matrix_size; i++) {
    for (int k = 0; k < num_points; k++) {
      right_side[i] += pow(x[k], i) * y[k];
    }
  }

  // Gaussian elimination (simplified)
  for (int i = 0; i < matrix_size; i++) {
    for (int j = i + 1; j < matrix_size; j++) {
      float factor = matrix[j][i] / matrix[i][i];
      for (int k = 0; k < matrix_size; k++) {
        matrix[j][k] -= factor * matrix[i][k];
      }
      right_side[j] -= factor * right_side[i];
    }
  }

  // Back substitution
  for (int i = matrix_size - 1; i >= 0; i--) {
    coefficients[i] = right_side[i] / matrix[i][i];
    for (int j = i - 1; j >= 0; j--) {
      right_side[j] -= matrix[j][i] * coefficients[i];
    }
  }
}

float predictLoad(float adc_value, float coefficients[], int degree) {
  if (degree < 0) {
    Serial.println("Invalid degree.");
    return 0;
  }

  float load_prediction = 0;
  for (int i = 0; i <= degree; i++) {
    load_prediction += coefficients[i] * pow(adc_value, i);
  }

  return load_prediction;
}

void setup() {
  Serial.begin(9600);

  // Given ADC and load values
  float adc_values[] = {24452, 44879, 65347, 85053,104051};
  float load_values[] = {0, 400, 800, 1200, 1600};

  // Number of data points
  int num_points = sizeof(adc_values) / sizeof(adc_values[0]);

  // Degree of the polynomial (you can adjust this)
  int polynomial_degree = 2;

  // Coefficients array to store the results
  float coefficients[polynomial_degree + 1];

  // Perform polynomial regression
  polyfit(adc_values, load_values, num_points, polynomial_degree, coefficients);

  // New ADC value for prediction
  float new_adc_value = 113568;

  // Use the predictLoad function to get the predicted load
  float predicted_load = predictLoad(new_adc_value, coefficients, polynomial_degree);

  // Print the results
  Serial.print("Predicted Load for ADC ");
  Serial.print(new_adc_value);
  Serial.print(": ");
  Serial.println(predicted_load);
}

void loop() {
  // Nothing to do here in this example
}