#include <algorithm>
#include <math.h>
#include "line_fit.h"
#include "line.h"
fit_item::fit_item()
	:x(0)
	,y(0)
	,xx(0)
	,xy(0)
{
}

fit_item&fit_item::set(double x,double y)
{
	this->x=x;
	this->y=y;

	xx=x*x;
	xy=x*y;

	return *this;
}

fit_batch::fit_batch()
{
	tool.reserve(8);
}

fit_batch::~fit_batch()
{
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		delete f;
	});
}

void fit_batch::add_tool(double max_x_span,int min_point)
{
	tool.push_back(new card_fit(*this,max_x_span,min_point));
}

void fit_batch::add_tool(double max_x_span,int min_point,int max_point)
{
	tool.push_back(new card_fit_lastn(*this,max_x_span,min_point,max_point));
}


void fit_batch::log()
{
//	printf("xo=%lf,yo=%.3lf\n",xo,yo);

	//for(int i=0,len=size();i<len;i++)
	//	at(i).log();
		printf("epos-bpos:%d, xo:%f, yo:%f\n",epos - bpos, xo, yo);
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->log();
	});
}

void fit_batch::reset_data()
{
	xo=yo=0;
	clear();
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->reset();
	});
}

double fit_batch::add(double x,double y)
{
	if(x==0 && y==0)
		return 0;
	
	if(empty())
	{
		xo=x; yo=y;
		std::for_each(tool.begin(),tool.end(),[this](card_fit*f){
				f->xo=xo;
				f->yo=yo;
		});
	}

	grow().set(x-xo,y-yo);
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
			f->add_last();
			f->fit();
	});
	

	return 0;
}
#if 0
void fit_batch::replace(double x,double y)
{
	if(empty())
		return;

	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->remove_last();
	});

	rat(0).set(x-xo,y-yo);

	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->add_last();
		f->fit();
	});
}
#endif


double fit_batch::x(int index)const
{
	return (*this)(index).x + xo;
}

double fit_batch::y(int index)const
{
	return (*this)(index).y + yo;
}

fit_result::fit_result()
	:k(0)
	,kb(0)
	,ke(0)
	,ka(0)
	,num_point(0)
	,min_point(0)
	,max_x_span(0)
{
}

fit_result::fit_result(double _max_x_span,int min_point_)
	:k(0)
	,kb(0)
	,ke(0)
	,ka(0)
	,num_point(0)
	,min_point(min_point_)
	,max_x_span(_max_x_span)
{
}
void fit_result::reset()
{
	k=kb=ke=ka=0;
	xo=yo=0;
	num_point=0;
}

card_fit::~card_fit()
{

}

card_fit::card_fit(const fit_batch&d_,double _max_x_span,int min_point_)
	:fit_result(_max_x_span,min_point_)
	,d(d_)
{
}

void card_fit::dec_(const fit_item&i)
{
	A -= i.xx;
	B -= i.x;
	C -= i.xy;
	D -= i.y;
}

void card_fit::add_(const fit_item&i)
{
	A += i.xx;
	B += i.x;
	C += i.xy;
	D += i.y;
}


void card_fit::add_last()
{
	++num_point;
	add_(d(0));
}
#if 0
void card_fit::remove_last()
{
	--num_point;
	dec_(d(0));
}
#endif

void card_fit::reset()
{
	fit_result::reset();
	A=B=C=D=0;
}

double fit_result::testk(double x,double y)const
{
	return k * (x-xo) + kb - (y-yo);
}

double fit_result::differ_k(double x,double y)const
{
	return fabs(k*x+kb-y);
}

bool card_fit::check_x_span()
{
	int i=size()-1;
	for(;i>0;i--)
	{
		double off=d(0).x-d(i).x;
		if(off>max_x_span+0.5)
		{
			--num_point;
			dec_(d(i));
			continue;
		}

		break;
	}

	return num_point>=min_point;
}

