#include "stdafx.h"
#include "monkeycar_person.h"
#include "monkeycar_area.h"
#include "algorithm/Fit.h"
const int num_fit_point[4]={5,10,20,30};
monkey_person::monkey_person(std::shared_ptr<Card> ptr,monkey_area* pma)
	:m_getOffIndex(0)
	,m_ori_ct(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_go_up_point.clear();
	//m_bus = nullptr;
	
	m_person = std::make_shared<person>();

	m_person->m_card = ptr;

	ptr->set_card_Ptr(m_person);

	//std::shared_ptr<area> ma(pma);
	m_person->m_area = pma;

	m_linear_fit=new comp_linear_fit(&num_fit_point[0],&num_fit_point[0]+sizeof(num_fit_point)/sizeof(num_fit_point[0]));

	//init monkey_fit
	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;
	//free mem
	for(auto & mp:mp_monkey_fit)
	{
		delete mp;
	}

	m_person->m_card->resetPtr();

}
void monkey_person::GetOnTheBus(std::shared_ptr<monkey_bus> bus,double speed)
{
	//assignment the monkeycar_bus value
	m_bus = bus;
	//
	m_go_up_point.setTime(m_person->m_card->m_ct_time);
	m_go_up_point.setCT(m_person->m_card->time_stamp);
	m_go_up_point.setX(m_person->m_card->origin_locate.x);
	m_go_up_point.setX(m_person->m_card->origin_locate.y);

	m_cur_distance = m_fit_point.back().x_;
	m_timeval = cacti::Timestamp::getNowTime().getAsMilliseconds();
	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();
	m_go_up_point.clear();
	//m_cur_point.clear();
	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();
	if (!m_bus.expired()&& k != 0)
	{
		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;
}
std::string & monkey_person::getCardId()
{
	return m_person->m_card->card_id;
}
bool monkey_person::is_on_bus() const
{
	return !m_bus.expired();
}
point_2 monkey_person::get_position()
{
	u64 timeval = cacti::Timestamp::getNowTime().getAsMilliseconds();
	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 
	monkey_area* mp_ptr =nullptr;
	mp_ptr = dynamic_cast<monkey_area*>(m_person->m_area);
	//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)/cacti::Timestamp::ONE_SEC_IN_MILLISEC;
	//get Distance
	double ds = tbus->getNextStep_Distance(val)/m_person->m_card->map_scale;
	m_cur_distance += ds;
	//compensation speed...
	m_cur_distance += m_compensation_speed * val;
	double total_length = mp_ptr->get_total_length();
	if (m_cur_distance < ZERO_)
	{
		m_cur_distance = 0;
	}
	else if (m_cur_distance >=total_length)
	{
		m_cur_distance = total_length;
	}
	//get point
	point_2 tmp = mp_ptr->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::save_point()
{
	return is_on_bus();
}
bool monkey_person::screen_Point(u16 ct,double d)
{
	bool ret = true;
	u32 tct = m_ori_ct < ct ? m_ori_ct + USHRT_MAX : m_ori_ct;
	if (!m_fit_point.empty() && tct - ct == 1)
	{
		double last_d = m_fit_point.back().x_;
		double ad = abs(last_d - d);
		if (ad == 0 || ad > 4.0)
		{
			//debug_print_syslog(0,"[framework on_step_map____+++____]cardId:%s, LastDis:%.2f CurDis:%.2f ct:%d lastCT:%d ",getCardId().c_str(),last_d,d,tct,ct);
			ret =  false;
		}
	}
	return ret;
}
bool monkey_person::on_step_map(POS_21::fp_path & fppath)
{
	u16 ct = m_ori_ct;
	st_coord pt=m_person->pin_point(m_ori_ct);
	if (!point_2::is_valid(pt))
	{
		return false;
	}
	//debug_print_syslog(0,"[framework on_step_map]cardId:%s,orix:%.2f,oriy:%.2f   ct:%d",getCardId().c_str(),pt.x_,pt.y_,pt.m_ct);
	m_timestamp = time(NULL)*1000;
	double d = fppath.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;
	do 
	{
		//if(!screen_Point(ct,d))
		//	return false;
		//	break;
		{
			if (!m_bus.expired())
			{
				for (auto & mp:mp_monkey_fit)
				{
					mp->push(1.*pt.gen_time_/1000.0,pt.x_*m_person->m_card->map_scale);
				}
			}
		}
	} while (false);

	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);

	//debug_print_syslog(0,"[framework on_step_map________]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 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_person->m_card->map_scale;
	//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::updata_position() 
{


}



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_person->m_card->map_scale;
	double compensation_speed = (ori_d-m_cur_distance)/(ADJUST_DISTANCE_TIMEVAL/2)*1000;
	setCompensationSpeed(compensation_speed);
	//debug_print_syslog(0,"[framework_2 compensationSpeed]%s,k:%f,b:%f,ori_dis:%f,cur_dis:%f,s:%f",getCardId().c_str(),fit_b->get_K(),fit_b->get_B(),ori_d,m_cur_distance,compensation_speed);
}