/*****************************************************************
 * Copyright (c) 2020 by Douglas Wilhelm Harder.
 * All rights reserved.
 *
 * To use, please contact dwharder@uwaterloo.ca
 *****************************************************************/

#pragma once

#include <utility>
#include <cassert>
#include <cmath>

// Function declarations
template <typename T>
void log( T numer, T denom, std::size_t k, bool forwards );

// Function definitions
template <typename T>
void log( T numer, T denom, std::size_t k, bool forwards ) {
    if ( k == 0 ) {
        std::clog << numer << ".0/" << denom << ".0*y[n]";
    } else {
        std::string pt;

        if ( forwards ) {
            pt = ".0*y[n+";
        } else {
            pt = ".0*y[n-";
        }

        if ( numer < 0.0 ) {
            std::clog << " - " << -numer << ".0/" << denom << pt << k << "]";
        } else if ( numer > 0.0 ) {
            std::clog << " + " <<  numer << ".0/" << denom << pt << k << "]";
        }
    }
}

/*
 * Given N points, y     , ..., y   , y , find the
 *                  n-N+1        n-1   n
 *
 *                   ^
 * best estimator of y  by finding the least-squares linear
 *                    n
 * polynomial of the points and evaluating that polynomial
 * at n.
 *   - It is assumed that the y-values are
 *     sampled periodically in time
 *
 *                                             y[n] *
 *     y[n-N+1] *                       *           x <----
 *                          *     *            
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *            n-N+1                                 n 
 *
 * If 'forwards' is true, then we assume the reverse order:
 *
 *              * y[n]
 *        ----> x           *                       * y[n+N-1]
 *                                *     *      
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *              n                                 n+N-1
 *
 */

template <typename T>
T lin_y0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ N*(N + 1.0)/2.0 };

    T result{ 0.0 };

    T coeff{ 2.0*N - 1.0 };

    std::clog << "// Linear polynomial approximating y[n]" << std::endl
              << "   y_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
            coeff  += -3.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff  += -3.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

/*
 * Given N points, y     , ..., y   , y , find the
 *                  n-N+1        n-1   n
 *
 *                   ^
 * best estimator of y    by finding the least-squares linear
 *                    n+1
 * polynomial of the points and evaluating that polynomial
 * at n + 1.
 *   - It is assumed that the y-values are
 *     sampled periodically in time
 *
 *                                             y[n] *
 *     y[n-N+1] *                       *                 x <----
 *                          *     *            
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *            n-N+1                                 n    n+1
 *
 * If 'forwards' is true, then we assume the reverse order:
 *
 *              * y[n]
 *  ----> x                 *                       * y[n+N-1]
 *                                *     *      
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *       n-1    n                                 n+N-1
 *
 */

template <typename T>
T lin_y1( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N - 1.0)*N/2.0 };

    T result{ 0.0 };

    T coeff{ 2.0*N - 2.0 };

    std::clog << "// Linear polynomial approximating y[n + 1]" << std::endl
              << "   y_n_1 = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
            coeff  += -3.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff  += -3.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

/*
 * Given N points, y     , ..., y   , y , find the
 *                  n-N+1        n-1   n
 *
 * best estimator of the change per period by finding
 * the least-squares linear polynomial of the points and returning
 * the slope of that line.
 *   - It is assumed that the y-values are
 *     sampled periodically in time
 *
 *                                             y[n] *
 *     y[n-N+1] *           ____________*------------
 *              ------------*     *            
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *            n-N+1                                 n 
 *
 * If 'forwards' is true, then we assume the reverse order:
 *
 *              * y[n]
 *                          *                       * y[n+N-1]
 *                                *     *      
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *              n                                 n+N-1
 *
 */

template <typename T>
T lin_dy0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N - 1.0)*N*(N + 1.0)/6.0 };

    T result{ 0.0 };

    T coeff{ N - 1.0 };

    std::clog << "// Linear polynomial approximating the change per period at y[n]" << std::endl
              << "   dy_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
            coeff  += -2.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff  += -2.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

/*
 * Given N points, y     , ..., y   , y , find the
 *                  n-N+1        n-1   n
 *
 * best estimator of the average value over the last period unit by
 * finding the least-squares linear polynomial of the points
 * integrating that polynomial over the last time interval.
 *   - It is assumed that the y-values are
 *     sampled periodically in time
 *
 *                                             y[n] *
 *     y[n-N+1] *           ____________*------------
 *              ------------*     *           |     |
 *                    *                       *     |
 *                                            |     |
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *            n-N+1                                 n 
 *
 * If 'forwards' is true, then we assume the reverse order:
 *
 *              * y[n]
 *              |     |     *                       * y[n+N-1]
 *              |     |           *     *      
 *              |     *                       *
 *              |     |
 *      --|-----|-----|-----|-----|-----|-----|-----|-----|--
 *              n                                 n+N-1
 *
 */

