#include "ZouZhiMotor.h"
#include <thread>

ZouZhiMotor::ZouZhiMotor()
	: Motor(motorPorts_Zouzhi)
{
	auto t_smbtype =MotorConfig::MTBDType::MT_TMC;
	m_zouparamex = m_mtconfig->GetMotorSpeedParam(true, t_smbtype, 4, 1, 1);
	speedConfig();
}

ZouZhiMotor::~ZouZhiMotor()
{
}

void ZouZhiMotor::start()
{
	Motor::start(delays, m_zouparamex.mt_param);
}

void ZouZhiMotor::speedChange(MotorSpeedParamEx paramex) 
{
	m_zouparamex = paramex;
	speedConfig();
}

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

void ZouZhiMotor::speedConfig()
{
	// printf("ZouZhiMotor 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_zouparamex.speed,
	// 	m_zouparamex.dpi,
	// 	m_zouparamex.colormode,
	// 	m_zouparamex.mt_param.finalPeriod,
	// 	m_zouparamex.mt_param.Fmin,
	// 	m_zouparamex.mt_param.a,
	// 	m_zouparamex.mt_param.offset,
	// 	m_zouparamex.mt_param.stepnum,
	// 	m_zouparamex.mt_param.finalDelay);

	delays.clear();

	delays = speedup_cfg(m_zouparamex.mt_param.finalPeriod, m_zouparamex.mt_param.Fmin, m_zouparamex.mt_param.stepnum, m_zouparamex.mt_param.a,
						 m_zouparamex.mt_param.offset, m_zouparamex.mt_param.finalDelay);
}