server/monkey_car/monkey_fit.h

#ifndef __MONKEY_FIT__
#define __MONKEY_FIT__

#include <cmath>
#include <iterator>
#include <map>
#include <deque>
#include <algorithm>
#include "base_data.h"

struct monkey_fit
{	
	virtual ~monkey_fit(){}
	template<typename T> static double get_probab_max(T it,T ite,int sum,int N=3)
	{
		sum/=5;
		sum=std::max(3,sum);

		std::vector<std::tuple<T,T,float>> p;


		if(N==0)
		{
			auto it0=it;
			for(int i=0;it0!=ite;++it0,++i)
			{
				if(i%10==0) printf("\n");

				printf("%5d:%2d,",it0->first,it0->second);
			}
			printf("\n");
		}

		int s=0;
		for(auto i1=it;it!=ite;)
		{
			for(;s<sum && i1!=ite;++i1)
				s+=i1->second;

			if(s<sum) break;

			p.push_back(std::make_tuple(it,i1,1.*s/(std::prev(i1)->first-it->first+1)));

			s-=it->second; ++it;
		}

		std::sort(p.begin(),p.end(),[](const std::tuple<T,T,int>&l,const std::tuple<T,T,int>&r){
			return 1.*std::get<2>(l) > 1.*std::get<2>(r);
		});

		int i=1;

		auto it1=std::get<0>(p[0]);
		auto it2=std::prev(std::get<1>(p[0]));

		for(;i<(int)p.size();++i)
		{
			if(std::get<2>(p[i])!=std::get<2>(p[i-1]))
				break;

			if(std::get<0>(p[i])->first < it1->first)
				it1=std::get<0>(p[i]);

			if(std::prev(std::get<1>(p[i]))->second > it2->second)
				it2=std::prev(std::get<1>(p[i]));
		}

		++it2;

		double rc=0;
		int    cc=0;
		int    x=0;
		for(;it1!=it2;++it1)
		{
			rc+=it1->first*it1->second;
			cc+=it1->second;

			if(N==0)
			{
				if(x++%10==0) printf("\n");
				printf("%5d:%2d,",it1->first,it1->second);
			}
		}

		if(N==0)
			printf("\n");

		return rc/cc;
	}

	int round(double value)
	{
		return (value > 0.0)?floor(value + 0.5):ceil(value - 0.5);
	}
	virtual void reset(double monkey_speed = 0){}
	virtual void push(double time_second,double dist_meter) {}
	virtual double get_K() = 0;
};

struct monkey_fit_b :monkey_fit
{
	static const int min_points = 60;
	static const int BN = 20;
	double k_init_;
	int    count_;

	std::map<int,int> probab_b;

	monkey_fit_b()
		:k_init_(0)
		,count_(0)
	{}
	monkey_fit_b(double s)
		:k_init_(s)
		,count_(0)
	{}

	virtual void reset(double monkey_speed/*=0*/)
	{
		k_init_=monkey_speed;

		count_=0;
		probab_b.clear();
	}

	virtual void push(double time_second,double dist_meter)
	{
		double k=k_init_;

		++count_;
		double b=(dist_meter-k*time_second)*BN;
		++probab_b[round(b)];
	}

	double get_dist(double time_second)const
	{
		return k_init_*time_second + get_B();
	}

	double get_K()
	{
		return k_init_;
	}

	double get_B()const
	{
		if(count_<min_points)
			return 0;

		if(probab_b.size()<3)
			return 0;

		return 1.*get_probab_max(probab_b.begin(),probab_b.end(),count_)/BN;
	}
};

struct monkey_fit_k:monkey_fit
{
	static const int min_points=60;
	static const int KN=50;
	int			step_;
	uint32_t    count_;
	std::map<int,int> probab_k;
	std::deque<point_2> m_points;

	monkey_fit_k()
		:step_(0)
		,count_(0)
	{}

	virtual void reset(double speed /*=0*/)
	{
		probab_k.clear();
		m_points.clear();
		step_=0;
		count_ = 0;
	}

	virtual void push(double time_second,double dist_meter)
	{
		int cc=m_points.size();

		if(cc<60)
		{
			m_points.push_back(point_2(time_second,dist_meter));
		}
		else if(step_++>2)
		{
			m_points.push_back(point_2(time_second,dist_meter));
			step_=0;
		}

		if(cc<10)
			return;

		cc*=9./10;
		for(int i=0;i<3;i++)
		{
			int index=rand()%cc;
			double k=(dist_meter-m_points[index].y_)/(time_second-m_points[index].x_);
			if((1<k && k<3) || (-3<k && k<-1))
			{
				count_++;
				++probab_k[round(k*KN)];
			}
		}
	}

	double get_K()
	{
		if(m_points.size()<min_points)
			return 0;
		return round(get_probab_max(probab_k.begin(),probab_k.end(),count_)/KN*10000)/10000.;
	}

};

#endif