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

#include <cmath>

// The constructor
//
// We assume that the initial state of the system is some constant
// that is passed to the constructor, a parameter that has a default
// value of 0.0.
//
// Assuming that all previous values were the passed value, then
//
//        S  = y N
//         y
//                 N (1 - N)
//       SP    = y ---------
//         xy          2
//
// and the least-squares linear polynomial passing through these poitns
// is p(k) = 0k + y

template <typename T, unsigned int N>
Linear_estimator<T, N>::Linear_estimator( T y ):
s_y_{y*N},
sp_xy_{y*N*(1.0 - N)/2.0},
index_{0},
a_{0.0},
b_{y} {
	for ( std::size_t k{0}; k < N; ++k ) {
		y_[k] = y;
	}
}

template <typename T, unsigned int N>
void Linear_estimator<T, N>::calculate_coefficients() {
	// From these, derive the coefficients of the 
	// least-squares best-fitting polynomial ax + b
        //  - 6 FLOPs + 1 ASSIGN

	T const ma{6.0/((N - 1.0)*N*(N + 1.0))};
	a_ = (    N - 1.0)*ma*s_y_
                     + 2.0*ma*sp_xy_;

	T const mb{2.0/(N*(N + 1.0))};
       	b_ = (2.0*N - 1.0)*mb*s_y_
	             + 3.0*mb*sp_xy_;
}

// Linear_estimator::add( T y, T epsilon )
//
// The addition of a new point requires a total of
//      12 FLOPs
//       6 assignments
//       2 arithmetic operations
//
// Recall that the compiler will calculate, for example, (2.0*N - 1.0)*mb
// at compile time, thus reducing (2.0*N - 1.0)*mb*s_y_ to a single
// floating-point operation with a fixed constant as the first operand.

