//======================================================================
// Driver program: Calculates first and second derivatives from
// tabulated data using local four-point Lagrange interpolation.
//
// The data points may be unequally spaced. Near either endpoint, the
// four-point stencil is shifted automatically so that it remains inside
// the tabulated interval.
//
// Based on deriv_Lg.f90 by Alexander Godunov (October 2009).
// Revised for the companion website (September 2026).
//======================================================================

#include <algorithm>
#include <array>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

constexpr int stencil_size = 4;

int derivative_from_data(double x_eval,
                         const std::vector<double>& x_data,
                         const std::vector<double>& y_data,
                         int derivative_order,
                         double& derivative);

int select_stencil(double x_eval, const std::vector<double>& x_data);

double lagrange_derivative(double x_eval,
                           const std::vector<double>& x,
                           const std::vector<double>& y,
                           int derivative_order);


int main()
{
    constexpr int number_of_data = 31;
    constexpr std::array<double, 7> x_test = {
        0.00, 0.20, 0.70, 1.25, 1.95, 2.75, 3.00
    };

    std::vector<double> x_data(number_of_data);
    std::vector<double> y_data(number_of_data);

    //------------------------------------------------------------------
    // Step 1: Generate sample tabulated data.
    //
    // The example uses f(x)=exp(x) on a mildly nonuniform grid. Both
    // exact derivatives equal exp(x), so the results are easy to check.
    //
    // For measured or previously calculated data, replace this loop
    // with assignments to x_data and y_data or with file-input code.
    //------------------------------------------------------------------
    for (int i = 0; i < number_of_data; ++i) {
        const double fraction =
            static_cast<double>(i)/static_cast<double>(number_of_data - 1);

        x_data[i] = 3.0*std::pow(fraction, 1.15);
        y_data[i] = std::exp(x_data[i]);
    }

    //------------------------------------------------------------------
    // Step 2: Calculate the derivatives at the requested points.
    //------------------------------------------------------------------
    std::cout << "Numerical derivatives from tabulated data\n"
              << "Local four-point Lagrange interpolation\n\n"
              << "        x      exact f'          numerical f'"
              << "      exact f''         numerical f''\n";

    for (double x_eval : x_test) {
        double first_derivative = 0.0;
        double second_derivative = 0.0;

        const int status_first =
            derivative_from_data(x_eval, x_data, y_data, 1,
                                 first_derivative);

        const int status_second =
            derivative_from_data(x_eval, x_data, y_data, 2,
                                 second_derivative);

        if (status_first != 0 || status_second != 0) {
            std::cout << "Derivative could not be evaluated at x = "
                      << x_eval << "; status = "
                      << status_first << ' ' << status_second << '\n';
            continue;
        }

        const double exact = std::exp(x_eval);

        std::cout << std::fixed << std::setprecision(3)
                  << std::setw(9) << x_eval
                  << std::scientific << std::setprecision(8)
                  << std::setw(18) << exact
                  << std::setw(18) << first_derivative
                  << std::setw(18) << exact
                  << std::setw(18) << second_derivative << '\n';
    }

    return 0;
}


