#include "monkeycar_person.h"
#include "monkeycar_area.h"
#include "monkeycar_bus.h"
#include <climits>
#include "log.h"
#include "tool_time.h"
const int num_fit_point[4]={5,10,20,30};

monkey_person::monkey_person(std::shared_ptr<monkey_area> area)
	:m_area(area)
	,m_getOffIndex(0)
	,m_cur_distance(0x12345678)
	,m_compensation_speed(0)
	,m_timeval(0)
	,m_adjustDis_timeval(0)
	,m_timestamp(0)
{
	m_timestamp = time(NULL) * 1000;
	
	m_linear_fit=new comp_linear_fit(&num_fit_point[0],&num_fit_point[0]+sizeof(num_fit_point)/sizeof(num_fit_point[0]));

	mp_monkey_fit.front() = new monkey_fit_k; //k
	mp_monkey_fit.back() = new monkey_fit_b;  //b
}

monkey_person::~monkey_person()
{
	delete m_linear_fit;
	for(auto & mp:mp_monkey_fit)
		delete mp;
}
void monkey_person::GetOnTheBus(std::shared_ptr<monkey_bus> bus,double speed)
{
	//assignment the monkeycar_bus value
	m_bus = bus;

	m_cur_distance = m_fit_point.back().x_;
	m_timeval = tool_time::now_to_ms();
	m_adjustDis_timeval = m_timeval;

	m_fit_point.clear();
	m_compensation_speed = 0;

	//when get on the bus, init monkey_fit
	reset(speed);
}
void monkey_person::GetOffTheBus()
{
	Set_AverageSpeed();

	m_bus.reset();

	resetOffIndex();
	m_fit_point.clear();
	m_compensation_speed = 0;


}
void monkey_person::Set_AverageSpeed()
{
	//set average speed.
	//monkey_fit_k* fit_k  = dynamic_cast<monkey_fit_k*>(mp_monkey_fit.front());
	//double k = fit_k->get_K();
	double k = mp_monkey_fit.front()->get_K();
	if (!m_bus.expired()&& k != 0)
	{
		//debug_print_syslog(0,"[framework_2 Set_AverageSpeed_getoff]%s,v:%f",getCardId().c_str(),k);
		m_bus.lock()->Set_AverageSpeed(k);
	}

}
void monkey_person::reset(double speed)
{
	for (auto & mp:mp_monkey_fit)
	{
		mp->reset(speed);
	}
}
bool monkey_person::Judge_OffIndex()
{
	m_getOffIndex++;
	if (m_getOffIndex == MAX_GETOFF_NUM)
	{
		return true;
	}
	return false;
}
void monkey_person::resetOffIndex()
{
	m_getOffIndex = 0;
}
bool monkey_person::is_on_bus() const
{
	return !m_bus.expired();
}
std::shared_ptr<monkey_bus> monkey_person::getBus()
{
	return m_bus.lock();
}
point_2 monkey_person::get_position(double &speedx)
{
	uint64_t timeval = tool_time::now_to_ms();
	if (m_bus.expired() || m_cur_distance == 0x12345678)
	{
		//debug_print_syslog(0,"[framework m_bus.expired()]cardId:%s",getCardId().c_str());
		m_timeval = timeval;
		return point_2::invalid_point();
	}

	//get shared_ptr 
	//debug_print_syslog(0,"[framework get_positon]cardId:%s  x:%.2f,time:%lld",getCardId().c_str(),m_cur_distance,m_timeval);
	//here should lock ...take care
	std::shared_ptr<monkey_bus> tbus = nullptr;
	if (!m_bus.expired())
	{
		tbus = m_bus.lock();
	}
	else
	{
		return point_2::invalid_point();
	}
	//get time val
	double val = 1.*(timeval - m_timeval)/1000;
	//get Distance
    double scale=m_area.lock()->getScale();
	double ds = tbus->getNextStep_Distance(val)/scale;
	m_cur_distance += ds;
	//compensation speed...
	m_cur_distance += m_compensation_speed * val;
	double total_length = m_area.lock()->get_total_length();
    speedx=(tbus->m_speed+m_compensation_speed*scale)*3.6;
	if (m_cur_distance < ZERO_)
	{
		m_cur_distance = 0;
        speedx=0;
	}
	else if (m_cur_distance >=total_length)
	{
		m_cur_distance = total_length;
        speedx=0;
	}
	//get point
	point_2 tmp = m_area.lock()->map(m_cur_distance);
	//debug_print_syslog(0,"[framework get_position]cardId:%s,d:%f",getCardId().c_str(),m_cur_distance);
	if (point_2::is_valid(tmp))
	{
		//debug_print_syslog(0,"[framework get_position__]cardId:%s,d:%f",getCardId().c_str(),m_cur_distance);
		m_timeval = timeval;
		return tmp;
	}
	m_timeval = timeval;
	return point_2::invalid_point();
}