template <typename T>
T lin_iy0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N - 1.0)*N*(N + 1.0) };

    T result{ 0.0 };

    T  coeff{ (N - 1.0)*(4.0*N - 5.0) };
    T dcoeff{ -6.0*N + 12.0 };

    std::clog << "// Linear polynomial approximating the average value over the last period" << std::endl
              << "   average_y_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
            coeff += dcoeff;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff += dcoeff;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

/*
 * Given N points, y     , ..., y   , y , assume the last reading
 *                  n-N+1        n-1   n
 *
 * was taken at time t  + epsilon /\t where /\t is the period .
 *                    n           --        --  ~
 * In this case, we find the best approximation y  of the reading y
 *                                               n                 n
 * that would return the same estimator of the current value if
 *                               ^
 * we were to use the point (t , y ) instead of (t + epsilon /\t, y )
 *                            n   n               n          --    n
 * 
 * The technique used is only a first-order approximation in epsilon
 * for the sake of efficiency so that the jitter may be corrected with
 * one linear combination of the values.         ^
 *                                               y[n]
 *                                                  x  * y[n]
 *     y[n-N+1] *           ____________*------------
 *              ------------*     *            
 *                    *                       *
 *
 *      --|-----|-----|-----|-----|-----|-----|-----|--|--|--
 *            n-N+1                                 n 
 *
 * If 'forwards' is true, then we assume the reverse order:
 *               ^
 *               y[n]
 *      y[n] *  x
 *              ------------*____________           * y[n+N-1]
 *                                *     *------------     
 *                    *                       *
 *
 *      --|--|--|-----|-----|-----|-----|-----|-----|-----|--
 *              n                                 n+N-1
 *
 */

template <typename T>
T lin_jit( T *y, std::size_t n, unsigned int N, T epsilon, bool forwards ) {
    T denominator{ (N - 1.0)*N*(N + 1.0)*(2.0*N - 1.0)/2.0 };

    T offset{ 0.0 };
    T coeff{ 1.5*(N - 1.0)*((N - 7.0)*N + 4.0)};

    std::clog << "// Linear correction for jitter" << std::endl
              << "   y[n] += epsilon*(";

    offset += coeff*y[n];
    log( coeff, denominator, 0, forwards );

    coeff += (-1.5*N*N + 22.5)*N - 15.0;
    T dcoeff{ 21.0*N - 15.0 };

    if ( forwards ) {
        for ( std::size_t k{1}; k < N; ++k ) {
            offset += coeff*y[n + k];
            log( coeff, denominator, k, forwards );

            coeff += dcoeff;
        }
    } else {
        for ( std::size_t k{1}; k < N; ++k ) {
            offset += coeff*y[n - k];
            log( coeff, denominator, k, forwards );

            coeff += dcoeff;
        }
    }

    std::clog << ");" << std::endl << std::endl;

    return y[n] + offset/denominator*epsilon;
}

template <typename T>
T lin_root( T *y, std::size_t n, unsigned int N, bool forwards ) {
    return -lin_y0( y, n, N, forwards )/lin_dy0( y, n, N, forwards );
}

template <typename T>
T quad_y0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ N*(N + 1.0)*(N + 2.0)/6.0 };

    T result{ 0.0 };

    T  coeff{ 3.0*(N - 1.0)*N/2.0 + 1.0 };
    T dcoeff{ -6.0*N + 8.0 };

    std::clog << "// Quadratic polynomial approximating y[n]" << std::endl
              << "   y[n] = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
             coeff += dcoeff;
            dcoeff += 10.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
             coeff += dcoeff;
            dcoeff += 10.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

template <typename T>
T quad_y1( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N - 2.0)*(N - 1.0)*N/6.0 };

    T result{ 0.0 };

    T  coeff{ 3.0*(N - 1.0)*(N - 2.0)/2.0 };
    T dcoeff{ -6.0*N + 12.0 };

    std::clog << "// Quadratic polynomial approximating y[n+1]" << std::endl
              << "   y_n_1 = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
             coeff += dcoeff;
            dcoeff += 10.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff  += dcoeff;
            dcoeff  += 10.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

template <typename T>
T quad_dy0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N - 2.0)*(N - 1.0)*N*(N + 1.0)*(N + 2.0)/12.0 };

    T result{ 0.0 };

    T   coeff{ 3.0*(2.0*N - 1.0)*(N - 2.0)*(N - 1.0)/2.0 };
    T  dcoeff{ -(16.0*(N - 1.0) + 3.0)*((N - 1.0) - 1.0) };
    T ddcoeff{ 30.0*(N - 1.0) };

    std::clog << "// Quadratic polynomial approximating the change per period at y[n]" << std::endl
              << "   dy_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
             coeff +=  dcoeff;
            dcoeff += ddcoeff;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
             coeff +=  dcoeff;
            dcoeff += ddcoeff;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

template <typename T>
T quad_ddy0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ (N + 2.0)*(N - 2.0)*N*(N - 1.0)*(N + 1.0)/120.0 };

    T result{ 0.0 };

    T  coeff{ (N - 2.0)*(N - 1.0)/2.0 };
    T dcoeff{ -3.0*(N - 2.0) };

    std::clog << "// Quadratic polynomial approximating the concavity at y[n]" << std::endl
              << "   ddy_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
            coeff  += dcoeff;
            dcoeff  += 6.0;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
            coeff  += dcoeff;
            dcoeff  += 6.0;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

