server/monkey_car/linear_fit.h

#ifndef _LINEAR_FIT_STUB_
#define _LINEAR_FIT_STUB_

#include <deque>
#include <algorithm>
#include <string.h>
#include <map>
#include <float.h>
#include "base_data.h"


struct linear_fit
{
private:
	const size_t count_;
	double a_,b_,t_[4];
	std::deque<double> x_,y_;
	//²»ÊÊÓÃ
	//linear_fit(const linear_fit&)=delete;
	//linear_fit(const linear_fit&);
	void start_fit()
	{
		for(auto ix=x_.begin(), iy=y_.begin();  ix!=x_.end();  ++ix,++iy)
		{  
			t_[0] += *ix * *ix;
			t_[1] += *ix;
			t_[2] += *ix * *iy;
			t_[3] += *iy;
		}  

		double t1=t_[0]*count_ - t_[1]*t_[1];

		a_ = (t_[2]*count_ - t_[1]*t_[3]) / t1 ;
		b_ = (t_[0]*t_[3] - t_[1]*t_[2]) / t1 ;
		//debug_print_syslog(0,"[framework push]count:%d,k:%f,b:%f",count_,a_,b_);
	}
public:

	linear_fit(int count)
		:count_(count)
	{
		reset();
	}

	void reset()
	{
		a_=b_=0;
		memset(&t_[0],0,sizeof(t_));
		x_.resize(0);
		y_.resize(0);
	}

	template<typename iteratorx,typename iteratory>
	bool start(iteratorx xi,iteratory yi,size_t count)
	{
		reset();
		std::copy_n(xi,count,std::back_inserter(x_));
		std::copy_n(yi,count,std::back_inserter(y_));

		if(count<count_)
			return false;

		while(count-- > count_)
		{
			x_.pop_front();
			y_.pop_front();
		}

		start_fit();

		return true;
	}

	bool push(double x,double y)
	{
		//debug_print_syslog(0,"[framework push]count:%d,  x:%.2f,y:%.2f",count_,x,y);
		x_.push_back(x);
		y_.push_back(y);

		if(x_.size() < count_)
		{
			return false; //throw logic_exception(...)
		}
		else if(x_.size()==count_)
		{
			start_fit();
			return true;
		}
		
		t_[0] += x * x - x_.front()*x_.front();
		t_[1] += x - x_.front();
		t_[2] += x * y - x_.front() * y_.front();
		t_[3] += y - y_.front();

		x_.pop_front();
		y_.pop_front();

		double t1=t_[0]*count_ - t_[1]*t_[1];

		a_ = (t_[2]*count_ - t_[1]*t_[3]) / t1 ;
		b_ = (t_[0]*t_[3] - t_[1]*t_[2]) / t1 ;
		//debug_print_syslog(0,"[framework push]count:%d,k:%f,b:%f",count_,a_,b_);


		return true;
	}

	double getY(double x)const
	{
		return a_ * x + b_;
	}
	double getY(uint64_t x) const
	{
		return a_ * x + b_;
	}
	double getK()const
	{
		return a_;
	}
};


struct comp_linear_fit
{
	std::map<int,linear_fit*> m_fit;
	comp_linear_fit(const int* begin,const int* end)
	{
		for(;begin!=end;++begin)
		{
			m_fit.insert(std::make_pair(*begin,new linear_fit(*begin)));
		}
	}

	~comp_linear_fit()
	{
		for_each(m_fit.begin(),m_fit.end(),[&](std::pair<int,linear_fit*> it){delete it.second;});
	}

	double getY(int num_point,uint64_t time)const
	{
		auto it=m_fit.find(num_point);
		if(it==m_fit.end())
		{
			//throw 
			return 0.0;
		}

		return it->second->getY(time);
	}

	double getK(int num_point)const
	{
		auto it=m_fit.find(num_point);
		if(it==m_fit.end())
		{
			//throw 
			return DBL_MAX;
		}

		return it->second->getK();
	}

	void push(uint64_t time, double x)
	{
		for_each(m_fit.begin(),m_fit.end(),[&](std::pair<int,linear_fit*> it){it.second->push(time,x);});
	}
};

#endif