#include "Motor.h"
#include "DevUtil.h"
#include <vector>
#include <cmath>
#include <exception>
#include "utilsfunc.h"
#include "MotorConfig.h"

#define ZZ_PWM 3
#define CZ_PWM 2

struct MotorMode
{
	int mod0 : 1;
	int mod1 : 1;
	int mod2 : 1;
};

// const MotorPorts motorPorts_Zouzhi = {.reset = ZouZhiMotor_Reset, .sleep = ZouZhiMotor_Sleep, .enable = ZouZhiMotor_Enable, .dir = ZouZhiMotor_Direction, .decay = ZouZhiMotor_Decay, .home = ZouZhiMotor_Home, .fault = ZouZhiMotor_Fault, .mode0 = ZouZhiMotor_Mode0, .mode1 = ZouZhiMotor_Mode1, .mode2 = ZouZhiMotor_Mode2, .pwm = ZZ_PWM};

// const MotorPorts motorPorts_Cuozhi = {.reset = CuoZhiMotor_Reset, .sleep = CuoZhiMotor_Sleep, .enable = CuoZhiMotor_Enable, .dir = CuoZhiMotor_Direction, .decay = CuoZhiMotor_Decay, .home = CuoZhiMotor_Home, .fault = CuoZhiMotor_Fault, .mode0 = CuoZhiMotor_Mode0, .mode1 = CuoZhiMotor_Mode1, .mode2 = CuoZhiMotor_Mode2, .pwm = CZ_PWM};
const MotorPorts motorPorts_Zouzhi = { 	.power = PIN_PORT_7010::MOTOR_POWER_1,
										.reset = PIN_PORT_7010::ZOUZHI_PIN_RESET, 
										.sleep = PIN_PORT_7010::ZOUZHI_PIN_SLEEP,
										.enable = PIN_PORT_7010::ZOUZHI_PIN_ENABEL, 
										.dir = PIN_PORT_7010::ZOUZHI_PIN_DIR, 
									   	.pwm = 1 
									 };

const MotorPorts motorPorts_Cuozhi = {	.power =PIN_PORT_7010::MOTOR_POWER_2,
										.reset = PIN_PORT_7010::CUOZHI_PIN_RESET, 
										.sleep = PIN_PORT_7010::CUOZHI_PIN_SLEEP,
										.enable = PIN_PORT_7010::CUOZHI_PIN_ENABEL, 
										.dir = PIN_PORT_7010::CUOZHI_PIN_DIR,
									  	.pwm = 0 
									 };
									 
std::vector<int> speedup_cfg(int finalPeriod, int Fmin, int stepnum, int a, int offset, int finalDelay)
{
	std::vector<int> freqs;
	int period = 0;
	double delay = ((double)finalDelay) / 1000000.0;
	double b = stepnum * delay;
	for (int i = 0; i < stepnum; i++)
	{
		period = (int)(finalPeriod + (Fmin - finalPeriod) / (1 + exp(-a * b + offset)));
		b = b - delay;
		freqs.push_back(period);
	}
	return freqs;
}

//GpioOut Motor::powerPin(MotorPower);

Motor::Motor(MotorPorts motorPorts):sleepPin(motorPorts.sleep),
									resetPin(motorPorts.reset),
									enablePin(motorPorts.enable),
									dirPin(motorPorts.dir),
									pwm(motorPorts.pwm),
									motor_power(motorPorts.power)
{
	sleepPin.setDirection(Gpio::out);
	resetPin.setDirection(Gpio::out);
	enablePin.setDirection(Gpio::out);
	dirPin.setDirection(Gpio::out);
	motor_power.setDirection(Gpio::out);

	motor_power.setValue(Gpio::High); //0：关闭电机电源 1：开启电机电源  //默认一直开启
	//默认设置 1 1 1
	resetPin.setValue(Gpio::High);	//0：重启 1：电机不重启
	sleepPin.setValue(Gpio::High);  //0：电机停止转动 1：电机开始转动
	enablePin.setValue(Gpio::High); //0：电机芯片打开电源 1：电机芯片关闭电源

}

Motor::~Motor()
{
	stop();
	if (m_mtconfig.get())
		m_mtconfig.reset();
}

void Motor::start(std ::vector<int> &delay_s, const MotorSpeedParam &msp)
{
	enablePin.setValue(Gpio::Low);
	pwm.enable(true);
	if (!delay_s.empty())
	{
		std::vector<int>::iterator iter = delay_s.begin();
		while (++iter != delay_s.end())
		{
			pwm.setFreq(PWM_PERIOD / (*iter));
			std::this_thread::sleep_for(std::chrono::microseconds((int)msp.finalDelay));
		}
	}
	pwm.setFreq(PWM_PERIOD / msp.finalPeriod);
	//pwm.setFreq((PWM_PERIOD / msp.finalPeriod)/10*10);
	//printf("\n-----pwm Freq =  %d------\n",PWM_PERIOD / msp.finalPeriod/10*10);
}

void Motor::start()
{
	enablePin.setValue(Gpio::Low);
	pwm.enable(true);
}

void Motor::stop()
{
	enablePin.setValue(Gpio::High);
	pwm.enable(false);
}

void Motor::pause()
{
	pwm.enable(false);
	enablePin.setValue(Gpio::Low);
}

void Motor::setMode(int mode_input)
{
	// MotorMode *mode = (MotorMode *)&mode_input;
	// mode0.setValue((Gpio::GpioLevel)mode->mod0);
	// mode1.setValue((Gpio::GpioLevel)mode->mod1);
	// mode2.setValue((Gpio::GpioLevel)mode->mod2);
}

void Motor::setDirection(int dir)
{
	dirPin.setValue((Gpio::GpioLevel)dir);
}

void Motor::setSpeed(int value)
{
	pwm.setFreq(value);
}

void Motor::enablePower(bool bEnable)
{
	LOG("Enable Motor PowerPin %s \n", bEnable ? "true" : "false");
	//powerPin.setValue((Gpio::GpioLevel)bEnable);
}