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

//y = Cbx + Ka
void least_square_init(least_square_t *ls, int samples){
	ls->max_samples = samples;
	ls->num_samples = 0;
	ls->Cb = 0;
	ls->Ka = 0;
	ls->mul_xx = 0;
	ls->mul_xy = 0;
	ls->sum_x = 0;
	ls->sum_y = 0;
}
int least_square_put(least_square_t *ls, double x, double y){
	if (ls->num_samples < ls->max_samples){
		ls->num_samples ++;
		ls->last_x = x;
		ls->last_y = y;
		ls->mul_xx += x * x;
		ls->mul_xy += x * y;
		ls->sum_x += x;
		ls->sum_y += y;
		return 0;
	}else {
		ls->Cb = (ls->max_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->max_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
		ls->Ka = ls->sum_y / ls->max_samples - ls->Cb * ls->sum_x / ls->max_samples;
		ls->mul_xx += x * x;
		ls->mul_xy += x * y;
		ls->sum_x += x;
		ls->sum_y += y;

		ls->mul_xx -= ls->last_x * ls->last_x;
		ls->mul_xy -= ls->last_x * ls->last_y;
		ls->sum_x -= ls->last_x;
		ls->sum_y -= ls->last_y;
		ls->last_x = x;
		ls->last_y = y;
		return 1;
	}
}

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

int get_x_by_y(least_square_t *ls, double y) {
	if (ls->Cb == 0){
		return -1; 
	}
	int x = (y - ls->Ka)/ls->Cb;
	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