#include <stdio.h>
#include <math.h>

#if defined(WIN32)
#include "SPTK.h"
#else
#include <SPTK.h>
#endif

#define DEBUG_LEVEL 0 // 0 | 1 | 2

/**********************************************************
 *
 * Created on: Mar 1, 2016
 * Author: Fabio Tesser
 * Email: fabio.tesser@gmail.com
 *
 **********************************************************/

// fame length odd?
double autocorr_function_C(double *samples, int frame_length, int upper_lag, int l,
		int m, int k) {
	double sum;
	int n;

	//int mid_frame_position = frame_length / 2;
	//int min = mid_frame_position - (k/2) - (upper_lag/2);
	//int max = mid_frame_position - (k/2) + (upper_lag/2);
	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr,"autocorr_function_C: L=%d, upper_lag=%d, l=%d, m=%d, k=%d\n",frame_length, upper_lag,  l, m, k);
	}

	int min = (frame_length - k - upper_lag) / 2;
	int max = (frame_length - k + upper_lag) / 2;

	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr,"autocorr_function: L=%d, min=%d, max=%d\n",frame_length, min,  max);
	}

	//if (max-min != upper_lag)
	//	fprintf(stderr,"MEGA ERROR\n");

	for (sum = 0.0, n = min; n <= max; n++) {
		sum += samples[n + l] * samples[n + m];
	}

	return sum;
}

/*
 * normalized_autocorr_function
 * compute normalized autocorrelation at index k using l samples
 * l length - number of samples to use
 * k index - order
 */
double normalized_autocorr_function_R(double *samples, int frame_length,
		int upper_lag, int l) {

	double numerator = autocorr_function_C(samples, frame_length, upper_lag, 0, l, l);

	if (numerator == 0) {
		return 0.0;
	}

	double denominator = sqrt(
			autocorr_function_C(samples, frame_length, upper_lag, 0, 0, l)
					* autocorr_function_C(samples, frame_length, upper_lag, l, l, l));

    //return numerator/denominator; //force positive strenghts!
	return fabs(numerator)/denominator;
}

void norm_auto_corr_in_interval(double *x, int frame_length, int upper_lag,
		double *r, int np_min, int np_max) {
	int l;

	for (l = np_min; l <= np_max; l++) {
		r[l - np_min] = normalized_autocorr_function_R(x, frame_length,
				upper_lag, l);
	}

	return;
}

double peak_norm_auto_corr_in_interval(double *samples, int frame_length, int upper_lag,
		double *correl, int actual_lower_lag, int actual_upper_lag) {
	norm_auto_corr_in_interval(samples, frame_length, upper_lag, correl,
			actual_lower_lag, actual_upper_lag);
	// find the max: str_value will be the max on correl array
	// T is the max index
	int T = actual_lower_lag;
	double str_value = correl[0];

	for (int k = actual_lower_lag; k <= actual_upper_lag; k++) {
		if (correl[k - actual_lower_lag] > str_value) {
			T = k;
			str_value = correl[k - actual_lower_lag];
		}
	}

	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr, "str_value: %f, max index: %d\n", str_value, T);
	}

	return str_value;
}