bool card_fit::fit()
{
	if(!check_x_span())
		return false;
	
	int count=size();
	double temp = count*A - B*B;

	kb = (A*D - B*C) / temp;
	k = (count*C - B*D) / temp;

	double sum=0;
	for (int i=0; i<count; i++)
		sum+=differ_k(d(i).x,d(i).y);

	ke = sum / count;

	return true;
}
#ifdef _LINE_FIT_TEST
#include "line_fit.h"
int a(int x)
{
    return x*2+1;
}
double y(int x)
{
    return 0.25*x+13.2;
}
int main()
{
    fit_batch f;
    f.add_tool(100,5);
    //y=2x+1;
    //f.add(30,a(30));
    for(int i=0;i<15;i++)
    {
        f.add(i*10,a(i*10));
    }
    f.add(20,45);
    f.log(); 
    f.reset_data();
    f.add(20,45);
    for(int i=14;i>=0;i--)
        f.add(i,a(i));
    f.log(); 
    printf("%f",f[0].testk(30,a(30)+20));
    f.reset_data();
    f.add(6721.744000,1273.000000);
    f.add(6721.742000,1273.000000);
    f.log();
    f.add(6721.592000,1273.000000);
    f.add(6721.545000,1273.000000);
    f.add(6721.259000,1273.000000);
    f.add(6721.395000,1273.000000);
    f.add(6721.378000,1273.000000);
    f.add(6721.081000,1273.000000);
    f.add(6721.043000,1273.000000);
    f.add(6720.991000,1273.000000);
    f.add(6720.485000,1273.000000);
    f.add(6720.633000,1273.000000);
    f.add(6720.562000,1273.000000);
    f.add(6720.443000,1273.000000);
    f.add(6720.145000,1273.000000);
    f.add(6719.575000,1273.000000);
    f.add(6710.775000,1273.000000);
    f.add(6710.384000,1273.000000);
    f.add(6710.030000,1273.000000);
    f.add(6709.847000,1273.000000);
    f.add(6710.098000,1273.000000);
    f.add(6706.457000,1273.000000);
    f.add(6706.419000,1273.000000);
    f.add(6706.608000,1273.000000);
    f.add(6706.513000,1273.000000);
    f.add(6706.438000,1273.000000);
    f.add(6706.239000,1273.000000);
    f.add(6706.048000,1273.000000);
    f.add(6705.962000,1273.000000);
    f.add(6705.707000,1273.000000);
    f.add(6705.839000,1273.000000);
    f.add(6705.594000,1273.000000);
    f.add(6705.417000,1273.000000);
    f.add(6705.136000,1273.000000);
    f.add(6705.608000,1273.000000);
    f.add(6705.505000,1273.000000);
    f.add(6705.422000,1273.000000);
    f.add(6705.640000,1273.000000);
    f.add(6705.280000,1273.000000);
    f.log();
    zlist<int,128> m_test;
    for(int i= 1 ;i<=20;i++)
    {
        m_test.grow()=i;
        if(i%5==0) 
        {
            int x=0;
            for(int j=0;j<5;j++)
            {
                x+=m_test(j);
            }
            m_test.rskip(5);
            m_test.grow()=x;
        }
    }
    for(int i=0;i<m_test.size();i++)
      printf("%d\n",m_test(i));

    line_r l(point(1,y(1)),point(13,y(13)));
    if(l.contain(point(4,y(4))))printf("contain:%d,%f\n",4,y(4));
    if(l.contain(point(5,y(5))))printf("contain:%d,%f\n",5,y(5));
    if(l.contain(point(-5,y(-5))))printf("contain:%d,%f\n",-5,y(-5));
    point p=l.projection(point(17,y(17)+1.5));
    printf("projection:%f,%f\n",p.x,p.y);
    p=l.projection(point(-17,y(-17)+1.5));
    printf("projection:%f,%f\n",p.x,p.y);
    printf("line:%s\n",l.to_string().c_str());
    line_r ll=l;
    printf("line:%s\n",ll.to_string().c_str());
    bool b=l.is_same_direction(ll);
    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),ll.to_string().c_str());
    line_r l2(point(4,y(4)),point(7,y(7)));
    b=l.is_same_direction(l2);
    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),l2.to_string().c_str());
    line_r l3(point(19,y(19)),point(31,y(31)));
    b=l.is_same_direction(l3);
    if(b)printf("%s [is same direction]%s\n",l.to_string().c_str(),l3.to_string().c_str());
    line_r l4(point(19,y(19)),point(4,y(4)));
    b=l.is_same_direction(l4);
    if(!b)printf("%s [is not same direction]%s\n",l.to_string().c_str(),l4.to_string().c_str());

    return 0;
}
#endif