bool monkey_person::on_step_map(const st_coord & pt_)
{
	st_coord pt(pt_);
	if (!point_2::is_valid(pt))
	{
		return false;
	}
	m_timestamp = time(NULL)*1000;
	double d = m_area.lock()->map(pt.x_,pt.y_);
	if (d < ZERO_)
	{
		//debug_print_syslog(0,"[framework d<0]cardId:%s,orix:%.2f,oriy:%.2f dis:%.2f  ct:%d  time:%ld",getCardId().c_str(),pt.x_,pt.y_,d,pt.m_ct,pt.gen_time_);
		return false;
	}
	pt.x_ = d;
	if (!m_bus.expired())
	{
		for (auto & mp:mp_monkey_fit)
		{
			mp->push(1.*pt.gen_time_/1000.0,pt.x_*m_area.lock()->getScale());
		}
	}

	m_linear_fit->push(pt.gen_time_,pt.x_);
	if (m_fit_point.size() >= MAX_POINT_V)
	{
		m_fit_point.pop_front();
	}
	m_fit_point.push_back(pt);

	return true;
}

bool monkey_person::fit_speed(double*ret_speed)
{
	int num_point=0;
	if(!get_num_point(&num_point))
		return false;
	//here multiply map_scale
	*ret_speed=m_linear_fit->getK(num_point)*m_area.lock()->getScale();
	//debug_print_syslog(0,"[framework fit_speed]cardId:%s,speed:%f",getCardId().c_str(),*ret_speed);
	return true;
}
bool monkey_person::get_num_point(int * p)
{
	int index = 5;
	int i = 0;
	while (index <= 30)
	{
		if (init_another_list(index))
		{
			*p = index;
			return true;
		}

		i += 2;
		index = 5 * i;
	}
	return false;
}
bool monkey_person::init_another_list(size_t index)
{
	if (m_fit_point.size() < index)
	{
		return false;
	}
	if (index < 5)
	{
		return false;
	}
	size_t begin_count = (*std::prev(m_fit_point.end(),index)).m_ct;
	size_t end_count = (*std::prev(m_fit_point.end())).m_ct;
	end_count = end_count>begin_count?end_count:end_count+USHRT_MAX;
	if (end_count - begin_count <= (index-5)*1/5+index)
	{
		return true;
	}
	return false;
}


void monkey_person::handle_monkeycar_fit(double time_second)
{
	monkey_fit_b * fit_b = dynamic_cast<monkey_fit_b*>(mp_monkey_fit.back());
	if (!fit_b->get_B())
	{
		return;
	}
	double ori_d = fit_b->get_dist(time_second)/m_area.lock()->getScale();
	double compensation_speed = (ori_d-m_cur_distance)/(ADJUST_DISTANCE_TIMEVAL/2)*1000;
	setCompensationSpeed(compensation_speed);
	log_info("monkeycar framework_2 compensationSpeed]k:%f,b:%f,ori_dis:%f,cur_dis:%f,s:%f",fit_b->get_K(),fit_b->get_B(),ori_d,m_cur_distance,compensation_speed);
}