double compute_fractional_pitch_period(double *samples, int frame_length,
		int upper_lag, int T) {

	// 1)
	int T_end=T;

	// 2)
	//upper_lag=T;
	//int T_end=0;
	/**********************************************************/
	double c_0_Tp1_T, c_T_T_T, c_0_T_T, c_T_Tp1_T, c_Tp1_Tp1_T;

	c_0_Tp1_T = autocorr_function_C(samples, frame_length, upper_lag, 0, T + 1,T_end);
	c_T_T_T = autocorr_function_C(samples, frame_length, upper_lag, T, T, T_end);
	c_0_T_T = autocorr_function_C(samples, frame_length, upper_lag, 0, T, T_end);
	c_T_Tp1_T = autocorr_function_C(samples, frame_length, upper_lag, T, T + 1, T_end);
	c_Tp1_Tp1_T = autocorr_function_C(samples, frame_length, upper_lag, T + 1, T + 1, T_end);

	if (DEBUG_LEVEL >= 2) {
		fprintf(stderr, "c_0_Tp1_T=%f\n", c_0_Tp1_T);
		fprintf(stderr, "c_T_T_T=%f\n", c_T_T_T);
		fprintf(stderr, "c_0_T_T=%f\n", c_0_T_T);
		fprintf(stderr, "c_T_Tp1_T=%f\n", c_T_Tp1_T);
		fprintf(stderr, "c_Tp1_Tp1_T=%f\n", c_Tp1_Tp1_T);
	}

	double fractional_pitch_period, fractional_pitch_period_num,
			fractional_pitch_period_den;

	fractional_pitch_period_num = (c_0_Tp1_T * c_T_T_T) - (c_0_T_T * c_T_Tp1_T);
	fractional_pitch_period_den = (c_0_Tp1_T * (c_T_T_T - c_T_Tp1_T))
			+ (c_0_T_T * (c_Tp1_Tp1_T - c_T_Tp1_T));

	if (fractional_pitch_period_num == 0) {
	  if (DEBUG_LEVEL >= 1) {
	   fprintf(stderr, "fractional_pitch_period: num=%f, den=%f\n",
			fractional_pitch_period_num, fractional_pitch_period_den);
	  }
	  return 0;
	} else if (fractional_pitch_period_den == 0) {
	  fprintf(stderr, "WARNING: fractional_pitch_period: num=%f, den=%f\n",
			fractional_pitch_period_num, fractional_pitch_period_den);
	  return 0;
	}

	fractional_pitch_period = fractional_pitch_period_num
			/ fractional_pitch_period_den;

	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr, "fractional_pitch_period: num=%f, den=%f, X=%f\n",
			fractional_pitch_period_num, fractional_pitch_period_den,
			fractional_pitch_period);
	}

	return fractional_pitch_period;
}

/*
 * 	Compute normalized auto_correlation value with fractional_pitch_period
 */
double normalized_auto_correlation_fractional_pitch_period(double *buffer_tmp,
		int frame_length, int upper_lag, int T, double fractional_pitch_period) {

	double c_0_T_T, c_0_Tp1_T, c_0_0_T, c_T_T_T, c_T_Tp1_T, c_Tp1_Tp1_T;

	c_0_T_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, 0, T, T);
	c_0_Tp1_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, 0, T + 1, T);
	c_0_0_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, 0, 0, T);
	c_T_T_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, T, T, T);
	c_T_Tp1_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, T, T + 1, T);
	c_Tp1_Tp1_T = autocorr_function_C(buffer_tmp, frame_length, upper_lag, T + 1, T + 1, T);

	if (DEBUG_LEVEL >= 2) {
		fprintf(stderr, "c_0_T_T=%f\n", c_0_T_T);
		fprintf(stderr, "c_0_Tp1_T=%f\n", c_0_Tp1_T);
		fprintf(stderr, "c_0_0_T=%f\n", c_0_0_T);
		fprintf(stderr, "c_T_T_T=%f\n", c_T_T_T);
		fprintf(stderr, "c_T_Tp1_T=%f\n", c_T_Tp1_T);
		fprintf(stderr, "c_Tp1_Tp1_T=%f\n", c_Tp1_Tp1_T);
	}

	double NUM = ((1 - fractional_pitch_period) * c_0_T_T)
			+ (fractional_pitch_period * c_0_Tp1_T);

	double DEN = c_0_0_T
			* (pow(1 - fractional_pitch_period, 2) * c_T_T_T
					+ 2 * fractional_pitch_period * (1 - fractional_pitch_period) * c_T_Tp1_T
					+ pow(fractional_pitch_period, 2) * c_Tp1_Tp1_T);

	double str_value = 0;

	if (DEN == 0){
		fprintf(stderr, "WARNING: DEN=%f, equal to zero, set str_value to zero\n", DEN);
	}
	else
	{
		// used fabs() in order to force positive strenghts!
		str_value = fabs(NUM) / sqrt(DEN);
	}

	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr, "NUM=%f, DEN=%f, str_value=%f\n", NUM, DEN, str_value);
	 fprintf(stderr, "str_value: %f, max index: %d\n", str_value, T);
	}

	return str_value;
}

