#include "mgos.h"
#include "mgos_system.h"
#include "mgos_i2c.h"
#include "WEMOS_Motor.h"

#define SUPPRESS_SENDING_UNCHANGED_VALUSE 0

static struct mgos_i2c *i2c;
static uint32_t motor_freq[MAX_NUM_MOTORS];
static uint16_t motor_i2c_address[MAX_NUM_MOTORS];
static bool initialized = false;

void wemos_motor_init() {
	if (!initialized) {
	    i2c = mgos_i2c_get_global();
		initialized = true;
	}
}

/* setfreq() -- set PWM's frequency
motorNumber: 0..n
freq: PWM's frequency	
*/
void wemos_motor_set_freq(uint8_t motorNumber, uint32_t freq) {
	uint8_t data[4];

	LOG(LL_INFO, ("Writing frequency %d to controller of motor %d (addr=%d)", freq, motorNumber, motor_i2c_address[motorNumber]));
	if (motorNumber < MAX_NUM_MOTORS || !initialized) {
		data[0] = (uint8_t) (freq >> 16) & 0x0f;
		data[1] = (uint8_t) (freq >> 16);
		data[2] = (uint8_t) (freq >> 8);
		data[3] = (uint8_t) freq;
		mgos_i2c_write(i2c, motor_i2c_address[motorNumber], data, 4, true);
		LOG(LL_INFO, ("mgos_i2c_write(i2c, addr=%d, data={%d,%d,%d,%d}, 4, stop=true)", motor_i2c_address[motorNumber], data[0], data[1], data[2], data[3]));
		// mgos_msleep(100);
	} else {
		LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
	}
}

/* Motor()
motorNumber: 0..n
address: Shield I2C address
freq: PWM's frequency		
*/
void wemos_motor_initMotor(uint8_t motorNumber, uint8_t address, uint32_t freq) {
	wemos_motor_init();
	LOG(LL_INFO, ("wemos_motor_initMotor(motorNumber=%d, address=%d, freq=%d", motorNumber, address, freq));
	if (motorNumber < MAX_NUM_MOTORS) {
		motor_freq[motorNumber] = freq;
		motor_i2c_address[motorNumber] = address;
		wemos_motor_set_freq(motorNumber, freq);
	} else {
		LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
	}
}


/* setmotor() -- set motor
motorNumber: 0..n
dir:
	_SHORT_BRAKE	0
	_CW				1
	_CCW			2
	_STOP			3
	_STANDBY		4

pwm_val:
	0.00 - 100.00  (%)
*/
void wemos_motor_setmotor(uint8_t motorNumber, uint8_t dir, float pwm_val)
{
	uint16_t _pwm_val;
	uint8_t data[4];

	#if SUPPRESS_SENDING_UNCHANGED_VALUSE
		static int lastDir = -1;
		static int lastPwm = -1.0;

		if (lastDir == dir && lastPwm == pwm_val) return;
		lastDir = dir;
		lastPwm = pwm_val;
	#endif

	wemos_motor_init();

	if (motorNumber < MAX_NUM_MOTORS) {
		data[0] = (uint8_t) (motorNumber & 0x01) | (uint8_t) 0x10;
		data[1] = dir;
		_pwm_val = (uint16_t) (pwm_val * 100);

		if(_pwm_val>10000)
			_pwm_val=10000;
		data[2] = (uint8_t) (_pwm_val >> 8);
		data[3] = (uint8_t) _pwm_val;
		mgos_i2c_write(i2c, motor_i2c_address[motorNumber], data, 4, true);
		// LOG(LL_INFO, ("mgos_i2c_write(Mot=%d, adr=%d, data={%d,%d,%d,%d}, 4, stop=true", motorNumber, motor_i2c_address[motorNumber], data[0], data[1], data[2], data[3]));
	} else {
		LOG(LL_ERROR, ("Invalid motor number %d, maximum %d motors known", motorNumber, MAX_NUM_MOTORS));
	}
}

void wemos_motor_stop(uint8_t motorNumber) {
	wemos_motor_setmotor(motorNumber, _STOP, 0.0);
}

void wemos_motor_standby(uint8_t motorNumber) {
	wemos_motor_setmotor(motorNumber, _STANDBY, 0.0);
}