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

ZouZhiMotor::ZouZhiMotor()
	: Motor(motorPorts_Zouzhi)
{
	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_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(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_TMC216 ? MotorConfig::MTBDType::MT_TMC : MotorConfig::MTBDType::MT_DRV;
	m_zouparamex = m_mtconfig->GetMotorSpeedParam(true, t_smbtype, speed, colormode, dpi);
	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);
}