double peak_norm_auto_corr_in_interval_frac(double *samples, int frame_length,
		int upper_lag, double *correl, int actual_lower_lag, int actual_upper_lag) {
	norm_auto_corr_in_interval(samples, frame_length, actual_upper_lag, correl,
			actual_lower_lag, actual_upper_lag);
	// find the max: str_value will be the max on correl array
	double str_value = correl[0];
	// T is the max index
	int T = actual_lower_lag;
	for (int k = actual_lower_lag; k <= actual_upper_lag; k++) {
		if (correl[k - actual_lower_lag] > str_value) {
			T = k;
			str_value = correl[k - actual_lower_lag];
		}
	}

	if (DEBUG_LEVEL >= 2) {
	 fprintf(stderr, "str_value 1: %f, max index: %d\n", str_value, T);
	}

	//Speech Coding Algorithms: Foundation and Evolution of Standardized Coders
	double c_0_Tp1_T, c_0_Tm1_T;

	c_0_Tp1_T = autocorr_function_C(samples, frame_length, upper_lag, 0, T + 1,T);
	c_0_Tm1_T = autocorr_function_C(samples, frame_length, upper_lag, 0, T - 1,T);

	if (DEBUG_LEVEL >= 2) {
		fprintf(stderr, "c_0_Tp1_T=%f\n", c_0_Tp1_T);
		fprintf(stderr, "c_0_Tm1_T=%f\n", c_0_Tm1_T);

	}

	if (c_0_Tm1_T > c_0_Tp1_T) {
		T = T - 1;
		if (DEBUG_LEVEL >= 2) {
			fprintf(stderr, "Updated T value: T=%d\n", T);
		}
	}

	double fractional_pitch_period = compute_fractional_pitch_period(samples,
			frame_length, upper_lag, T);

	if (fractional_pitch_period == 0)
		return str_value;

	int original_T = T;
	int number_of_iteration=1;
	int max_number_of_iteration = 10; //TODO_TEST HIGH number of iteration in combination with -p

	while (fractional_pitch_period < 0 || fractional_pitch_period > 1)
	{
		if (number_of_iteration >= max_number_of_iteration)
		{
			fprintf(stderr, "WARN: fractional_pitch_period; max_number_of_iteration reached. original_T=%d, original_str=%f\n", original_T, str_value);
			fprintf(stderr, "fractional_pitch_period: KO\n");
			return str_value;
		}
        if (number_of_iteration > 1 && T == original_T)
		{
			fprintf(stderr, "WARN: fractional_pitch_period; exit from probable infinite loop computation. original_T=%d, original_str=%f\n", original_T, str_value);
			fprintf(stderr, "fractional_pitch_period:KO\n");
			return str_value;
		}


		fprintf(stderr, "WARN: fractional_pitch_period out of bounds! fractional_pitch_period=%f, for T=%d;original_T=%d;number_of_iteration=%d\n",
				fractional_pitch_period,T,original_T,number_of_iteration);

		if (fractional_pitch_period < 0)
			T = T - 1;
		else // ractional_pitch_period > 0
			T = T + 1;

		fractional_pitch_period = compute_fractional_pitch_period(samples,frame_length, upper_lag, T);
		number_of_iteration++;
	}


	if (DEBUG_LEVEL >= 2) {
		fprintf(stderr, "fractional_pitch_period:OK1\n");
		fprintf(stderr, "str_value 1: %f, max index: %d\n", str_value, T);
	}

	double old_str = str_value;

	str_value = normalized_auto_correlation_fractional_pitch_period(samples,
			frame_length, upper_lag, T, fractional_pitch_period);

	if (DEBUG_LEVEL >= 2) {
		fprintf(stderr, "str_value 2: %f, max index: %d\n", str_value, T);
		fprintf(stderr, "diff str_value 2: %f, max index: %d\n", str_value-old_str, T-original_T);
	}

    if (str_value > 1) {
    	str_value = 1;
    	fprintf(stderr, "WARN: str_value Out of bounds! str_value=%f\n", str_value);
    	fprintf(stderr, "fractional_pitch_period:KO1\n");
    } else if (str_value < - 1) {
    	str_value = -1;
    	fprintf(stderr, "WARN: str_value Out of bounds! str_value=%f\n", str_value);
    	fprintf(stderr, "fractional_pitch_period:KO1\n");
    } else {
    	if (DEBUG_LEVEL >= 2) {
    	 fprintf(stderr, "fractional_pitch_period:OK2\n");
    	}
    }

    if (DEBUG_LEVEL >= 2) {
     fprintf(stderr, "str_value 3: %f, max index: %d\n", str_value, T);
    }

    return str_value;
}