/*
    voltlogger_analyzer
    
    Copyright (C) 2015 Dmitry Yu Okunev <dyokunev@ut.mephi.ru> 0x8E30679C
    
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "configuration.h"
#include "macros.h"

#include <stdio.h>	/* FILE		*/
#include <stdlib.h>	/* size_t	*/
#include <stdint.h>	/* uint64_t	*/
#include <string.h>	/* memset()	*/
#include <math.h>	/* fabs()	*/
#include <assert.h>	/* assert()	*/
//#include <errno.h>	/* errno	*/

#include "error.h"
#include "malloc.h"
#include "analyzer.h"
#include "binary.h"

#define MAX_HISTORY (1 << 20)


typedef int (*vl_checkfunct_t)(history_item_t *value_history, int channelId, uint64_t *value_history_filled_p, char *checkpointfile, int concurrency, void *arg, double error_threshold, char realtime);
typedef int (*vl_interpolatefunct_t)(FILE *o_f, history_item_t *value_history, int channelsNum, int channelId, int addperiods, uint64_t *value_history_filled_p, char *checkpointfile, int concurrency, void *arg, char realtime);

#define SIN_PHASE_PARTS 3

int vl_realcheck_sin(history_item_t *value_history, int channelId, uint64_t value_history_filled, char *checkpointfile, int concurrency, float frequency_p, double error_threshold, char realtime) {
	history_item_t *p, *e;

	p =  value_history;
	e = &value_history[value_history_filled-1];

	uint64_t sensorTS_start     = p->row.sensorTS;
	uint64_t sensorTSDiff_total = e->row.sensorTS - sensorTS_start;
	double y_sum = 0;

	while (p <= e) {
		y_sum += p->row.value[channelId];

		p++;
	}


	double y_avg = (double)y_sum / (double)value_history_filled;
	double y_int = 0;

	history_item_t *p_old = p = value_history;

	while (++p <= e) {
		double y_diff = p->row.value[channelId] - y_avg;
		y_int += y_diff*y_diff * (p->row.sensorTS - p_old->row.sensorTS);

		p_old = p;
	}

	y_int *= 2*M_PI / sensorTSDiff_total;

	double lambda = (double)2*M_PI / sensorTSDiff_total;
	double amp    = sqrt(y_int / M_PI);

	double lambda_expected = 2*M_PI / 20000;

	if (lambda < lambda_expected*0.95) {
		p = value_history;
		while (p <= e) {
			printf("Z\t%lu\t%lu\t%u\t%lf\t%lf\t%lf\t%lf\t%lf\n", p->row.unixTSNano, p->row.sensorTS, p->row.value[channelId], (double)-1, amp, lambda, NAN, y_avg);
			p++;
		}
		return 0;
	}

	// f(x) =  amp * sin(lambda*x + phase) + avg
	double phase;

	p_old = p = value_history;
	phase = 0;

/*	//TODO: Complete this method of phase calculation (instead of iteration method below)

	while (++p <= e) {
		if ((p_old->value - y_avg) * (p->value - y_avg) < 0) {
			double avg_value = (p->value + p_old->value) / 2;

			if (p_old->value < y_avg)
				phase = -lambda * avg_value;
			else
				phase =  lambda * avg_value;

			break;
		}

		p_old = p;
	}
*/

	double sqdeviation_min = 1E100;

	double phase_add_best = M_PI;

	double scan_interval = 2*M_PI;
	while (scan_interval > 1E-3) {
		double phase_add = -scan_interval;
		double phase_add_best_add = 0;
		//fprintf(stderr, "scanning: %le -> %le\n", phase_add_best + phase_add, phase_add_best + scan_interval);
		while (phase_add_best + phase_add < phase_add_best + scan_interval) {
			double sqdeviation = 0;
			p = value_history;
			while (p <= e) {
				double y_calc = amp*sin(lambda*p->row.sensorTS + phase_add_best + phase + phase_add) + y_avg;

				double y_diff = y_calc - (double)p->row.value[channelId];
				//fprintf(stderr, "Y: %li %i %lf %le\n", p->sensorTS, p->value, y_calc, y_diff);

				sqdeviation += y_diff*y_diff;
				p++;
			}
			//fprintf(stderr, "try: %le: %le\n", phase_add_best + phase_add, sqdeviation);
			if (sqdeviation < sqdeviation_min) {
				sqdeviation_min    = sqdeviation;
				phase_add_best_add = phase_add;
			}

			phase_add += scan_interval/SIN_PHASE_PARTS;
		}
		phase_add_best += phase_add_best_add;
		scan_interval /= SIN_PHASE_PARTS;
		//fprintf(stderr, "phase_add_best == %le (%le). sqdeviation_min == %le. scan_interval == %le\n", phase_add_best, phase_add_best_add, sqdeviation_min, scan_interval);
	}

	double err = sqdeviation_min/value_history_filled;///(value_history_filled-1);

	if (err > error_threshold) {
		p = value_history;
		while (p <= e) {
			printf("Z\t%lu\t%lu\t%u\t%lf\t%lf\t%lf\t%lf\t%lf\n", p->row.unixTSNano, p->row.sensorTS, p->row.value[channelId], err, amp, lambda, phase, y_avg);
			p++;
		}
	}

	return 0;
}

