#include <functional>
#include "monkeycar_bus.h"
#include "monkeycar_area.h"
#include "monkeycar_person.h"
#include "log.h"
#include "tool_time.h"
#ifndef WIN32
#include <unistd.h>
#endif
monkey_bus::monkey_bus(double speed, int direct)
	:m_speed(speed)
	,m_direct(direct)
	,m_stop(false)
{
	m_thread.reset(new std::thread(std::bind(&monkey_bus::adjust_monkeyperson_distance,this)));
}
void monkey_bus::adjust_monkeyperson_distance()
{
	bool cflag = false;
	std::vector<std::shared_ptr<monkey_person>> v;
	while (!m_stop)
	{
		v.clear();
		uint64_t nowTime = tool_time::tool_time::now_to_ms();
		{
			std::lock_guard<std::mutex> lk(m_mtx);
			std::for_each(person_list_.begin(),person_list_.end(),[&](const std::shared_ptr<monkey_person> mp){
				if (nowTime - mp->m_adjustDis_timeval >= ADJUST_DISTANCE_TIMEVAL/2 && !cflag)
				{
					mp->setCompensationSpeed(0);
					cflag = true;
				}
				if (nowTime - mp->m_adjustDis_timeval >= ADJUST_DISTANCE_TIMEVAL)
				{
					mp->m_adjustDis_timeval = nowTime;
					mp->handle_monkeycar_fit(1.*(nowTime-monkey_area::m_startTime)/1000.0);
					cflag = false;
				}
			});
			std::for_each(person_list_.begin(),person_list_.end(),[&](const std::shared_ptr<monkey_person> mp){
				if (nowTime-mp->m_timestamp > LONG_TIME_NO_DATA)
					v.push_back(mp);
			});
		}

		std::for_each(v.begin(),v.end(),[&](const std::shared_ptr<monkey_person>& mp){
			getOffTheBus(mp);
		});
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
}

bool monkey_bus::test_get_on(double speed)
{
	double s=speed>0?speed:-speed;
	double ms=m_speed>0?m_speed:-m_speed;
	log_info("monkeycar test_get_on:%.2f,%.2f",s,ms);
	log_info("monkeycar ...........:%.2f,%.2f",ms*ON_BUS_SPEED_LIMIT,ms*ON_BUS_SPEED_MAX_LIMIT);
	if (s >= ms*ON_BUS_SPEED_LIMIT && s < ms*ON_BUS_SPEED_MAX_LIMIT)
	{
		log_info("monkeycar test_get_on_true:%.2f,%.2f",speed,m_speed);
		return true;
	}
	return false;
}

bool monkey_bus::test_get_off(std::shared_ptr<monkey_person> mp,double speed)
{
	bool ret = false;
	double s=speed>0?speed:-speed;
	double ms=m_speed>0?m_speed:-m_speed;
	ret = (speed > ZERO_ ? NEGTIVE_DIRECTION:POSTIVE_DIRECTION) == m_direct ;
	if (ret)
	{
		if (s < ms*OFF_BUS_SPEED_LIMIT)
		{
			if(mp->Judge_OffIndex())
			{
				ret = false;
			}
		}
		else
		{
			mp->resetOffIndex();
		}
	}
	//debug_print_syslog(0,"[lemon the second choice true___]cardId:%s,v:%f,state:%s",mp->getCardId().c_str(),speed,ret?"ON":"OFF");

	return ret;
}
void monkey_bus::Set_AverageSpeed(double speed)
{
	m_speed = m_speed * 0.99 + speed * 0.01;
}
void monkey_bus::getOffTheBus(std::shared_ptr<monkey_person> mp)
{
	//can not free here .
	mp->GetOffTheBus();
	std::lock_guard<std::mutex> lk(m_mtx);
	auto it = std::find(person_list_.begin(),person_list_.end(),mp);
	if (it != person_list_.end())
	{
		person_list_.erase(it);

	}
}
void monkey_bus::getOnTheBus(std::shared_ptr<monkey_person> mp,std::shared_ptr<monkey_bus> bus)
{
	log_info("monkeycar getOn");
	mp->GetOnTheBus(bus,m_speed);
	std::lock_guard<std::mutex> lk(m_mtx);
	person_list_.push_back(mp);
}