#include "i2csensor.h"

namespace {
	void callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
		if(i2cp->id_slave_config->restart) {
			i2cp->id_slave_config->restart = FALSE;
			i2cMasterReceive(i2cp, i2cscfg);
		} else {
			i2cMasterStop(i2cp);
		}
	}
	
	void err_callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
		
	}
	
	I2CConfig i2c_conf = {
		opmodeI2C,
		100000,
		stdDutyCycle,
		0,
		0
	};
};

uint8_t I2CSensor::rxdata[];
uint8_t I2CSensor::txdata[];

I2CSlaveConfig I2CSensor::i2c_slave_conf = {
	callback, err_callback,
	
	rxdata, sizeof(rxdata), 0, 0,
	txdata, sizeof(txdata), 0, 0,
	
	0,
	FALSE,
};

void I2CSensor::enable_bus() {
	palSetPadMode(GPIOB, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
	palSetPadMode(GPIOB, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
	
	i2cStart(&I2CD2, &i2c_conf);
	
	while(I2CD2.id_state != I2C_READY) {
		chThdSleepMilliseconds(1);
	}
}

#include "usbserial.h"
extern USBSerial usbs;
int usbprintf(USBSerial& usbs, const char* format, ...);

void I2CSensor::write(uint8_t reg, uint8_t value) {
	I2CD2.id_state = I2C_READY;
	i2c_slave_conf.address = i2c_address;
	i2c_slave_conf.txbytes = 2;
	i2c_slave_conf.txbuf[0] = reg;
	i2c_slave_conf.txbuf[1] = value;
	
	i2cMasterTransmit(&I2CD2, &i2c_slave_conf);
	chThdSleepMilliseconds(2);
}

void I2CSensor::read(uint8_t reg, size_t len) {
	I2CD2.id_state = I2C_READY;
	i2c_slave_conf.address = i2c_address;
	i2c_slave_conf.txbufhead = 0;
	i2c_slave_conf.txbuf[0] = reg;
	i2c_slave_conf.txbytes = 1;
	i2c_slave_conf.rxbufhead = 0;
	i2c_slave_conf.rxbytes = len;
	i2c_slave_conf.restart = TRUE;
	
	i2cMasterTransmit(&I2CD2, &i2c_slave_conf);
	chThdSleepMilliseconds(2);
}