int derivative_from_data(double x_eval,
                         const std::vector<double>& x_data,
                         const std::vector<double>& y_data,
                         int derivative_order,
                         double& derivative)
{
    //------------------------------------------------------------------
    // Estimate a first or second derivative from tabulated data.
    //
    // A cubic Lagrange polynomial is constructed from four consecutive
    // data points near x_eval. Differentiating this polynomial gives the
    // requested derivative.
    //
    // Input
    //   x_eval            point where the derivative is required
    //   x_data            strictly increasing data abscissas
    //   y_data            tabulated function values
    //   derivative_order  1 for the first derivative, 2 for the second
    //
    // Output
    //   derivative        numerical value of the requested derivative
    //
    // Return value
    //   0: success
    //   1: x_data and y_data have different sizes
    //   2: fewer than four data points were supplied
    //   3: derivative_order is not 1 or 2
    //   4: x_data is not strictly increasing
    //   5: x_eval is outside the tabulated interval
    //------------------------------------------------------------------
    derivative = 0.0;
    const int number_of_points = static_cast<int>(x_data.size());

    // Check the input before selecting the interpolation stencil.
    if (y_data.size() != x_data.size()) {
        return 1;
    }

    if (number_of_points < stencil_size) {
        return 2;
    }

    if (derivative_order < 1 || derivative_order > 2) {
        return 3;
    }

    for (int i = 0; i < number_of_points - 1; ++i) {
        if (x_data[i + 1] <= x_data[i]) {
            return 4;
        }
    }

    if (x_eval < x_data.front() || x_eval > x_data.back()) {
        return 5;
    }

    // Select four nearby points. At a boundary, the stencil is shifted
    // automatically so that all four points remain in the data range.
    const int first = select_stencil(x_eval, x_data);

    std::vector<double> x_local(stencil_size);
    std::vector<double> y_local(stencil_size);

    for (int i = 0; i < stencil_size; ++i) {
        x_local[i] = x_data[first + i];
        y_local[i] = y_data[first + i];
    }

    derivative = lagrange_derivative(x_eval, x_local, y_local,
                                     derivative_order);
    return 0;
}


int select_stencil(double x_eval, const std::vector<double>& x_data)
{
    //------------------------------------------------------------------
    // Locate the first point of the local four-point stencil.
    //
    // The binary search finds left such that
    //
    //       x_data[left] <= x_eval < x_data[left+1].
    //
    // The stencil is centered on this interval as closely as possible.
    //------------------------------------------------------------------
    const int number_of_points = static_cast<int>(x_data.size());
    int left = 0;
    int right = number_of_points - 1;

    while (right > left + 1) {
        const int middle = (left + right)/2;

        if (x_eval < x_data[middle]) {
            right = middle;
        } else {
            left = middle;
        }
    }

    int first = left - (stencil_size - 2)/2;
    first = std::max(0, std::min(first,
                                number_of_points - stencil_size));
    return first;
}


double lagrange_derivative(double x_eval,
                           const std::vector<double>& x,
                           const std::vector<double>& y,
                           int derivative_order)
{
    //------------------------------------------------------------------
    // Differentiate the local Lagrange interpolating polynomial.
    //
    // For the local data (x_j,y_j),
    //
    //       P(x) = sum_j y_j L_j(x),
    //
    // where
    //
    //       L_j(x) = product_{k != j} (x-x_k)/(x_j-x_k).
    //
    // The loops evaluate the first or second derivative of each basis
    // polynomial L_j directly. No constant grid spacing is assumed.
    //------------------------------------------------------------------
    const int number_of_points = static_cast<int>(x.size());
    double value = 0.0;

    for (int j = 0; j < number_of_points; ++j) {
        // Denominator of the j-th Lagrange basis polynomial.
        double denominator = 1.0;
        for (int k = 0; k < number_of_points; ++k) {
            if (k != j) {
                denominator *= x[j] - x[k];
            }
        }

        if (derivative_order == 1) {
            // In each term of L'_j(x), one factor is differentiated.
            for (int k = 0; k < number_of_points; ++k) {
                if (k == j) {
                    continue;
                }

                double term = 1.0;
                for (int i = 0; i < number_of_points; ++i) {
                    if (i != j && i != k) {
                        term *= x_eval - x[i];
                    }
                }
                value += y[j]*term/denominator;
            }

        } else {
            // In each term of L''_j(x), two factors are differentiated.
            for (int k = 0; k < number_of_points; ++k) {
                if (k == j) {
                    continue;
                }

                for (int l = 0; l < number_of_points; ++l) {
                    if (l == j || l == k) {
                        continue;
                    }

                    double term = 1.0;
                    for (int i = 0; i < number_of_points; ++i) {
                        if (i != j && i != k && i != l) {
                            term *= x_eval - x[i];
                        }
                    }
                    value += y[j]*term/denominator;
                }
            }
        }
    }

    return value;
}