template <typename T>
T quad_iy0( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T denominator{ ((N - 1.0) - 1.0)*(N - 1.0)*((N - 1.0) + 1.0)*((N - 1.0) + 2.0)*((N - 1.0) + 3.0)/2.0 };

    T result{ 0.0 };

    T   coeff{ (N - 1.0)*(N - 2.0)*(9.0*(N - 1.0)*(N - 1.0) - 9.0*(N - 1.0) + 7.0)/2.0 };
    T  dcoeff{ -3.0*((N - 1.0) - 1.0)*(6*(N - 1.0)*(N - 1.0) - 18.0*(N - 1.0) + 7.0) };
    T ddcoeff{ 30.0*((N - 1.0)*(N - 1.0) - 4.0*(N - 1.0) + 2.0) };

    std::clog << "// Quadratic polynomial approximating the average value over the last period" << std::endl
              << "   average_y_n = ";

    if ( forwards ) {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n + k];
            log( coeff, denominator, k, forwards );
             coeff +=  dcoeff;
            dcoeff += ddcoeff;
        }
    } else {
        for ( std::size_t k{0}; k < N; ++k ) {
            result += coeff*y[n - k];
            log( coeff, denominator, k, forwards );
             coeff +=  dcoeff;
            dcoeff += ddcoeff;
        }
    }

    std::clog << ";" << std::endl << std::endl;

    return result/denominator;
}

template <typename T>
T quad_jit( T *y, std::size_t n, unsigned int N, T epsilon, bool forwards ) {
    T denominator{ (N - 2.0)*(N - 1.0)*N*(N + 1.0)*(N + 2.0)
                            *(1.5*(N - 1.0)*N + 1.0)/12.0 };

    T  offset{ 0.0 };
    T   coeff{ (N - 1.0)*(N - 2.0)*(2.0*N - 1.0)
                        *(((N - 15.0)*N + 20.0)*N - 12.0)/4.0 };
    T  dcoeff{ -(N - 2.0)*((
          (((2.0*N + 3.0)*N - 172.0)*N + 375.0)*N - 320.0
       )*N + 100.0)/4.0 };

    std::clog << "// Quadratic correction for jitter" << std::endl
              << "   y[n] += epsilon*(";

    offset += coeff*y[n];
    log( coeff, denominator, 0, forwards );
      coeff +=  dcoeff;
     dcoeff += (N - 1.0)*((
         (((2.0*N + 1.0)*N - 9.0)*N - 304.0)*N + 484.0
     )*N - 240.0)/4.0;

    T ddcoeff{ -15.0*(N - 1.0)*((5.0*N - 8.0)*N + 4.0) };

    if ( forwards ) {
        for ( std::size_t k{1}; k < N; ++k ) {
            offset += coeff*y[n + k];
            log( coeff, denominator, k, forwards );

              coeff +=  dcoeff;
             dcoeff += ddcoeff;
        }
    } else {
        for ( std::size_t k{1}; k < N; ++k ) {
            offset += coeff*y[n - k];
            log( coeff, denominator, k, forwards );

              coeff +=  dcoeff;
             dcoeff += ddcoeff;
        }
    }

    std::clog << ");" << std::endl << std::endl;

    return y[n] + offset/denominator*epsilon;
}

template <typename T>
T quad_ext( T *y, std::size_t n, unsigned int N, bool forwards ) {
    return -quad_dy0( y, n, N, forwards ) / 
                 quad_ddy0( y, n, N, forwards );
}

template <typename T>
std::pair<T, T> quad_roots( T *y, std::size_t n, unsigned int N, bool forwards ) {
    T a2{ quad_ddy0( y, n, N, forwards ) };
    T  b{  quad_dy0( y, n, N, forwards ) };
    T  c{   quad_y0( y, n, N, forwards ) };

    if ( a2 == 0 ) {
        return std::make_pair( std::nan( "" ), -c/b );
    }

    T disc{ b*b - 2.0*a2*c };

    if ( (disc < 0.0) || std::isnan( disc ) ) {
        return std::make_pair( std::nan( "" ), std::nan( "" ) );
    } else if ( disc == 0.0 ) {
        return std::make_pair( -b/a2, -b/a2 );
    } else {
        disc = std::sqrt( disc );

        if ( b == 0.0 ) {
            return std::make_pair( disc/a2, disc/a2 );
        } else if ( std::isnan( b ) ) {
            return std::make_pair( std::nan( "" ), std::nan( "" ) );
        } else {
            T r0, r1;
            if ( b < 0.0 ) {
                disc -= b;
                r0 = 2.0*c/disc;
                r1 = disc/a2;
            } else {
                assert( b > 0.0 );
                disc += b;
                r0 =  -disc/a2;
                r1 = -2.0*c/disc;
            }

           return std::make_pair( std::min( r0, r1 ), std::max( r0, r1 ) );
        }
    }
}