#include "CuoZhiMotor.h"
#include <random>

CuoZhiMotor::CuoZhiMotor()
	: Motor(motorPorts_Cuozhi)
{
	auto t_smbtype = MotorConfig::MTBDType::MT_TMC;//SMBType::MB_DRV_TMC216;//smbtype == SMBType::MB_DRV_TMC216 ? MotorConfig::MTBDType::MT_TMC : MotorConfig::MTBDType::MT_DRV;
	m_cuoparamex = m_mtconfig->GetMotorSpeedParam(false, t_smbtype, 4, 1, 1);
	speedConfig();
	mspCuozhiFeeding = {.finalPeriod = 1427500, .Fmin = 2027500, .stepnum = 30, .a = 100, .offset = 4, .finalDelay = 3000};
	if (smbtype == SMBType::MB_DRV_TMC216)
	{
		mspCuozhiFeeding.finalPeriod /= 4;
		mspCuozhiFeeding.Fmin /= 4;
	}
	delays_feeding = speedup_cfg(mspCuozhiFeeding.finalPeriod, mspCuozhiFeeding.Fmin, mspCuozhiFeeding.stepnum, mspCuozhiFeeding.a,
								 mspCuozhiFeeding.offset, mspCuozhiFeeding.finalDelay);
}

CuoZhiMotor::~CuoZhiMotor()
{
}

void CuoZhiMotor::pauseWaitForThread()
{
	if (thStart.joinable())
	{
		thStart.join();
	}
	pause();
}

void CuoZhiMotor::startAsyn()
{
	if (thStart.joinable())
	{
		thStart.join();
	}
	thStart = std::thread(&CuoZhiMotor::forward, this);
}

void CuoZhiMotor::start()
{
	forward();
}

void CuoZhiMotor::reset()
{
	if (thStart.joinable())
	{
		thStart.join();
	}

	setDirection(1);
	delays_forward.clear();
	delays_forward = speedup_cfg(mspCuozhiBackward.finalPeriod,
								 mspCuozhiBackward.Fmin,
								 mspCuozhiBackward.stepnum,
								 mspCuozhiBackward.a,
								 mspCuozhiBackward.offset,
								 mspCuozhiBackward.finalDelay);
	//printf("finalPeriod=%d Fmin=%d \n", mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin);
	Motor::start(delays_forward, mspCuozhiBackward);
	std::random_device rd;							 // obtain a random number from hardware
	std::mt19937 gen(rd());							 // seed the generator
	std::uniform_int_distribution<> distr(100, 400); // define the range
	std::this_thread::sleep_for(std::chrono::milliseconds(distr(gen)));
	stop();
	delays_backward.clear();
	delays_backward = speedup_cfg(mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin, mspCuozhiBackward.stepnum, mspCuozhiBackward.a,
								  mspCuozhiBackward.offset, mspCuozhiBackward.finalDelay);
	setDirection(0);
	Motor::start(delays_backward, mspCuozhiBackward);

	std::this_thread::sleep_for(std::chrono::milliseconds(150));

	stop();
	speedRecover();
}

void CuoZhiMotor::forward()
{
	setDirection(1);	
	Motor::start(delays_forward, m_cuoparamex.mt_param);
}

void CuoZhiMotor::backward()
{
	setDirection(0);
	Motor::start(delays_backward, mspCuozhiBackward);
}

void CuoZhiMotor::feeding()
{
	setDirection(1);
	Motor::start(delays_feeding, mspCuozhiFeeding);
}

void CuoZhiMotor::speedChange(int speed, int dpi, int colormode) // speed should be in range [0,5] (by ply,at 2019.5.23)
{
	auto t_smbtype = MotorConfig::MTBDType::MT_TMC;//SMBType::MB_DRV_TMC216;//smbtype == SMBType::MB_DRV_8825 ? MotorConfig::MTBDType::MT_DRV : MotorConfig::MTBDType::MT_TMC;
	m_cuoparamex = m_mtconfig->GetMotorSpeedParam(false, t_smbtype, speed, colormode, dpi);
	speedConfig();
}

void CuoZhiMotor::speedRecover()
{
	speedConfig();
}

void CuoZhiMotor::speedConfig()
{
	LOG("CuoZhiMotor speed=%d \n dpi=%d \n colormode=%d \n finalPeriod=%d \n Fmin=%d \n a=%.2f \n offset=%.2f \n stepnum=%.2f \n finalDelay=%.2f \n ",
		m_cuoparamex.speed,
		m_cuoparamex.dpi,
		m_cuoparamex.colormode,
		m_cuoparamex.mt_param.finalPeriod,
		m_cuoparamex.mt_param.Fmin,
		m_cuoparamex.mt_param.a,
		m_cuoparamex.mt_param.offset,
		m_cuoparamex.mt_param.stepnum,
		m_cuoparamex.mt_param.finalDelay);

	delays_forward.clear();
	delays_forward = speedup_cfg(m_cuoparamex.mt_param.finalPeriod, m_cuoparamex.mt_param.Fmin, m_cuoparamex.mt_param.stepnum, m_cuoparamex.mt_param.a,
								 m_cuoparamex.mt_param.offset, m_cuoparamex.mt_param.finalDelay);
								 	std::vector<int> vct_speedupdon(delays_forward);

	int size= vct_speedupdon.size();
	int half_len = vct_speedupdon.size()/3;
	for (int i = size-1; i > 0; i--)
	{
		delays_forward.push_back(vct_speedupdon[i]);
	}
	delays_backward.clear();
	delays_backward = speedup_cfg(mspCuozhiBackward.finalPeriod, mspCuozhiBackward.Fmin, mspCuozhiBackward.stepnum, mspCuozhiBackward.a,
								  mspCuozhiBackward.offset, mspCuozhiBackward.finalDelay);
}