int vl_check_sin(history_item_t *value_history, int channelId, uint64_t *value_history_filled_p, char *checkpointfile, int concurrency, void *_frequency_p, double error_threshold, char realtime) {
	float frequency = *(float *)_frequency_p;

	//history_item_t *cur = &value_history[*value_history_filled_p - 1];

	uint64_t expectedEndOffset = (uint64_t)(1E9 /* nanosecs in sec */ / frequency);
	uint64_t     currentOffset = *value_history_filled_p * 50000;

	int rc = 0;
	if ( currentOffset  >=  expectedEndOffset ) {

		rc = vl_realcheck_sin(value_history, channelId, *value_history_filled_p, checkpointfile, concurrency, frequency, error_threshold, realtime);
		*value_history_filled_p = 0;
	}

	return rc;
}

static inline void vl_analyze(FILE *i_f, FILE *o_f, int channelsNum, int channelId, char *checkpointfile, int concurrency, void *arg, vl_checkfunct_t checkfunct, double error_threshold, char realtime) {
	history_item_t *value_history, *history_item_ptr;
	uint64_t        value_history_filled = 0;
	static uint64_t unixTSNano_prev = 0;

	value_history = xcalloc(MAX_HISTORY, sizeof(*value_history));

	while (!feof(i_f)) {
		assert (value_history_filled < MAX_HISTORY);	// overflow

		history_item_ptr = &value_history[ value_history_filled++ ];

		while(1) {
			long pos = ftell(i_f);

			history_item_ptr->row.unixTSNano = get_uint64(i_f, realtime);
			if (unixTSNano_prev != 0 && llabs((int64_t)history_item_ptr->row.unixTSNano - (int64_t)unixTSNano_prev) > 1E9*1E8) {
				fprintf(stderr, "Wrong unixTSNano\n");
				fseek(i_f, pos+1, SEEK_SET);
				continue;
			}

			unixTSNano_prev = history_item_ptr->row.unixTSNano;

			break;
		}

		history_item_ptr->row.sensorTS   = get_uint64(i_f, realtime);

		int i = 0;
		while (i < channelsNum) {
			uint32_t value = get_uint32(i_f, realtime);

			if (i == channelId)
				history_item_ptr->row.value[channelId] = value;

			i++;
		}

		if (checkfunct(value_history, 0, &value_history_filled, checkpointfile, concurrency, arg, error_threshold, realtime)) {
			printf("Problem\n");
		}


	}

	free(value_history);

	return;
}

void vl_analyze_sin(FILE *i_f, FILE *o_f, int channelsNum, int channelId, char *checkpointfile, int concurrency, float frequency, double error_threshold, char realtime)
{
	float *frequency_p = xmalloc(sizeof(float));
	*frequency_p = frequency;

	vl_analyze(i_f, o_f, channelsNum, channelId, checkpointfile, concurrency, frequency_p, vl_check_sin, error_threshold, realtime);

	free(frequency_p);
	return;
}

static inline void out_row(FILE *o_f, history_item_t *history_item_ptr, int channelsNum) {
	out_uint64(o_f, history_item_ptr->row.unixTSNano);
	out_uint64(o_f, history_item_ptr->row.sensorTS);

	int i = 0;
	while (i < channelsNum) {
		out_uint32(o_f,  history_item_ptr->row.value[i++]);
	}

	return;
}

