#include <stdio.h>
#include "Least_Square.h"

//y = Kax + Cb
void least_square_init(least_square_t *ls, int samples){
	ls->max_samples = samples;
	ls->num_samples = 0;
	ls->coeff.Ka = 1.0f;
	ls->coeff.Cb = 0.0f;
	ls->mul_xx = 0;
	ls->mul_xy = 0;
	ls->sum_x = 0;
	ls->sum_y = 0;
	ls->finished = 0;
}
int least_square_put(least_square_t *ls, double x, double y){
	if (!ls->finished){
		ls->num_samples ++;
		ls->mul_xx += x * x;
		ls->mul_xy += x * y;
		ls->sum_x += x;
		ls->sum_y += y;
		if (ls->num_samples == ls->max_samples){
			ls->coeff.Ka = (ls->num_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->num_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
			ls->coeff.Cb = ls->sum_y / ls->num_samples - ls->coeff.Ka * ls->sum_x / ls->num_samples;
			ls->finished = 1;
		}
	}else {
		ls->num_samples ++;

		ls->mul_xx += x * x;
		ls->mul_xy += x * y;
		ls->sum_x += x;
		ls->sum_y += y;
	
		ls->coeff.Ka = (ls->num_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->num_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
		ls->coeff.Cb = ls->sum_y / ls->num_samples - ls->coeff.Ka * ls->sum_x / ls->num_samples;
	}
	return ls->finished;
}

double get_y_by_x(least_square_t *ls, double x){
	return ls->coeff.Ka * x + ls->coeff.Cb;
}

int get_x_by_y(least_square_t *ls, double y) {
	if (ls->coeff.Ka == 0){
		return -1; 
	}
	int x = (y - ls->coeff.Cb)/ls->coeff.Ka;
	return  (x >= 0) ? x : -1;
}

#if 0
void Least_square_method(void)
{
	int i=0;
	double A = 0, B = 0, C = 0, D = 0;	
	for(i=0;i<sample_count;i++) {
		A += samples[i].x * samples[i].y;
		B += samples[i].x;
		C += samples[i].y;
		D += samples[i].x * samples[i].x;
	}
	AA = (sample_count * A - B * C) / ( sample_count * D - B * B);
	BB = C / sample_count - AA * B / sample_count;
	sys_debug("Gain = %f, zero Off = %f\n", AA, BB);
}

void add_smaple(float x, float y){
	if (sample_count < 5){
		samples[sample_count].x = x;
		samples[sample_count].y = y;
		sample_count ++;
		sys_debug("add x = %f, y = %f\n", x, y);
	}
	if (sample_count == 5){
		Least_square_method();
	}
}
#endif