#include "calc.h"

static NUM_TYPE quadratic_fit_sumx1_arr[CALC_POINTS];
static NUM_TYPE quadratic_fit_sumx2_arr[CALC_POINTS];
static NUM_TYPE quadratic_fit_sumx1;
static NUM_TYPE quadratic_fit_sumx2;
static NUM_TYPE quadratic_fit_sumx3;
static NUM_TYPE quadratic_fit_sumx4;
static NUM_TYPE quadratic_fit_d;
static NUM_TYPE quadratic_fit_matrix[3][3];

void calc_quadratic_fit_init(int length)
{
    for (int i = 0; i < length; i++) {
        quadratic_fit_sumx1_arr[i] = (NUM_TYPE)i;
        quadratic_fit_sumx2_arr[i] = quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx1_arr[i];
    }
    quadratic_fit_sumx1 = (NUM_TYPE)0;
    quadratic_fit_sumx2 = (NUM_TYPE)0;
    quadratic_fit_sumx3 = (NUM_TYPE)0;
    quadratic_fit_sumx4 = (NUM_TYPE)0;
    for (int i = 0; i < length; i++) {
        quadratic_fit_sumx1 += quadratic_fit_sumx1_arr[i];
        quadratic_fit_sumx2 += quadratic_fit_sumx2_arr[i];
        quadratic_fit_sumx3 += quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx2_arr[i];
        quadratic_fit_sumx4 += quadratic_fit_sumx2_arr[i] * quadratic_fit_sumx2_arr[i];
    }
    quadratic_fit_d = \
        quadratic_fit_sumx2 * quadratic_fit_sumx2 * quadratic_fit_sumx2 + \
        quadratic_fit_sumx1 * quadratic_fit_sumx1 * quadratic_fit_sumx4 + \
        (NUM_TYPE)length * quadratic_fit_sumx3 * quadratic_fit_sumx3 - \
        (NUM_TYPE)length * quadratic_fit_sumx2 * quadratic_fit_sumx4 - \
        (NUM_TYPE)2 * quadratic_fit_sumx1 * quadratic_fit_sumx2 * quadratic_fit_sumx3;
    quadratic_fit_matrix[0][0] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - quadratic_fit_sumx1 * quadratic_fit_sumx3;
    quadratic_fit_matrix[0][1] = (NUM_TYPE)length * quadratic_fit_sumx3 - quadratic_fit_sumx1 * quadratic_fit_sumx2;
    quadratic_fit_matrix[0][2] = quadratic_fit_sumx1 * quadratic_fit_sumx1 - (NUM_TYPE)length * quadratic_fit_sumx2;
    quadratic_fit_matrix[1][0] = quadratic_fit_sumx1 * quadratic_fit_sumx4 - quadratic_fit_sumx2 * quadratic_fit_sumx3;
    quadratic_fit_matrix[1][1] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - (NUM_TYPE)length * quadratic_fit_sumx4;
    quadratic_fit_matrix[1][2] = quadratic_fit_matrix[0][1];
    quadratic_fit_matrix[2][0] = quadratic_fit_sumx3 * quadratic_fit_sumx3 - quadratic_fit_sumx2 * quadratic_fit_sumx4;
    quadratic_fit_matrix[2][1] = quadratic_fit_matrix[1][0];
    quadratic_fit_matrix[2][2] = quadratic_fit_matrix[0][0];
}

void calc_quadratic_fit(NUM_TYPE *buff, struct quadratic_coefficient_s *coeff, int length)
{
    NUM_TYPE sum_y1, sum_xy, sum_x2y;

    if ((buff == (void *)0) || (coeff == (void *)0) || (length == 0)) {
        return;
    }
    sum_y1 = (NUM_TYPE)0;
    sum_xy = (NUM_TYPE)0;
    sum_x2y = (NUM_TYPE)0;
    for (int i = 0; i < length; i++) {
        sum_y1 += buff[i];
        sum_xy += quadratic_fit_sumx1_arr[i] * buff[i];
        sum_x2y += quadratic_fit_sumx2_arr[i] * buff[i];
    }
   coeff->a = (sum_y1 * quadratic_fit_matrix[0][0] + sum_xy * quadratic_fit_matrix[0][1] + sum_x2y * quadratic_fit_matrix[0][2]) / quadratic_fit_d;
   coeff->b = (sum_y1 * quadratic_fit_matrix[1][0] + sum_xy * quadratic_fit_matrix[1][1] + sum_x2y * quadratic_fit_matrix[1][2]) / quadratic_fit_d;
   coeff->c = (sum_y1 * quadratic_fit_matrix[2][0] + sum_xy * quadratic_fit_matrix[2][1] + sum_x2y * quadratic_fit_matrix[2][2]) / quadratic_fit_d;
}