static int vl_interpolatefunct_repeat(FILE *o_f, history_item_t *value_history, int channelsNum, int channelId, int addperiods, uint64_t *value_history_filled_p, char *checkpointfile, int concurrency, void *arg, char realtime) {
	float *freq_p = arg;

	assert (*value_history_filled_p > 2);
	assert (*freq_p != 0);

	float period = 1 / *freq_p;

	fprintf(stderr, "Interpolating (freq: %e; period: %e; value_history_filled: %lu)\n", *freq_p, period, *value_history_filled_p);

	history_item_t *value_history_last       = &value_history[*value_history_filled_p - 1];
	history_item_t *value_history_period_end = &value_history_last[-1];
	history_item_t *value_history_period_start;

	// Getting the value of value_history_period_start
	{
		value_history_period_start = value_history;

		uint64_t end_unixTSNano = value_history_period_end->row.unixTSNano;

		while (end_unixTSNano - value_history_period_start->row.unixTSNano > period) {
			value_history_period_start++;
			assert(value_history_period_start < value_history_period_end);
		}
		value_history_period_start--;

		assert (value_history_period_start >= value_history);
	}


	// Interpolating
	{ 

		uint64_t last_unixTSNano           =  value_history_last->row.unixTSNano;
		uint64_t unixTSNano_cur            =  value_history_period_end->row.unixTSNano;
		uint64_t sensorTS_cur              =  value_history_period_end->row.sensorTS;
		history_item_t *value_history_prev =  value_history_period_start;
		history_item_t *value_history_cur  = &value_history_period_start[1];

		while(1) {

			if (value_history_cur == value_history_period_end) {
				value_history_prev =  value_history_period_start; 
				value_history_cur  = &value_history_period_start[1];
			}

			unixTSNano_cur += value_history_cur->row.unixTSNano - value_history_prev->row.unixTSNano;
			sensorTS_cur   += value_history_cur->row.sensorTS   - value_history_prev->row.sensorTS;

			fprintf(stderr, "Interpolating… value_history_period_start: %lu (%lu < %lu); %lu %p %p\n", sensorTS_cur, unixTSNano_cur, last_unixTSNano, value_history_cur->row.sensorTS, value_history_cur, value_history_period_end);
			if (unixTSNano_cur >= last_unixTSNano)
				break;

			uint64_t unixTSNano_orig = value_history_cur->row.unixTSNano;
			uint64_t sensorTS_orig   = value_history_cur->row.sensorTS;
			value_history_cur->row.unixTSNano = unixTSNano_cur;
			value_history_cur->row.sensorTS   = sensorTS_cur;
			out_row(o_f, value_history_cur, channelsNum);
			value_history_cur->row.unixTSNano = unixTSNano_orig;
			value_history_cur->row.sensorTS   = sensorTS_orig;

			value_history_prev = value_history_cur;
			value_history_cur++;
		}
	}


	return 0;
}

static inline void vl_interpolate(FILE *i_f, FILE *o_f, int channelsNum, int channelId, int addperiods, char *checkpointfile, int concurrency, void *arg, vl_interpolatefunct_t interpolatefunct, char realtime) {
	history_item_t *value_history, *history_item_ptr;
	uint64_t        value_history_filled =  0;
	static uint64_t unixTSNano_prev      = (1E9*1E8)/2;
	static uint64_t unixTSNano_diff_prev = ((uint64_t)~0)/2;
	static uint64_t unixTSNano_diff      = ((uint64_t)~0)/2;

	value_history = xcalloc(MAX_HISTORY, sizeof(*value_history));

	while (!feof(i_f)) {
		assert (value_history_filled < MAX_HISTORY);	// overflow

		history_item_ptr = &value_history[ value_history_filled++ ];

		while(1) {
			long pos = ftell(i_f);

			history_item_ptr->row.unixTSNano = get_uint64(i_f, realtime);
			if (unixTSNano_prev != 0 && llabs((int64_t)history_item_ptr->row.unixTSNano - (int64_t)unixTSNano_prev) > 1E9*1E8) {
				fprintf(stderr, "Wrong unixTSNano\n");
				fseek(i_f, pos+1, SEEK_SET);
				continue;
			}

			unixTSNano_diff_prev = unixTSNano_diff;
			unixTSNano_diff      = history_item_ptr->row.unixTSNano - unixTSNano_prev;

			unixTSNano_prev      = history_item_ptr->row.unixTSNano;

			break;
		}

		history_item_ptr->row.sensorTS   = get_uint64(i_f, realtime);

		{
			int i = 0;
			while (i < channelsNum)
				history_item_ptr->row.value[i++] = get_uint32(i_f, realtime);
		}

		if ((unixTSNano_diff+1) / (unixTSNano_diff_prev+1) > 10)
			if (interpolatefunct(o_f, value_history, channelsNum, channelId, addperiods, &value_history_filled, checkpointfile, concurrency, arg, realtime)) {
				fprintf(stderr, "Problem\n");
			}

		out_row(o_f, history_item_ptr, channelsNum);
	}

	free(value_history);

	return;
}

void vl_interpolate_repeat(FILE *i_f, FILE *o_f, int channelsNum, int channelId, int addperiods, char *checkpointfile, int concurrency, float frequency, char realtime)
{
	float *frequency_p = xmalloc(sizeof(float));
	*frequency_p = frequency;

	vl_interpolate(i_f, o_f, channelsNum, channelId, addperiods, checkpointfile, concurrency, frequency_p, vl_interpolatefunct_repeat, realtime);

	free(frequency_p);
	return;
}