#include "motor_module.h"
#include "string.h"
#include "AI8051U_I2C.h"
#include "global.h"
#include "intrins.h"

MotorModuleHandleTypeDef motor_module;

void motor_init(uint8_t motor_type)
{
	memset(&motor_module, 0, sizeof(MotorModuleHandleTypeDef));
	motor_module.dev_addr = MOTOR_MODULE_ADDR;
	i2c_init();
	set_motor_type(motor_type);
	set_motor_polarity(0);
}

void get_motor_module_voltage(uint16_t* bat)
{
	uint8_t val[2] = {0};
	
	i2c_read_data(MOTOR_MODULE_ADDR << 1, ADC_BAT_REG, val, sizeof(val));
	*bat = BYTE_TO_HW(val[1], val[0]);
}

void get_motor_encoder(int32_t* encoder_val)
{
	uint8_t i;
	int32_t val[4] = {0};
	
	i2c_read_data(MOTOR_MODULE_ADDR << 1, MOTOR_GET_ENCODER_REG,(uint8_t*)val, sizeof(val));
	for (i = 0; i < 4; i++)
	{
		encoder_val[i] = val[i];
	}
}

void set_motor_type(uint8_t type)
{
	i2c_write_data(MOTOR_MODULE_ADDR << 1, MOTOR_TYPE_REG, &type, 1);
}

void set_motor_polarity(uint8_t polarity)
{
	i2c_write_data(MOTOR_MODULE_ADDR << 1, MOTOR_ENCODER_POLARITY_REG, &polarity, 1);
}

void set_motor_pwm(int8_t* pwm)
{
	motor_module.set_pwm[0] = pwm[0];
	motor_module.set_pwm[1] = pwm[1];
	motor_module.set_pwm[2] = pwm[2];
	motor_module.set_pwm[3] = pwm[3];
	
	i2c_write_data(MOTOR_MODULE_ADDR << 1, MOTOR_SET_PWM_REG, (uint8_t*)pwm, 4);
}

void set_motor_speed(int8_t* speed)
{
	motor_module.set_speed[0] = speed[0];
	motor_module.set_speed[1] = speed[1];
	motor_module.set_speed[2] = speed[2];
	motor_module.set_speed[3] = speed[3];
	
	i2c_write_data(MOTOR_MODULE_ADDR << 1, MOTOR_SET_SPEED_REG, (uint8_t*)speed, 4);
}