template <typename T, unsigned int N>
T Linear_estimator<T, N>::add( T y, T epsilon ) {
	// Update the sum of the ys
	//    and the sum of the products of the xys
	sp_xy_ += N*y_[index_] - s_y_;        // 4 FLOPs + 1 ASSIGN
	s_y_   += -y_[index_] + y;            // 2 FLOP  + 1 ASSIGN
	y_[index_] = y;                       //           1 ASSIGN

	if ( epsilon != 0.0 ) {
		// Deal with jitter
		//  - the reading was taken at the current
		//    time plus epsilon times the period
		//  - find that reading at the current time
		//
		// Note that while this appears complex, most of these
		// coefficients can be calculated at compile time, so
		// the total number of FLOPs at execution time is only 
		// 20, as can be seen here:
		//
		//     y = (C0*y + (
		//        (C1*y - C2*s_y_ + C3*sp_xy_)*epsilon
		//      + (C4*y - C5*s_y_ - C6*sp_xy_)
		//     )*epsilon)/(
		//         C7
		//          + C8*epsilon*(C9 + epsilon)
		//     );

		y = (N*N*(N - 1.0)*(2.0*N - 1.0)*(N + 1.0)*y + ((
		    6.0*(3.0*N - 1.0)*(N - 2.0)*y
		  - 6.0*(3.0*N - 1.0)*(N - 2.0)*s_y_
		               + 36.0*(1.0 - N)*sp_xy_
		)*epsilon + (
		    3.0*N*(N - 1.0)*((N + 9.0)*N - 4.0)*y
		       - 12.0*N*(N - 1.0)*(2.0*N - 1.0)*s_y_
		                  - 6.0*N*(7.0*N - 5.0)*sp_xy_
		))*epsilon)/(
		    (2.0*N - 1.0)*(N - 1.0)*N*N*(N + 1.0)
		     + 12.0*(2.0*N - 1.0)*(N - 1.0)*epsilon*(N + epsilon)
		);

		// Update the sum of the ys
		//  - no need to upsdate the sum of the products xys
		s_y_ += -y_[index_] + y;
		y_[index_] = y;
	}

	// If after incrementation, index_ == N, reset N to 0
	if ( ++index_ == N ) {
		index_ = 0;
	}

	calculate_coefficients();

	return y;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::recalculate_sums() {
	T old_a{ a_ };
	T old_b{ b_ };

	s_y_   = 0.0;
	sp_xy_ = 0.0;

	for ( std::size_t k{0}; k < N; ++k ) {
		 s_y_  +=   y_[k];
		sp_xy_ += (-1.0*k)*y_[(N + index_ - k) % N];
	}

	calculate_coefficients();

	return std::sqrt( (a_ - old_a)*(a_ - old_a)
	                + (b_ - old_b)*(b_ - old_b) );
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::current() const {
	return b_;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::next() const {
	return b_ + a_;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::change_per_period() const {
	return a_;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::average_last_period() const {
	return b_ - a_/2.0;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::average_next_period() const {
	return b_ + a_/2.0;
}

template <typename T, unsigned int N>
T Linear_estimator<T, N>::zero() const {
	return -b_/a_;
}

template <typename T, unsigned int N>
Quadratic_estimator<T, N>::Quadratic_estimator( T y ):
s_y_{y*N},
sp_xy_{y*N*(N - 1.0)/2.0},
sp_xxy_{y*N*(N - 1.0)*(2.0*N - 1.0)/6.0},
index_{0},
a_{0.0},
b_{0.0},
c_{y} {
	for ( std::size_t k{0}; k < N; ++k ) {
		y_[k] = y;
	}
}

template <typename T, unsigned int N>
void Quadratic_estimator<T, N>::calculate_coefficients() {
        // 15 FLOPs and 3 ASSIGNs
	T const ma{30.0/((N - 2.0)*(N - 1.0)*N*(N + 1.0)*(N + 2.0))};
	a_ = (N - 1.0)*(N - 2.0)*ma*s_y_
	         + 6.0*(N - 1.0)*ma*sp_xy_
	                + 6.0*ma*sp_xxy_;

	T const mb{6.0/((N - 2.0)*(N - 1.0)*N*(N + 1.0)*(N + 2.0))};
	b_ = (((6.0*N - 21.0)*N + 21.0)*N - 6.0)*mb*s_y_
	        + 2.0*((16.0*N - 30.0)*N + 11.0)*mb*sp_xy_
	                        + 30.0*(N - 1.0)*mb*sp_xxy_;

	T const mc{3.0/(N*(N + 1.0)*(N + 2.0))};
	c_ = ((3.0*N - 3.0)*N + 2.0)*mc*s_y_
	         + 6.0*(2.0*N - 1.0)*mc*sp_xy_
	                   + 10.0*mc*sp_xxy_;
}

// The addition of a new point requires a total of
//       25 FLOPs
//        8 assignments
//        2 arithmetic operations
//
// Recall that the compiler will calculate, for example,
//                (6.0*N*N*N - 21.0*N*N + 21.0*N - 6.0)*mb
// at compile time, thus reducing
//                (6.0*N*N*N - 21.0*N*N + 21.0*N - 6.0)*mb*s_y_
// to a single floating-point operation with a fixed constant as the
// first operand.

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::add( T y, T epsilon ) {
	// Update the sum of the ys
	//    and the sum of the products of the x-ys
	//    and the sum of the products of the xxys
	sp_xxy_ += -N*N*y_[index_] - 2.0*sp_xy_ + s_y_; // 5 FLOPs + 1 ASSIGN
	sp_xy_  +=    N*y_[index_]              - s_y_; // 3 FLOPs + 1 ASSIGN
	s_y_    +=     -y_[index_] + y;                 // 2 FLOP  + 1 ASSIGN
	y_[index_] = y;                                 //           1 ASSIGN

	if ( epsilon != 0.0 ) {
		// Deal with jitter
		//  - the reading was taken at the current
		//    time plus epsilon times the period
		//  - find that reading at the current time
		//
		// Note that while this appears complex, most of these
		// coefficients can be calculated at compile time, so
		// the total number of FLOPs at execution time is only 
		// 44, as can be seen here:
		//
		//     y = (
		//       (
		//         (
		//           (
		//             (
		//               C0*y + C1*s_y_ + C2*sp_xy_ - C3
		//             )*epsilon + (
		//               C4*y - C5*s_y_ - C6*sp_xy_ - C7*sp_xxy_
		//             )
		//           )*epsilon + (
		//             C8*y + C9*s_y_ - C10*sp_xy_ + C11*sp_xxy_
		//           )
		//         )*epsilon + (
		//           C12*y - C13*s_y_ - C14*sp_xy_ - C15*sp_xxy_
		//         )
		//       )*epsilon + C16
		//     ) /(((
		//       (C17*epsilon + C18)*epsilon + C19
		//     )*epsilon + C20)*epsilon + C21);

		// Use Horner's rule
		y = (
		  (
		    (
		      (
		        (
		          60.0*(((
		              (5.0*N - 46.0)*N + 49.0
		          )*N - 32.0)*N + 12.0)*y + 60.0*(((
		              (-5.0*N + 46.0)*N - 49.0
		          )*N + 32.0)*N - 12.0)*s_y_ + 360.0*((
		              (24.0 - 5.0*N)*N - 19.0
		          )*N + 6.0)*sp_xy_ - 1800.0*(N - 1.0)*(N - 2.0)*sp_xxy_
		        )*epsilon + 120.0*(
		            6.0*N*(N - 1.0)*(((N - 6.0)*N + 3.0)*N - 2.0)*y
		          - 6.0*N*(N - 1.0)*(((N - 6.0)*N + 3.0)*N - 2.0)*s_y_
		             - N*(2.0*N - 1.0)*((17.0*N - 57.0)*N + 34.0)*sp_xy_
		                       - 3.0*N*((11.0*N - 27.0)*N + 22.0)*sp_xxy_
		        )
		      )*epsilon + (
		        2.0*((((
		          (((5.0*N + 311.0)*N - 1591.0)*N + 2015.0)*N - 802.0
		        )*N - 58.0)*N + 264.0)*N - 72.0)*y + 12.0*(((
		          ((3.0*(87.0 - 17.0*N)*N - 340.0)*N + 137.0)*N + 13.0
		        )*N - 44.0)*N + 12.0)*s_y_ - 36.0*(N - 1.0)*((
		          ((89.0*N - 183.0)*N + 52.0)*N + 42.0
		        )*N - 12.0)*sp_xy_ + 120.0*(
		          ((25.0*(2.0 - N)*N - 32.0)*N - 11.0)*N + 6.0
		        )*sp_xxy_
		      )
		    )*epsilon + (
		      6.0*N*(N - 1.0)*(N - 2.0)*(2.0*N - 1.0)
		        *(N + 1.0)*(((N + 21.0)*N - 16.0)*N + 12.0)*y
		      - 36.0*N*(N - 1.0)*(N - 2.0)*(N + 1.0)
		             *(2.0*N - 1.0)*(3.0*(N - 1.0)*N + 2.0)*s_y_
		      - 24.0*N*(2.0*N - 1.0)*(N + 1.0)
		             *(((21.0*N - 60.0)*N + 56.0)*N - 20.0)*sp_xy_
		      - 180.0*N*(N - 1.0)*(N + 1.0)
		                           *((5.0*N - 8.0)*N + 4.0)*sp_xxy_
		    )
		  )*epsilon + (
		      N*N*(N - 1.0)*(N - 2.0)*(2.0 + N)
		         *(3.0*(N - 1.0)*N + 2.0)*(N + 1.0)*(N + 1.0)*y
		  )
		) / (
		  (
		    (
		      (
		        180.0*(N - 2.0)*(N - 1.0)
		            *((3.0*N - 3.0)*N + 2.0)*epsilon 
		        + 360.0*N*(N - 2.0)*(N - 1.0)*((3.0*N - 3.0)*N + 2.0)
		      )*epsilon + 36.0*(N - 2.0)*((7.0*N + 1.0)*N - 1.0)
		          *(N - 1.0)*((3.0*N - 3.0)*N + 2.0)
		    )*epsilon + 36.0*N*(N - 1.0)*(N - 2.0)*(2.0*N - 1.0)
		        *(N + 1.0)*((3.0*N - 3.0)*N + 2.0)
		  )*epsilon + N*N*(N - 1.0)*(N - 2.0)*(2.0 + N)
		      *((3.0*N - 3.0)*N + 2.0)*(N + 1.0)*(N + 1.0)
		);

		// Update the sum of the ys
		//  - no need to upsdate the sum of the products xys
		s_y_ += -y_[index_] + y;
		y_[index_] = y;
	}

	// If after incrementation, index_ == N, reset N to 0
	if ( ++index_ == N ) {
		index_ = 0;
	}

	calculate_coefficients();

	return y;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::recalculate_sums() {
	T old_a{ a_ };
	T old_b{ b_ };
	T old_c{ c_ };

	 s_y_   = 0.0;
	sp_xy_  = 0.0;
	sp_xxy_ = 0.0;

	for ( std::size_t k{0}; k < N; ++k ) {
		 s_y_  +=   y_[k];
		sp_xy_ += (-1.0*k)*y_[(N + index_ - k) % N];
		sp_xxy_ += k*k*y_[(N + index_ - k) % N];
	}

	calculate_coefficients();

	return std::sqrt( (a_ - old_a)*(a_ - old_a)
	                + (b_ - old_b)*(b_ - old_b)
	                + (c_ - old_c)*(c_ - old_c) );
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::current() const {
	return c_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::next() const {
	return c_ + b_ + a_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::change_per_period() const {
	return b_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::change_per_period_per_period() const {
	return 2.0*a_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::average_last_period() const {
	return a_/3.0 - b_/2.0 + c_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::average_next_period() const {
	return a_/3.0 + b_/2.0 + c_;
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::zero() const {
	return std::nan( "" );
}

template <typename T, unsigned int N>
T Quadratic_estimator<T, N>::extremum() const {
	return -b_/(2.0*a_);
}