/**
 * @brief 多项式拟合，求解拟合系数。 y=a[0]+a[1]*x^1+a[2]*x^2+...+a[n-1]*x^(n-1)
 * 
 * @param xn    输入：自变量。
 * @param yn    输入：因变量。
 * @param n     输入：采样点数量。
 * @param order 输入：拟合次数，最大次数为 5。
 * @param an    输出：拟合系数。
 * @return int  输出：成功返回 0，失败返回 -1。
 */
int Polyfit(const double* const xn,
            const double* const yn,
            const unsigned int n,
            const unsigned int order,
            double* const an)
{
    // Declarations...
    // ----------------------------------
    enum {maxOrder = 5};

    double B[maxOrder+1] = {0.0};
    double P[((maxOrder+1) * 2)+1] = {0.0};
    double A[(maxOrder + 1)*2*(maxOrder + 1)] = {0.0};

    double x, y, powx;

    unsigned int ii, jj, kk;

    // Verify initial conditions....
    // ----------------------------------

    // This method requires that the n >
    // (order+1)
    if (n <= order)
        return -1;

    // This method has imposed an arbitrary bound of
    // order <= maxOrder.  Increase maxOrder if necessary.
    if (order > maxOrder)
        return -1;

    // Begin Code...
    // ----------------------------------

    // Identify the column vector
    for (ii = 0; ii < n; ii++)
    {
        x    = xn[ii];
        y    = yn[ii];
        powx = 1;

        for (jj = 0; jj < (order + 1); jj++)
        {
            B[jj] = B[jj] + (y * powx);
            powx  = powx * x;
        }
    }

    // Initialize the PowX array
    P[0] = n;

    // Compute the sum of the Powers of X
    for (ii = 0; ii < n; ii++)
    {
        x    = xn[ii];
        powx = xn[ii];

        for (jj = 1; jj < ((2 * (order + 1)) + 1); jj++)
        {
            P[jj] = P[jj] + powx;
            powx  = powx * x;
        }
    }

    // Initialize the reduction matrix
    //
    for (ii = 0; ii < (order + 1); ii++)
    {
        for (jj = 0; jj < (order + 1); jj++)
        {
            A[(ii * (2 * (order + 1))) + jj] = P[ii+jj];
        }

        A[(ii*(2 * (order + 1))) + (ii + (order + 1))] = 1;
    }

    // Move the Identity matrix portion of the redux matrix
    // to the left side (find the inverse of the left side
    // of the redux matrix
    for (ii = 0; ii < (order + 1); ii++)
    {
        x = A[(ii * (2 * (order + 1))) + ii];
        if (x != 0.0)
        {
            for (kk = 0; kk < (2 * (order + 1)); kk++)
            {
                A[(ii * (2 * (order + 1))) + kk] =
                        A[(ii * (2 * (order + 1))) + kk] / x;
            }

            for (jj = 0; jj < (order + 1); jj++)
            {
                if ((jj - ii) != 0)
                {
                    y = A[(jj * (2 * (order + 1))) + ii];
                    for (kk = 0; kk < (2 * (order + 1)); kk++)
                    {
                        A[(jj * (2 * (order + 1))) + kk] =
                                A[(jj * (2 * (order + 1))) + kk] -
                                y * A[(ii * (2 * (order + 1))) + kk];
                    }
                }
            }
        }
        else
        {
            // Cannot work with singular matrices
            return -1;
        }
    }

    // Calculate and Identify the an
    for (ii = 0; ii < (order + 1); ii++)
    {
        for (jj = 0; jj < (order + 1); jj++)
        {
            x = 0;
            for (kk = 0; kk < (order + 1); kk++)
            {
                x = x + (A[(ii * (2 * (order + 1))) + (kk + (order + 1))] *
                        B[kk]);
            }
            an[ii] = x;
        }
    }

    return 0;
}


/** Demo */
static double Xn[] = {0, 0.3000, 0.6000, 0.9000, 1.2000, 1.5000, 1.8000, 2.1000, 2.4000, 2.7000, 3.0000};
static double Yn[] = {2.0000, 2.3780, 3.9440, 7.3460, 13.2320, 22.2500, 35.0480, 52.2740, 74.5760, 102.6020, 137.0000};

/**
 * @brief   求拟合系数及多项式拟合示例。
 * 
 * @return int 
 */
int main(void)
{
    double out = 0.0;
    unsigned int i;
    double coff[3];

    polyfit(Xn, Yn, 11, 2, coff);

    for (i=0; i<3; i++)
    {
        out += coff[i]*pow(Xn[4], i);
    }

    return 0;
}